Introduction

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.

Smooth the Path

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.

<!-- Path smoothing demo — drag waypoints, press Optimize to run gradient descent -->
<p class="hint">Drag the <b>red waypoints</b> to reshape the path, then press <b>Optimize</b> to watch gradient descent smooth it around the obstacle. The optimizer minimizes jerk and maximizes clearance from the circle.</p>
<canvas id="canvas" width="420" height="300"></canvas>
<div class="status" id="status">Drag waypoints, then optimize.</div>
<div class="btns">
  <button id="btn-optimize" type="button">Optimize</button>
  <button id="btn-reset" type="button" class="ghost">Reset</button>
</div>

/* Demo styles — canvas + controls */
* { 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; }

/* Trajectory optimizer — smoothness + obstacle clearance costs, finite-difference gradient */
const i18nFmt = (s, v) => s.replace(/%(\w+)%/g, (_, k) => String(v[k] ?? ''));

const canvas = document.getElementById('canvas');
const ctx = canvas.getContext('2d');
const W = canvas.width, H = canvas.height;

/* Circular obstacle: center + radius */
const OBS = { x: W * 0.5, y: H * 0.5, r: 44 };

/* Initial (jerky) waypoint positions as fractions of canvas size */
const INITIAL_PTS = [
  [W * 0.08, H * 0.85],
  [W * 0.25, H * 0.15],
  [W * 0.50, H * 0.80],
  [W * 0.75, H * 0.15],
  [W * 0.92, H * 0.85]
];

/* Optimization hyperparameters: smoothness weight, clearance weight, learning rate, iteration count */
const SMOOTH_W = 6.0;
const CLEAR_W  = 2200.0;
const LR       = 0.018;
const ITERS    = 400;

let pts = INITIAL_PTS.map(p => [...p]);
let origPts = INITIAL_PTS.map(p => [...p]);
let dragging = -1;
let animId = null;

/* Smoothness cost = sum of squared distances between consecutive waypoints */
function smoothnessCost(p) {
  let c = 0;
  for (let i = 1; i < p.length; i++) {
    const dx = p[i][0] - p[i-1][0];
    const dy = p[i][1] - p[i-1][1];
    c += dx * dx + dy * dy;
  }
  return c;
}

/* Clearance cost = squared penetration into obstacle margin for each interior waypoint */
function clearanceCost(p) {
  let c = 0;
  for (let i = 1; i < p.length - 1; i++) {
    const dx = p[i][0] - OBS.x;
    const dy = p[i][1] - OBS.y;
    const dist2 = dx * dx + dy * dy;
    const margin = OBS.r + 14;
    if (dist2 < margin * margin) {
      const pen = margin - Math.sqrt(dist2);
      c += pen * pen;
    }
  }
  return c;
}

/* Gradient step via central finite differences — moves each interior waypoint to reduce total cost */
function gradStep(p) {
  const EPS = 1.5;
  const next = p.map(pt => [...pt]);
  for (let i = 1; i < p.length - 1; i++) {
    for (let dim = 0; dim < 2; dim++) {
      const orig = p[i][dim];
      p[i][dim] = orig + EPS;
      const cp = SMOOTH_W * smoothnessCost(p) + CLEAR_W * clearanceCost(p);
      p[i][dim] = orig - EPS;
      const cm = SMOOTH_W * smoothnessCost(p) + CLEAR_W * clearanceCost(p);
      p[i][dim] = orig;
      const grad = (cp - cm) / (2 * EPS);
      next[i][dim] = orig - LR * grad;
    }
  }
  return next;
}

/* Draw the canvas frame — obstacle, path line, and waypoint dots */
function draw(p, optimized) {
  ctx.clearRect(0, 0, W, H);

  /* Draw the circular obstacle */
  ctx.beginPath();
  ctx.arc(OBS.x, OBS.y, OBS.r, 0, Math.PI * 2);
  ctx.fillStyle = '#e8eef3';
  ctx.fill();
  ctx.strokeStyle = '#adb1b8';
  ctx.lineWidth = 1.5;
  ctx.stroke();

  /* Draw the path — dashed blue when unoptimized, solid green when done */
  ctx.beginPath();
  ctx.moveTo(p[0][0], p[0][1]);
  for (let i = 1; i < p.length; i++) ctx.lineTo(p[i][0], p[i][1]);
  ctx.strokeStyle = optimized ? '#0a7d33' : '#1d3557';
  ctx.lineWidth = optimized ? 2.5 : 1.5;
  ctx.setLineDash(optimized ? [] : [6, 4]);
  ctx.stroke();
  ctx.setLineDash([]);

  /* Draw waypoint dots — blue for endpoints, red for interior (draggable) points */
  for (let i = 0; i < p.length; i++) {
    ctx.beginPath();
    ctx.arc(p[i][0], p[i][1], i === 0 || i === p.length - 1 ? 7 : 5, 0, Math.PI * 2);
    ctx.fillStyle = i === 0 || i === p.length - 1 ? '#1d3557' : '#e63946';
    ctx.fill();
    ctx.strokeStyle = '#fff';
    ctx.lineWidth = 1.5;
    ctx.stroke();
  }
}

function setStatus(txt, cls) {
  const s = document.getElementById('status');
  s.textContent = txt;
  s.className = 'status ' + cls;
}

/* Run the optimizer — animate one gradient step per animation frame */
function optimize() {
  if (animId) return;
  document.getElementById('btn-optimize').disabled = true;
  setStatus('Optimizing…', 'info');
  let step = 0;
  function tick() {
    pts = gradStep(pts);
    step++;
    if (step % 20 === 0) draw(pts, false);
    if (step < ITERS) {
      animId = requestAnimationFrame(tick);
    } else {
      animId = null;
      draw(pts, true);
      document.getElementById('btn-optimize').disabled = false;
      setStatus(i18nFmt('Done — %n% gradient steps applied.', { n: step }), 'ok');
    }
  }
  animId = requestAnimationFrame(tick);
}

/* Reset waypoints to their initial positions and cancel any running animation */
function reset() {
  if (animId) { cancelAnimationFrame(animId); animId = null; }
  pts = INITIAL_PTS.map(p => [...p]);
  document.getElementById('btn-optimize').disabled = false;
  draw(pts, false);
  setStatus('Drag waypoints, then optimize.', '');
}

/* Drag-and-drop support for both mouse and touch input */
function ptIndex(ex, ey) {
  for (let i = 1; i < pts.length - 1; i++) {
    const dx = pts[i][0] - ex, dy = pts[i][1] - ey;
    if (Math.sqrt(dx * dx + dy * dy) < 12) return i;
  }
  return -1;
}
function evtXY(e) {
  const r = canvas.getBoundingClientRect();
  const scaleX = W / r.width, scaleY = H / r.height;
  if (e.touches) {
    return [(e.touches[0].clientX - r.left) * scaleX, (e.touches[0].clientY - r.top) * scaleY];
  }
  return [(e.clientX - r.left) * scaleX, (e.clientY - r.top) * scaleY];
}
canvas.addEventListener('mousedown', e => { const [x, y] = evtXY(e); dragging = ptIndex(x, y); });
canvas.addEventListener('mousemove', e => {
  if (dragging < 0) return;
  const [x, y] = evtXY(e);
  pts[dragging] = [Math.max(4, Math.min(W - 4, x)), Math.max(4, Math.min(H - 4, y))];
  draw(pts, false);
  setStatus('Drag waypoints, then optimize.', '');
});
canvas.addEventListener('mouseup', () => { dragging = -1; });
canvas.addEventListener('touchstart', e => { e.preventDefault(); const [x, y] = evtXY(e); dragging = ptIndex(x, y); }, { passive: false });
canvas.addEventListener('touchmove', e => {
  e.preventDefault();
  if (dragging < 0) return;
  const [x, y] = evtXY(e);
  pts[dragging] = [Math.max(4, Math.min(W - 4, x)), Math.max(4, Math.min(H - 4, y))];
  draw(pts, false);
}, { passive: false });
canvas.addEventListener('touchend', () => { dragging = -1; });

document.getElementById('btn-optimize').addEventListener('click', optimize);
document.getElementById('btn-reset').addEventListener('click', reset);

draw(pts, false);
Result
Console

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.

The Real Complexity

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 nn waypoints. Finer discretization (n→∞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.

Where It Matters

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.

Conclusion

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.

Share this article

Pick a channel — or use your device's native share sheet.

Comments

Loading comments...

https://www.kipuhub.com/en/article/trajectory-optimization/Content licensed under CC BY-NC 4.0.