Princeton Intro to Robotics (Fall 2026) | Lecture 6: Trajectory Optimization

Overview

Trajectory optimization makes motion planning respect robot dynamics by replacing RRT’s straight-line extensions with feasible motions found through boundary-value optimization. The lecture derives a linearized planar-quadrotor model, shows how linear dynamics and constraints produce an efficiently solvable convex quadratic program, then explains sequential quadratic programming (SQP) for nonlinear systems and illustrates it with acrobot, fixed-wing aircraft, and precomputed flight funnels.

Key takeaways

Differential flatness simplifies trajectory generation but does not give direct authority over every state: planar-quadrotor flat outputs are x and y, while 3D quadrotor flat outputs are x, y, z, and yaw.
A dynamics-aware RRT should extend its tree using control inputs and the equations of motion; merely sampling existing tree states can be probabilistically complete in theory yet fail to explore effectively in practice.
Linearizing the planar quadrotor about hover, with u0 = [mg, 0], yields a local model ẋ ≈ A(x − x0) + B(u − u0), where the key nonzero control derivatives are 1/m and 1/I.
A quadratic control-effort objective combined with linearized dynamics and polytope state constraints forms a convex QP, which can be solved efficiently even for large trajectory problems.
SQP handles nonlinear robot dynamics by repeatedly solving locally approximated QPs, but it may fail to find collision-free paths in complex environments and has no general feasibility guarantee.
Precomputing trajectories with guaranteed funnels can shift expensive planning offline, leaving runtime obstacle avoidance to collision checking against a library of candidate motions.

Chapters

0:00 Differential Flatness: Useful for Planning, Limited for Full-State Control

For a planar quadrotor, the flat outputs are center-of-mass coordinates x and y; for a 3D quadrotor, they are x, y, z, and yaw.
Smooth trajectories for flat outputs let the planner recover the remaining state and suitable control inputs.
Flatness does not provide direct control over planar orientation or 3D roll and pitch, so obstacle avoidance may require approximating the vehicle as a sphere.

5:19 A First Dynamics-Aware RRT: Build the Tree in State Space

The proposed RRT variant stores full states rather than configurations; a planar quadrotor has a six-dimensional state and a 3D quadrotor has a 12-dimensional state.
From a sampled tree state, it applies a randomly chosen control for a short interval and integrates the equations of motion.
A predicted state is added only if it passes collision checking.

9:59 Why Sampling Only Existing RRT States Stalls Exploration

Sampling exclusively from the existing tree causes new samples to cluster around the tree’s current states.
Repeated control extensions then produce a muddled cluster rather than exploring new regions of state space.
The method is theoretically probabilistically complete as iterations approach infinity, but the lecture characterizes it as practically useless because exploration is poor.

12:22 Replace RRT Line Segments with a Boundary-Value Trajectory

The improved RRT retains the usual random target and nearest-tree-state steps but changes how each extension is computed.
Instead of connecting the nearest state to the target with a straight line, it searches for control inputs that move the robot between them.
This boundary-value problem is also called trajectory optimization and is the computationally difficult part of dynamics-aware RRT.

17:00 Why Robot Dynamics Make Straight-Line Extensions Infeasible

A car cannot translate sideways directly; it must maneuver through forward and reverse motion or another dynamically feasible path.
A planar quadrotor likewise cannot execute every arbitrary configuration-space line segment.
Trajectory optimization asks whether a sequence of controls can move a system from a specified start state to a specified target state.

21:00 Linearize the Planar Quadrotor Around a Hovering State

The planar quadrotor state is [x, y, θ, vx, vy, ω], with control inputs u1 for total thrust and u2 for total moment.
Its equations include nonlinear terms such as sin θ and cos θ, so a first-order Taylor approximation can simplify the trajectory-optimization problem.
A natural reference is hovering at the origin with zero velocity and orientation, using nominal controls u0 = [mg, 0] to cancel gravity without applying torque.

27:38 Compute the Quadrotor’s First-Order Taylor Model

The linear approximation expands f(x,u) around a nominal state and input using the Jacobians ∂f/∂x and ∂f/∂u.
At the hovering reference, f(x0,u0) = 0, so the local model is expressed using deviations from the reference state and control.
The lecture derives the state Jacobian A; its kinematic rows map velocities to position and angle rates, while the θ-to-vertical-acceleration entry is −g.

38:00 Derive the Control Jacobian and Assemble Linear Dynamics

For the six-state, two-input planar quadrotor, the control Jacobian B is a 6×2 matrix.
At the hovering reference, its nonzero entries are 1/m for total thrust’s effect on vertical acceleration and 1/I for moment’s effect on angular acceleration.
The resulting local model is ẋ ≈ A(x − x0) + B(u − u0), which is accurate near the chosen reference.

45:00 Discretize the Linear Model and Define the Trajectory Search

Forward Euler integration converts continuous-time dynamics into a stepwise relation between x(t) and x(t + Δt).
The optimization variables are states and control inputs over a finite horizon, such as a specified five- or ten-second interval.
The boundary conditions require the trajectory to start at xA and reach xB while satisfying the discretized dynamics at every step.

52:00 Add Control-Effort and Polytope Constraints to the QP

A sample objective minimizes the sum over time of squared control-input magnitudes, favoring lower total thrust and torque effort.
Linear constraints Cx(t) ≤ d can bound the state and approximate collision-free regions as polytopes.
The formulation combines endpoint conditions, linear dynamics, and state inequalities; obstacle geometry that cannot be captured by a suitable polytope needs a more general treatment.

58:00 Why Convex Quadratic Programs Solve Linear Planning Problems Efficiently

With linear dynamics, a quadratic control-effort objective, and linear constraints, the trajectory problem is a convex quadratic program (QP).
Convex objectives have bowl-shaped level sets, while linear inequalities define a feasible region such as the intersection of half-spaces.
The lecture notes that QP solvers can handle thousands of variables efficiently, with very short runtimes in well-implemented cases.

1:00:47 Use Sequential Quadratic Programming for Nonlinear Trajectories

For nonlinear dynamics and obstacle constraints, SQP repeatedly builds local linear approximations and solves QPs.
A typical iteration starts from a guessed path, updates the dynamics approximation around the latest trajectory, and increases penalties for obstacle violations.
SQP has no guarantee of finding a collision-free solution, especially in environments with many obstacles or complicated geometry.
Objectives can combine control effort, path length, and time; minimum-time control alone can produce bang-bang inputs that are harsh for physical hardware.

1:05:15 SQP Plans an Acrobot Swing-Up and a Fixed-Wing Obstacle Maneuver

The acrobot is an underactuated double pendulum with two degrees of freedom but only one motor, at the elbow joint.
An SQP trajectory takes the acrobot from a hanging state toward an upright balancing state while respecting its dynamics.
A fixed-wing aircraft trajectory plans a knife-edge maneuver through PVC-pipe obstacles spaced less than a wingspan apart, using a model that includes aerodynamics.

1:08:34 Precomputed Flight Funnels Enable Fast Runtime Collision Checking

To avoid solving trajectory optimization under tight runtime constraints, a library of candidate trajectories and associated guaranteed funnels can be computed offline.
At runtime, the aircraft searches the precomputed library and checks which funnel avoids the observed obstacles.
Planning does not guarantee exact execution on a real robot: wind, friction, and unmodeled aerodynamics create deviations, motivating feedback control as the next topic.

Summary, takeaways, and chapters were generated by AI from the video's transcript and may contain errors. The video belongs to its creator, Introduction to Robotics @ Princeton.