Trajectory Optimization

Give a robot a list of waypoints and it will try to visit each one in sequence. The result is jerky: hard right angles, instantaneous direction changes, velocity spikes that no physical motor can sustain. The robot either ignores the waypoints or shakes itself apart.

Trajectory optimization is the layer that turns a rough path into a dynamically feasible one — a smooth curve that a real system can actually execute. It does this by treating the path as a variable and minimizing a cost function: penalize jerk (the derivative of acceleration), penalize dangerous proximity to obstacles, enforce kinematic limits, and let a numerical solver reshape the waypoints until the cost is as low as possible.

The two landmark algorithms — CHOMP (Covariant Hamiltonian Optimization for Motion Planning, Ratliff et al. 2009) and TrajOpt (Schulman et al. 2013) — made this pipeline fast enough for real-time robotics. Both start from a rough initial guess and iterate, but they differ in which cost they minimize and how they handle obstacles. The core idea, however, is older than either: it is calculus of variations applied to motion.

Drag the red waypoints to build any rough path you like, then press Optimize to watch gradient descent reshape it into a smooth trajectory. Press Reset to go back to the original.

<!-- {{c_html_intro}} -->
<p class="hint">{{hint_para}}</p>
<canvas id="canvas" width="420" height="300"></canvas>
<div class="status" id="status">{{status_idle}}</div>
<div class="btns">
  <button id="btn-optimize" type="button">{{btn_optimize}}</button>
  <button id="btn-reset" type="button" class="ghost">{{btn_reset}}</button>
</div>

/* {{c_css_intro}} */
* { box-sizing: border-box; }
body { font-family: system-ui, sans-serif; color: #222; margin: 0; }
.hint { font-size: .9rem; color: #444; margin: 0 0 .6rem; line-height: 1.45; }
canvas { display: block; border: 1px solid #cdd9e3; border-radius: 8px;
         background: #f4f7fa; cursor: crosshair; max-width: 100%; touch-action: none; }
.status { font-size: .95rem; font-weight: 600; margin: .5rem 0; min-height: 1.4em; }
.status.ok { color: #0a7d33; }
.status.info { color: #1d3557; }
.status.bad { color: #c92f3c; }
.btns { display: flex; gap: .5rem; flex-wrap: wrap; }
button { font: 600 14px system-ui, sans-serif; padding: .45rem .9rem;
         border: 1px solid #1d3557; background: #1d3557; color: #fff;
         border-radius: 8px; cursor: pointer; }
button.ghost { background: #fff; color: #1d3557; }
button:disabled { opacity: .45; cursor: default; }

// Code not found

The optimizer minimizes a weighted sum of two terms: a smoothness cost (summed squared differences between consecutive points, which penalizes sharp turns) and a clearance cost (a repulsion from the circular obstacle in the middle). Each iteration nudges every waypoint in the direction that reduces the total cost. After enough steps the path bends smoothly around the obstacle — exactly what CHOMP does in higher dimensions and with real geometry.

How hard is trajectory optimization, formally?

• The objective is nonlinear. Smoothness costs are quadratic (easy), but obstacle avoidance introduces nonconvex constraints — the feasible region is a maze of concave holes. In general, finding the global optimum of a nonconvex program is NP-hard.
• Local minima are everywhere. Both CHOMP and TrajOpt find only local optima: the path they return is smooth and collision-free around where the search started, but a completely different path with lower cost might exist. Multiple random restarts are often used to escape bad local optima.
• Sequential Convex Optimization (TrajOpt) sidesteps the worst nonconvexity by replacing each obstacle constraint with a linear approximation at the current iterate and solving the resulting convex Quadratic Program (QP). This is fast (a QP solver finishes in milliseconds), but the guarantee only holds locally — if the linearization is too coarse, the solver can violate the original constraint.
• Discretization error. A trajectory is a continuous curve; we represent it as $n$ waypoints. Finer discretization ($n \to \infty$) approaches the true variational problem but makes the QP larger. Coarser discretization is fast but misses dynamic obstacles that fit between waypoints.
• Practical verdict. Despite the worst-case hardness, trajectory optimization works in practice because the cost landscape near a reasonable initial guess is "nearly convex," and the typical solution time is tens of milliseconds — fast enough for real-time replanning. The gap between theory and practice is large, and it is an active research area.

The problem is therefore in the same boat as non-convex optimization: globally hard, locally tractable, and tamed in practice by good initialization and domain structure.

Anywhere a physical system must move from A to B while respecting dynamics and avoiding obstacles, trajectory optimization is at work:

• Robot arm motion planning: CHOMP and TrajOpt both originated in manipulator planning. A robot arm picking objects off a conveyor belt needs a collision-free, jerk-limited path replanned hundreds of times per minute.
• Legged locomotion: bipedal and quadruped robots (Boston Dynamics, Unitree) use whole-body trajectory optimization to plan footstep sequences that balance the center of mass while the legs swing.
• Autonomous vehicles: real-time trajectory planning for lane changes and emergency maneuvers solves a constrained nonlinear program under time pressure, often via model-predictive control (MPC) with a quadratic objective.
• Spacecraft and launch vehicles: NASA and SpaceX trajectory designers minimize fuel burn subject to orbital mechanics — a classical optimal-control problem solved by direct transcription (Hermite-Simpson, Gauss-Lobatto) and nonlinear programming.
• Surgical robotics: endoscopes and laparoscopic tools must follow smooth, instrument-safe paths inside the body; trajectory optimization respects tissue-contact limits.
• Animation and motion capture: smoothing and retargeting motion-capture data uses the same jerk-minimization objective to produce plausible, natural-looking character motion.

In every case the algorithmic pattern is identical: parameterize the path, define a cost, and iterate. The difference is which physics the cost encodes. Learn trajectory optimization and you have learned the blueprint for dynamic programming applied to continuous motion.

A list of waypoints is an aspiration; a trajectory is a commitment. Trajectory optimization is the step in between — it takes the wishful thinking out of motion planning and replaces it with mathematics: a cost function, a gradient, and an iterative solver that reshapes the path until it is as smooth and safe as the physics allow.

CHOMP brought the calculus of variations into real-time robotics. TrajOpt made collision avoidance a linear constraint that a QP solver can handle in milliseconds. Neither guarantees the global best path — the problem is too hard for that — but both reliably find good local solutions from a reasonable start.

The next time you watch a robot arm glide gracefully to a target, or a rover thread a boulder field on Mars, remember: behind that smooth arc is a numerical optimizer running gradient steps on a cost function, turning a jerky guess into motion a physical system can actually follow.