Trajectory Optimization and MPC

Step 1 — the Hamiltonian. With $x = (q, \dot q)$, $f = (x_2, u)$ and $\ell = u^2$: $H = u^2 + \lambda_1 x_2 + \lambda_2 u$ (eq. 11.26).

Step 2 — extremality. $\partial H/\partial u = 2u + \lambda_2 = 0$, so $u = -\lambda_2/2$ (eq. 11.31).

Step 3 — the adjoint equation. $\dot\lambda = -\partial H/\partial x$ gives $\dot\lambda_1 = 0$ and $\dot\lambda_2 = -\lambda_1$ (eq. 11.32): $\lambda_1$ is constant, $\lambda_2$ is affine in $t$, and therefore so is $u(t) = c_0 + c_1 t$.

Step 4 — the terminal conditions. $x_2(t_f) = \int_0^{t_f} u\,dt = 0$ gives $c_0 t_f + c_1 t_f^2/2 = 0$; $x_1(t_f) = \int_0^{t_f}\!\int_0^t u = d$ gives $c_0 t_f^2/2 + c_1 t_f^3/6 = d$. Solving: $c_0 = 6d/t_f^2$, $c_1 = -12d/t_f^3$.

Step 5 — convexity. $\partial^2 H/\partial u^2 = 2 > 0$: the extremal is a local minimum. The position is the cubic $q(t) = 3d(t/t_f)^2 - 2d(t/t_f)^3$ — the minimum-acceleration-energy profile every spline library rediscovers.

Discretized and solved numerically. With $N = 100$ stages of $\Delta t = 0.01$, $\ell_k = u_k^2\Delta t$ and a terminal weight of $10^6$ on $\|x_N - (1, 0)\|^2$, iLQR's first and only iteration returns $u_0 = 5.9405$ against Pontryagin's $6 - 12 \cdot 0.005 = 5.94$, with $\max_k |u_k - (6 - 12\,t_{k+1/2})| = 5.1 \times 10^{-4}$ and $x_N = (1, 0)$ to $10^{-4}$. One iteration, because the problem is linear-quadratic — the next derivation but one says why.

Step 1 — no $u$ in $\partial H/\partial u$. $H = 1 + \lambda_1 x_2 + \lambda_2 u$ is affine in $u$; $\partial H/\partial u = \lambda_2$ says nothing about $u$, and $\partial^2 H/\partial u^2 = 0$, so Legendre–Clebsch is silent. The extremality condition 11.28 is useless here.

Step 2 — use the inequality instead. The minimum principle 11.27 says $H^* \le H$ for every feasible $u$: $\lambda_2 u^* \le \lambda_2 u$ for all $|u| \le u_{\max}$.

Step 3 — so $u^*$ is at a bound. If $\lambda_2 < 0$ the inequality forces $u^* = u_{\max}$; if $\lambda_2 > 0$, $u^* = -u_{\max}$. The control lives on the boundary of its set — bang-bang.

Step 4 — at most one switch. $\lambda_2$ is affine in $t$ (same adjoint equation), so it changes sign at most once. Rest to rest over $d$: accelerate for $t_s$, brake for $t_s$, with $u_{\max}t_s^2 = d$, so $t_s = \sqrt{d/u_{\max}}$ — the $0.3963$ s of Chapter 18's micro-example read off a Hamiltonian instead of a phase plane.

Shooting. When the extremality conditions cannot be solved by hand, guess $\lambda(0)$, integrate state and adjoint with $u$ from $\partial H/\partial u = 0$, compare $x(t_f)$ with the target, and correct the guess — typically by a Gauss–Newton step on the terminal miss. This is where the "initial guess" problem is first explicit, and optim::lm below is the corrector.

Step 1 — choose a basis. Choset's list: polynomial coefficients, a truncated Fourier series, spline coefficients, wavelets, piecewise-constant accelerations or forces. The basis decides whether one parameter moves the whole trajectory or a finite support, which decides the conditioning of everything downstream.

Step 2 — sample the constraints. Torque limits 11.42 and obstacle constraints 11.43 are checked at $j\Delta t$, $j = 0 \dots k$; endpoint conditions 11.44–11.45 are equalities on $X$.

Step 3 — hand it to a solver. Sequential quadratic programming needs $J$, $g$, $h$ and their gradients, usually a Hessian approximation too, and all of it at least $C^1$ in $X$. Choset's two warnings are exactly the two the modern methods still trip on: smoothness in $X$, and local optima.

The modern names. (a) is state collocation: CHOMP's $\xi$ is a sampled $q(t)$, and its smoothness functional is the acceleration energy the dynamics would charge. (b) is direct shooting: iLQR parameterizes $u_k$ and integrates $f$ to get $x_k$. (c) is full collocation: TrajOpt carries both and enforces the dynamics as equalities. Choset's penalty variant — constraints folded into the objective — is CHOMP's obstacle term and TrajOpt's outer loop.

Step 1 — the smoothness functional is a quadratic. $\mathcal{F}_{smooth} = \tfrac12\sum_k\|q_{k+1} - q_k\|^2 = \tfrac12\|\mathsf{E}\xi + e\|^2$ with $\mathsf{E}$ the finite-difference operator on the free waypoints and $e$ carrying the fixed ends. Its gradient is $A\xi + b$ with $A = \mathsf{E}\T\mathsf{E} = \mathrm{tridiag}(-1, 2, -1) \otimes \mathbb{1}$, i.e. $2q_k - q_{k-1} - q_{k+1}$ at each free waypoint.

Step 2 — the obstacle functional is an arc-length integral. $\mathcal{F}_{obs} = \sum_\beta \int_0^1 c(D(x_\beta(q(s))))\,\|x_\beta'\|\,ds$: cost per metre travelled by each body point, so retiming the path does not change it, and a longer path through cost costs more.

Step 3 — differentiate under the integral. The variation of $c(D(x))\|x'\|$ with respect to the body point's path gives the Euler–Lagrange expression $\|x'\|\big[(\mathbb{1} - \hat x'\hat x'\T)\nabla c - c\,\kappa\big]$, with $\kappa = (\mathbb{1} - \hat x'\hat x'\T)x''/\|x'\|^2$ the path's curvature. The projection $(\mathbb{1} - \hat x'\hat x'\T)$ discards the component of $\nabla c$ along the path — pushing a bead along the band does nothing but retime it — and the curvature term says that where the cost is positive, straightening the path shortens the exposure.

Step 4 — pull back to joint space. A workspace force $f$ at body point $\beta$ is the joint torque $J_\beta\T f$ — Chapter 7's §4.7 lift, Chapter 17's eq. 10.12, the same virtual-work identity every time. Summing over body points gives $\nabla_q\mathcal{F}_{obs}$ at each waypoint. The library uses Chapter 7's controlPointJacobian for $J_\beta$ unchanged.

Step 5 — the covariant step. Minimize the first-order model $\mathcal{F}(\xi) + \nabla\mathcal{F}\T\delta\xi$ subject to $\tfrac12\,\delta\xi\T A\,\delta\xi \le$ const. The Lagrange condition gives $\delta\xi \propto -A^{-1}\nabla\mathcal{F}$: steepest descent in the metric that measures a perturbation by the smoothness it costs. $A$ is block-tridiagonal, so $A^{-1}\nabla\mathcal{F}$ is one Thomas sweep per coordinate, $O(N)$; the library's sweep agrees with the dense inverse to $4 \times 10^{-15}$.

The obstacle cost and its slope. $c(d) = -d + \epsilon/2$ for $d < 0$, $(\epsilon - d)^2/(2\epsilon)$ on $[0, \epsilon]$, $0$ above: $C^1$ at both seams, with $c'(0) = -1$ on both sides. The pointwise form $\nabla_q\mathcal{F}_{obs} = \sum_\beta J_\beta\T c'(D)\nabla D$ drops the arc-length weighting; it is what the micro-example pins exactly, and the library checks it against finite differences of its cost to $4 \times 10^{-10}$.

Why $A^{-1}$ spreads a push. $A^{-1}$ is dense and positive: its columns are tent functions peaked at the pushed waypoint and decaying linearly to the pinned ends. A force on one bead moves every bead, most where it was pushed — the band bends rather than kinks.

The step bound. Linearize the obstacle term as a Hessian of at most $\lambda/\epsilon$ per waypoint ($c'' = 1/\epsilon$ in the margin, $\|\nabla D\| \le 1$). The iteration matrix is $\mathbb{1} - \tfrac1\eta(\mathbb{1} + A^{-1}H)$ with eigenvalues $1 - (1 + \mu_i)/\eta$, $\mu_i \in [0, (\lambda/\epsilon)/\lambda_{\min}(A)]$ and $\lambda_{\min}(A) = 2 - 2\cos(\pi/(m+1))$. It is a contraction iff $\eta > \tfrac12(1 + (\lambda/\epsilon)/\lambda_{\min}(A))$. For $m = 1$, $\epsilon = \tfrac12$: $\eta > (1 + \lambda)/2$. For $m = 20$, $\lambda = 2$, $\epsilon = \tfrac12$: $\eta^* = 90.0$; the library runs at $2\eta^*$ without a single cost increase in $400$ steps and at $\eta^*/4$ records $36$ increases in $100$. The bound is worst-case — a path with few waypoints inside the margin tolerates a smaller $\eta$ — and it is the mechanism behind the widget's headline.

STOMP's gradient-free replacement. Sample $\delta\xi_k \sim \mathcal{N}(0, A^{-1})$ for $K$ rollouts — smooth noise, because its covariance is the smoothness metric — evaluate $S_k = \mathcal{F}(\xi + \delta\xi_k)$, weight $w_k \propto \exp(-h(S_k - S_{\min})/(S_{\max} - S_{\min}))$, update by $\sum_k w_k\delta\xi_k$ smoothed once more through $A^{-1}$. No gradient is formed, so any cost that can be evaluated can be optimized. On the micro-example at $\lambda = 2$, seed 7, $K = 24$, 120 iterations end at $y = 0.6566$ against CHOMP's $y^* = 0.6667$, with the accepted cost never rising.

Step 1 — Bellman. $V_k(x) = \min_u\big[\ell(x, u) + V_{k+1}(f(x, u))\big]$, with $V_N = \ell_f$.

Step 2 — expand about the nominal. Write $x = \bar x_k + \delta x$, $u = \bar u_k + \delta u$, and expand the bracket to second order: the coefficients are the $Q$ terms of the statement. Dropping the second derivatives of $f$ — the tensor terms $V'_x\cdot f_{xx}$, $V'_x\cdot f_{uu}$, $V'_x\cdot f_{ux}$ — is iLQR; keeping them is DDP. The library computes them by central differences of the Jacobians when ddp: true.

Step 3 — minimize the quadratic in $\delta u$. $\partial/\partial\delta u = 0$ gives $Q_u + Q_{uu}\delta u + Q_{ux}\delta x = 0$, i.e. $\delta u = -Q_{uu}^{-1}(Q_u + Q_{ux}\delta x) = k + \mathbf{K}\delta x$. This is a policy: a feedforward correction plus a linear feedback on the deviation from the nominal.

Step 4 — substitute back. The minimized quadratic is the new value model: $V_x = Q_x + \mathbf{K}\T Q_{uu}k + \mathbf{K}\T Q_u + Q_{ux}\T k$ and $V_{xx} = Q_{xx} + \mathbf{K}\T Q_{uu}\mathbf{K} + \mathbf{K}\T Q_{ux} + Q_{ux}\T\mathbf{K}$, which collapse to the statement's forms once $k$ and $\mathbf{K}$ are substituted (Tassa et al. keep the cross terms for numerical symmetry; so does the library).

Step 5 — recurse, then roll out. From $V_N = \ell_f$ down to $k = 0$. The forward pass is closed-loop: $u_k = \bar u_k + \alpha k_k + \mathbf{K}_k(x_k - \bar x_k)$, with backtracking on $\alpha$ against the expected decrease $\Delta V(\alpha) = \alpha\sum_k k_k\T Q_u + \tfrac12\alpha^2\sum_k k_k\T Q_{uu}k_k$. A step is accepted when the realized decrease is a reasonable fraction of the promise.

Regularization. $Q_{uu}$ need not be positive definite far from a minimum; the library adds $\mu\mathbb{1}$, raises $\mu$ tenfold when a Cholesky pivot fails or no $\alpha$ descends, and lowers it on success. Defaults: $\mu_0 = 10^{-6}$ (zero for linear-quadratic checks), $\alpha \in \{1, \tfrac12, \dots, 1/64\}$.

On a linear-quadratic problem one pass is Riccati. If $f = Ax + Bu$ and $\ell$ is quadratic about the origin, $f_x = A$, $f_u = B$ and the recursion for $V_{xx}$ is exactly the discrete Riccati recursion $P_k = Q + A\T P_{k+1}A + A\T P_{k+1}B\,\mathbf{K}_k$ with $\mathbf{K}_k = -(R + B\T P_{k+1}B)^{-1}B\T P_{k+1}A$. The library computes that recursion independently; on a 40-stage double-integrator problem the gains agree to $1.1 \times 10^{-15}$ and the $\alpha = 1$ forward pass lands on the optimal cost $\tfrac12 x_0\T P_0 x_0 = 10.88611263$ in one iteration.

The first nonlinear term that breaks this. For the unicycle, $f_x$ depends on $\theta$ through $\cos\theta$, $\sin\theta$; the rollout is no longer the linear prediction, $\alpha = 1$ may overshoot, and the pass has to be repeated. Hitch's parking problem below takes 79 iLQR iterations, or 30 with DDP's second-order terms, and the two reach different local optima — DDP's cheaper on this instance, $56.24$ against $60.62$, both parking within 5 cm and 0.05 rad.

Step 1 — the $\ell_1$ penalty is exact. For a finite $\mu_{pen}$ larger than the constraints' multipliers, the minimizer of $J + \mu_{pen}\sum|g_i|^+$ satisfies $g_i \le 0$ exactly — not approximately, as a quadratic penalty would. The hinge is non-smooth, which is why the subproblem is solved as a convex program rather than by a gradient step.

Step 2 — convexify. Replace $J$ by its quadratic model and each $g_i$ by its linearization; the hinge of an affine function is convex. For collision, $g = d_{safe} - sd(q)$ and the linearization at the witness pair is $d_{safe} - sd(q_0) - \hat n\T J_p(q_0)\,\delta q$ — exactly the contact normal times the body-point Jacobian, Chapter 7's lift once more.

Step 3 — the trust region guarantees descent on the merit. The convex model agrees with the true merit to first order; inside a small enough box the true merit decreases when the model's does. Accept and expand $\Delta$ on success; reject and shrink on failure.

Step 4 — the penalty loop. When the inner loop converges with violations left, multiply $\mu_{pen}$ and repeat. Swept volumes between consecutive footprints are handled as convex hulls of the two shapes — a recipe this book states and does not implement.

What our sco simplifies, and says so. The convex subproblem is a QP in TrajOpt. Here it is the book's own Levenberg–Marquardt on the residual vector $[\mathsf{E}\xi + e;\ \sqrt{\mu}\,|g_{lin}|^+]$, boxed by the same $\Delta$ — a recipe in TrajOpt's shape, not TrajOpt. On the twelve-waypoint disc problem it ends after 27 convexifications at $\mu_{pen} = 10^5$ with every waypoint at least $d_{safe} = 0.1$ from the disc, the tightest at $0.1000$ and a violation of $6 \times 10^{-7}$; CHOMP on the same scene with $\lambda = 1$, $\epsilon = 0.5$ settles at a clearance of $0.471$ — far more than asked, because it never saw the constraint. Cost versus constraint, in one number each.