ALTRO: The Augmented Lagrangian Trajectory Optimizer

Why this matters. Say you must steer an underactuated robot — a cart-pole, a quadrotor, a 7-DOF arm — through a cluttered room, subject to torque limits and obstacle constraints, from rest to a target pose. Sampling-based planners struggle with dynamic constraints, and differential flatness only works for special systems. The general tool is trajectory optimization: search over the whole time history of controls (and states) to minimize a cost while satisfying the dynamics and any constraints. This is the problem ALTRO solves, fast.

The mathematical form (following the ALTRO paper, Howell, Jackson & Manchester, IROS 2019):

Typical costs are quadratic (control effort, distance to target); typical constraints are torque limits, joint limits, obstacle avoidance, and terminal state goals.

The paper’s contributions, which this page derives one by one: (1) a numerically robust square-root backward pass; (2) a way to start from a dynamically infeasible state trajectory; (3) a free-time formulation; and (4) an objective-weighted Newton projection for solution polishing.

Why start here. Every piece of ALTRO enters through iLQR, so we need its mechanism cold before anything else. The idea is old and beautiful: the problem has a chain (Markov) structure — each \(x_{k+1}\) depends only on \((x_k,u_k)\) — so we can peel it step by step from the end, exactly like dynamic programming for shortest paths, but in continuous state space using local quadratic models.

2.1 The value function and the local model

Define the cost-to-go \(V_k(x_k)\): the cheapest remaining cost from state \(x_k\) using the best possible controls \(u_k,\dots,u_{N-1}\). Bellman’s principle gives

We cannot solve this exactly for nonlinear \(f,\ell\), so we linearize the dynamics about the current nominal trajectory \((\bar{x}_k, \bar{u}_k)\) and quadratize the cost. Write the value function locally as \[ V_{k+1}(\bar x_{k+1} + \delta) \;\approx\; \tfrac12 \delta^\mathsf{T} P_{k+1}\,\delta + p_{k+1}^\mathsf{T}\delta . \] Then the local expansion of the quantity inside the \(\min\) — the action-value function \(Q_k\) — has (dropping stage indices, primed quantities evaluated at \(k{+}1\), \(\delta x = x-\bar x\), \(\delta u = u-\bar u\)):

with \(A = \partial f/\partial x\), \(B = \partial f/\partial u\) evaluated at \((\bar x_k,\bar u_k)\). These six blocks are all a Riccati sweep needs. Why drop second derivatives of \(f\)? Because the \(\ell_{xx}\) terms are usually available and cheap, while \(f\)'s second derivatives are expensive and often destabilizing — the Gauss–Newton choice keeps the model positive-semidefinite-like and reliable, and it is what ALTRO uses.

2.2 The backward pass: one Riccati sweep

Minimizing the quadratic model over \(\delta u\) — with a small regularization \(\rho_{\mathrm{reg}} \ge 0\) to keep \(Q_{uu} + \rho_{\mathrm{reg}} I\) safely invertible — yields the optimal control correction, split into a feedforward term \(d_k\) and a feedback gain \(K_k\):

Sweeping \(k = N-1, N-2, \dots, 0\) with terminal \(P_N = \ell_{xx}(x_N)\), \(p_N = \ell_x(x_N)\) produces the gain sequence \(\{K_k, d_k\}\) — the exact optimal feedback law of the local LQR problem. This is the whole backward pass: \(O(N)\) steps, each just a few small matrix products.

2.3 The forward pass, line search, and iteration

The gains describe the best local policy, not the final answer for a nonlinear problem. We re-simulate the system from \(x_0\) with the updated policy, scaled by a step size \(\alpha \in [0,1]\):

Why \(\alpha\)? The quadratic model is only locally accurate; by backtracking \(\alpha = 1, \tfrac12, \tfrac14, \dots\) until the true cost decreases, we guarantee progress (a line search on the actual objective). Then we rebuild the model at the new trajectory and repeat: backward sweep, forward roll, repeat. This is Iterative Linear Quadratic Regulator (iLQR) — and with the exact second-order terms included it becomes DDP; the mathematical core is the same.

2.4 Worked example: two steps of a 1-D chain

Let’s make the machinery concrete. Take \(N=2\) steps, \(x_{k+1} = x_k + u_k\), \(x_0 = 1\), cost \(J = \tfrac12 u_0^2 + \tfrac12 u_1^2 + \tfrac12 x_2^2\) (softly reach \(x=0\)). All numbers below are exact rationals, verified by computer.

Rolling the whole policy forward from \(x_0 = 1\): \(u_0 = -\tfrac13\), \(x_1 = 1-\tfrac13 = \tfrac23\), then \(u_1 = -\tfrac12 - \tfrac12(\tfrac23-1) = -\tfrac13\), \(x_2 = \tfrac13\). Cost: \(\tfrac12(\tfrac19)+\tfrac12(\tfrac19)+\tfrac12(\tfrac19) = \tfrac16\). The exact optimum (solve the 2-variable quadratic directly) is \((u_0^*,u_1^*) = (-\tfrac13,-\tfrac13)\) with \(J^* = \tfrac16\). One sweep landed exactly on the optimum. That is the LQR-exactness of iLQR: for linear dynamics and quadratic cost the model is globally exact, so the first Riccati sweep is Newton’s method and converges in one step. Nonlinearities are precisely the reason the loop iterates.

Why the augmented Lagrangian (AL) and not a plain penalty? The naïve penalty method keeps the constraints’ residual \(g(x)\) only as a squared term, \(\tfrac12\rho\,g^2\), and grows \(\rho \to \infty\) to drive the solution onto the constraint. The 1-D picture explains both why that works and why it is the wrong tool:

Figure 1. Minimize \(\tfrac12 x^2\) subject to \(x = 2\). (a) Penalty method: the minimizer of \(\tfrac12 x^2 + \tfrac12\rho(x-2)^2\) is \(x^* = 2\rho/(1+\rho)\), inching toward 2 as \(\rho\to\infty\) — never exact, and the objective’s valley narrows with condition number \(\approx 1+\rho\) (shades of §5). (b) Augmented Lagrangian, \(\rho = 1\) fixed: adding the multiplier term \(\lambda(x-2)\) slides the minimum onto \(x = 2\) at a finite penalty. The multiplier — not the penalty — does the work.

3.1 The augmented Lagrangian

For constraints \(g \le 0\) and \(h = 0\), ALTRO combines them into \(c = (g,h)\) and augments every stage cost with the indicator-weighted term

Why the indicator \(I_\rho\)? For an inequality that is strictly satisfied (\(g < 0\)) and never active (\(\lambda = 0\)), the constraint costs nothing — it is simply off. Once violated (or once its multiplier is positive), the penalty switches on and the inequality is treated with the same machinery as an equality. This active-set trick is what lets one framework swallow arbitrary mixed state-action constraints.

3.2 The outer loop: dual and penalty updates

AL methods abandon the \(\rho \to \infty\) chase. Instead the multiplier is the memory: the gradient of the constraint violation accumulates into \(\lambda_k\). After each minimization of the augmented cost (with \(\lambda,\rho\) held fixed — here, a few iLQR sweeps), update

The \(\max(0,\cdot)\) for inequalities enforces complementary slackness — a multiplier for a strictly satisfied inequality would otherwise drift negative and pull the trajectory across the boundary. The penalty grows only geometrically, \(\rho \leftarrow \phi\rho\) (ALTRO typically \(\phi = 10\)) up to a cap; the exactness comes from \(\lambda\), not from \(\rho\).

AL-iLQR (paper Algorithm 1)
 1  input: x0, U, tolerance
 2  initialize lambda = 0, rho = 1, growth factor phi
 3  while max constraint violation > tolerance:
 4      (X, U) ← minimize L_A(X, U; lambda, rho) with iLQR     # inner loop
 5      lambda ← dual update (eq. above)                        # outer loop
 6      rho    ← min(rho_max, phi * rho)
 7  return X, U, lambda

3.3 Why this converges (without blowing up the conditioning)

Figure 2. Outer-loop convergence on the 1-D problem of Figure 1. The penalty curve (blue) marks its \(\rho\) value at each iterate — it only makes progress by pushing \(\rho\) to \(10^8\), which is exactly the regime that destroys the inner problem’s conditioning. Augmented Lagrangian with fixed \(\rho = 10\) and multiplier updates (purple) contracts the violation by a factor \(\approx 1+\rho\) per outer iteration — here, machine precision in eight iterations at a penalty that never stresses the inner solver. (Derivation: with multiplier error \(e_k = \lambda_k - \lambda^*\) — here \(\lambda^* = -2\) — the dual update gives \(e_{k+1} = e_k/(1+\rho)\).)

Why assemble them? iLQR (fast, structure-exploiting, no constraints) plus AL (general constraint machinery, but it needs a good unconstrained solver inside) is a marriage with a long lineage — ALTRO’s phase-1 solver is exactly AL-iLQR: the algorithm of §3 with the iLQR of §2 as its inner minimizer.

4.1 How the constraints enter the inner iLQR

Inside the backward pass, the augmented stage cost \(\ell_k + \lambda_k^\mathsf{T} c_k + \tfrac12 c_k^\mathsf{T} I_\rho c_k\) contributes, under the Gauss–Newton treatment of the constraints (linearized \(c\), dropped second derivatives), exactly four extra terms to each \(Q\)-block:

Why this shape? The multiplier acts like a state-dependent gradient (\(c_x^\mathsf{T}\lambda\) pushes along the constraint gradient), while the penalty adds curvature (\(c_x^\mathsf{T} I_\rho c_x\)) that makes the local QP trust the constraint direction. Nothing else about the backward pass changes — which is the design win: the constraint machinery is data, not a new algorithm.

4.2 The whole loop, end to end, on a real problem

To see phase 1 behave (both its power and its honest limitations), here is a miniature ALTRO — the exact structure above, implemented from scratch in a compact numpy script — applied to a 2-D double-integrator (point mass) that must reach the goal \((1,1)\) while staying outside a circular obstacle. Because the dynamics are linear and the cost quadratic, the Riccati sweep is exact; the only nonlinear object is the obstacle constraint — which is the thing we are here to learn about. (Full source: mini_altro.py in this folder.)

Figure 3. Mini-ALTRO on “avoid the circle, reach the goal”. (a) The early iterate (gray dashed) cuts straight through the obstacle; the final trajectory (purple) arcs over it and lands on \((1,1)\). (b) The outer-loop max constraint violation \(c_{\max} = \max_k \max(0,g_k(x_k))\): a long plateau while the multiplier accumulates, then a phase transition — the detour finally costs less than cutting through, and \(c_{\max}\) snaps to \(0\). (c) The resulting control profile (steep maneuver away from the circle). The plateau-then-cliff shape is characteristic of plain AL-iLQR and is exactly the “slow tail” the paper reports; §6 shows the fix.

Motivation. AL convergence improves with larger \(\rho\) (§3.3), and ALTRO pushes \(\rho\) to \(10^4\)–\(10^6\) to drive constraints below \(10^{-6}\). But every outer iteration inflates the condition number of the value-function matrix \(P_k\) that the Riccati recursion maintains. A badly conditioned \(P\) means the inner LQR solve loses digits — on hardware with single precision this is fatal. The paper’s first contribution is to run the recursion on a square-root factor instead.

Figure 4. A 2-state Riccati recursion whose stage Hessian carries an active penalty term \(\rho\, c_x c_x^\mathsf{T}\) (the AL regime). The value matrix’s condition number grows proportionally to \(\rho\) (slope 1, top); the square-root factor \(S\) with \(S^\mathsf{T}S = P\) has \(\kappa(S) = \sqrt{\kappa(P)}\) (slope 1/2, bottom) — doubling the usable dynamic range before float precision runs out (the dashed wall at \(1/\varepsilon \approx 4.5\times 10^{15}\) for IEEE double). Curves shown are exact (rational) and matching numerically; data in figD_conditioning.py.

5.1 Matrix square roots for sums and differences

The tools are classical (square-root Kalman filtering). Given upper-triangular factors \(\sqrt{A}, \sqrt{B}\) with \(\sqrt{A}^\mathsf{T}\sqrt{A} = A\) (likewise \(B\)):

QR of the stacked factors gives an upper-triangular \(R\) with \(R^\mathsf{T}R = A + B\) — without ever forming the sum. The downdate \(\mathrm{DD}\) removes one rank at a time (the rank-one downdate of a Cholesky factor). Why bother? Forming \(A + B\) or \(A - B\) explicitly adds and subtracts across wildly different scales when \(\rho\) is large; the factorized form keeps relative digits of each scale.

5.2 The square-root recursion (structure, not bookkeeping)

Define \(S_k = \sqrt{P_k}\) with \(S_k^\mathsf{T} S_k = P_k\), and let \(Z_{xx} = \sqrt{Q_{xx}}\), \(Z_{uu} = \sqrt{Q_{uu}}\) be the factors of the action-value Hessians. The factorized backward pass is (paper eqs. 19–29):

Every update is either a \(\{ \text{multiply, solve, QR} \}\) step or a \(\{ \text{multiply, solve, downdate} \}\) step — no large-matrices mixing of scales, no subtraction of near-equal numbers. The scalar gains and expected decrease can be read off the factors: \(p = Q_x + (Z_{uu}K)^\mathsf{T}(Z_{uu}d) + K^\mathsf{T}Q_u + Q_{xu}d\) and \(\Delta V = d^\mathsf{T}Q_u + \tfrac12 (Z_{uu}d)^\mathsf{T}(Z_{uu}d)\), keeping the same \(O(N(n^3 + m^3))\) cost as the standard pass.

Motivation. Phase 1 gets you to constraint violations around \(10^{-3}\)–\(10^{-4}\) quickly, but AL-iLQR’s tail is slow — in the paper’s cartpole benchmark, Figure 1, plain AL-iLQR never reaches the target tolerance at all, while ALTRO with polishing converges to \(10^{-8}\) in roughly the same wall-clock time as a single IPOPT solve. The plateau in Figure 3(b) is the same phenomenon. Polishing trades the AL slow tail for a Newton projection onto the active-constraint manifold.

6.1 The projection problem

Collect the decision variables \(z = (x_0, u_0, \dots, u_{N-1}, x_N)\) and the active constraints \(\bar E(z) = 0\): the dynamics plus the equality constraints plus the currently-active inequalities (detected by the multipliers from phase 1). Phase 2 solves the minimum-norm problem

where the norm is weighted by the objective Hessian \(J\): among all points satisfying the constraints exactly, take the one nearest to the coarse solution in the cost’s notion of distance. Each Newton step on this problem is the projection map

followed by a backtracking line search on the constraint residual \(\|\bar E(z + \alpha\,\delta z)\|_\infty < \|\bar E(z)\|_\infty\). The big middle factorization \((\bar E_z J^{-1} \bar E_z^\mathsf{T})^{-1}\) is reused across steps and only refactored when convergence slows or the active set changes, and the chain structure of \(\bar E\) lets the whole procedure run sequentially without ever building the large matrices — keeping it embeddable.

7.1 Start from a state trajectory, not a control trajectory

Why. For direct methods it is trivial to hand a solver a state path (from a sampling planner or an expert sketch). For shooting methods the decision variables are controls, and finding a control sequence that produces the desired states is itself an optimal-control problem — a chicken-and-egg that historically made iLQR hard to initialize. ALTRO instead makes the system artificially fully actuated with slack controls \(s_k\):

with an extra quadratic cost \(\tfrac12 \sum s_k^\mathsf{T} R_s\, s_k\) and constraints \(s_k = 0\). The given trajectory \(\tilde x\) makes the slacks start at zero; the dynamics are instantly “feasible,” and the AL machinery drives \(s_k \to 0\) in the limit, recovering a solution of the nominal system. The trajectory stays dynamically feasible (with slacks) at every iterate — anytime-usable even mid-solve.

7.2 Minimum-time (free-time) problems

Why. Many plans want “get there as fast as possible.” ALTRO encodes the time-step duration as a decision variable through its square root, \(\tau_k = \sqrt{\Delta t_k}\ge 0\), making it an input. The augmented dynamics and constraints

where \(\delta_k\) is a carried state that must equal the input square root — this equality prevents the solver from exploiting discretization error by shrinking/expanding step sizes inconsistently. The paper’s car example plans a 5 s fixed-horizon trajectory; the free-time version finds a 2.2 s bang-bang plan with the same machinery.

8.1 What the paper reports

ALTRO was benchmarked against direct collocation with SNOPT and IPOPT on a suite: a 1-D block move, pendulum, acrobot, cartpole, a Reeds–Shepp car (parallel park, obstacle avoidance, room escape), a 14-state Kuka iiwa arm, and a 13-state quadrotor maze. The headline numbers:

8.2 Lineage and relatives

ALTRO sits in a long family of constraint-handling DDP/iLQR methods: control-bound box constraints via per-step QPs (Tassa et al.), stage-wise constrained-QP backward passes (Xie & Duan; Lin; Lantoine & How’s hybrid), penalty methods (Farshidian et al.), and earlier AL-iLQR hybrids (Plancher et al.). The AL outer loop itself goes back to Hestenes and Powell’s augmented Lagrangian method of the 1960s. Immediate descendants and neighbors include ALTRO’s successor anymotion / AL-iLQR variants, the stochastic AL-iLQR line (chance constraints), and the closely-related Crocoddyl (a DDP-framework with its own constraint handling), while generic solvers like IPOPT/SNOPT remain the robust direct-method baseline.

8.3 Implementations

The reference implementation is Julia: Altro.jl (solver) on top of TrajectoryOptimization.jl (problem definitions, derivative evaluations via ForwardDiff.jl). The inner loop is designed to be allocation-free, and the paper emphasizes suitability for embedded deployment; the accompanying mini_altro.py in this folder is a from-scratch, dependency-light version of the same core.

8.4 The big picture

ALTRO is a case study in algorithm design by composition: a structure-exploiting inner solver, a general constraint layer, a numerical-conditioning repair, and a convergence accelerator, each independently simple and each earning its place. If you take one idea away beyond the mechanics, take the pattern: when a fast solver can’t handle constraints, don’t build a new solver — build a loop around it that lets a simple, provably convergent multiplier scheme do the constraining, then polish.