iLQR and Trajectory Optimization

LQR solved one problem exactly: linear dynamics, quadratic cost, a closed-form feedback gain. Real robots are neither linear nor cheap to write in closed form. Trajectory optimization keeps the objective and drops the restriction: minimize a cost over an entire control sequence subject to the true nonlinear dynamics. iLQR and its second-order sibling DDP are the model-based workhorse, and they get there by solving, over and over, the one problem you already know how to solve.

The problem

Fix a horizon $T$ and search over the controls $u_0,\dots,u_{T-1}$:

$$\min_{u_0,\dots,u_{T-1}}\ J = \phi(x_T) + \sum_{t=0}^{T-1}\ell(x_t,u_t) \quad\text{s.t.}\quad x_{t+1}=f(x_t,u_t),\ \ x_0\ \text{given}.$$

The dynamics are equality constraints, so the states $x_1,\dots,x_T$ are not free: once the controls are chosen, a rollout of $f$ fixes them. The decision variable is the control sequence alone.

Take a pendulum. State $x=(\theta,\dot\theta)$ with $\theta$ measured from the downward vertical, so $\theta=0$ hangs straight down and $\theta=\pi$ balances upright. The continuous dynamics are $\ddot\theta=-\tfrac{g}{L}\sin\theta+\tfrac{u}{mL^2}$ with the torque bounded, $|u|\le\bar u$. A semi-implicit Euler step gives the discrete map $f$:

$$\dot\theta_{t+1}=\dot\theta_t+h\!\left(-\tfrac{g}{L}\sin\theta_t+\tfrac{u_t}{mL^2}\right), \qquad \theta_{t+1}=\theta_t+h\,\dot\theta_{t+1}.$$

A cost that asks for the tip upright at the end with little effort spent:

$$J=\sum_{t=0}^{T-1} w_u\,u_t^2 \;+\; w_T\,(1+\cos\theta_T),$$

whose terminal term is $0$ exactly when $\theta_T=\pi$ and $2$ at the bottom. When $\bar u < mgL$ the torque cannot hold the arm horizontal, let alone lift it in one push, so any optimal solution has to pump: swing back to build energy, then up.

The iLQR loop

iLQR never touches the nonlinear problem directly. Given the current controls $\bar u$ and their rollout $\bar x$, it repeats three steps.

1. Local model. Linearize the dynamics and quadratize the cost about $(\bar x_t,\bar u_t)$. Writing $\delta x_t=x_t-\bar x_t$, $\delta u_t=u_t-\bar u_t$,

$$\delta x_{t+1}=A_t\,\delta x_t+B_t\,\delta u_t,\qquad A_t=\frac{\partial f}{\partial x}\Big|_t,\quad B_t=\frac{\partial f}{\partial u}\Big|_t,$$

with $\ell$ and $\phi$ replaced by their second-order Taylor models. That is precisely a time-varying LQR problem in the deviations $(\delta x,\delta u)$.

2. Backward pass. Run the LQR / Riccati recursion of the previous chapter on this local model. It returns an affine update

$$\delta u_t = k_t + K_t\,\delta x_t,$$

a feedforward correction $k_t$ plus a feedback gain $K_t$. This is a Newton-like step on the whole trajectory, not a coordinate-wise nudge.

3. Forward pass. Apply the update to the true dynamics with a line-search scale $\alpha\in(0,1]$,

$$u_t \leftarrow \bar u_t + \alpha\,k_t + K_t\,(x_t-\bar x_t),$$

roll out the nonlinear $f$, and accept if $J$ dropped, shrinking $\alpha$ otherwise. Iterate to a local optimum. DDP is the same loop but keeps the second-order dynamics terms in the backward pass; iLQR drops them (a Gauss-Newton step), which is cheaper and, for many problems, more robust.¹ Hard limits $|u|\le\bar u$ enter as a small box-constrained QP solved inside the backward pass.²

Reading the gradient

Before the full second-order machinery, the first-order picture makes the mechanism concrete. A rollout is a composition of $T$ maps, so $J$ is an ordinary function of $u$, and $\nabla_u J$ comes from one reverse sweep, the discrete adjoint (back-propagation through time):

$$\lambda_T=\frac{\partial\phi}{\partial x_T},\qquad \lambda_t=\frac{\partial\ell}{\partial x_t}+A_t^{\top}\lambda_{t+1},\qquad \frac{\partial J}{\partial u_t}=\frac{\partial\ell}{\partial u_t}+B_t^{\top}\lambda_{t+1}.$$

A two-step toy. Set $h=0.2$, $g/L=5$, $mL^2=1$, $w_u=0.01$, $w_T=1$, start at $\theta_0=1.0,\ \dot\theta_0=0$, and guess $u_0=u_1=0$. The forward rollout is

$$\theta:\ 1.000\to 0.832\to 0.516,\qquad \dot\theta:\ 0\to -0.841\to -1.581,$$

so $J=1+\cos(0.516)=1.870$. With $a'_t=-\tfrac{g}{L}\cos\theta_t$ the Jacobians are

$$A_t=\begin{bmatrix}1+h^2 a'_t & h\\[2pt] h\,a'_t & 1\end{bmatrix},\qquad B=\begin{bmatrix}h^2\\[2pt] h\end{bmatrix}.$$

Seed the adjoint with $\lambda_2=\big(-w_T\sin\theta_2,\,0\big)=(-0.493,\,0)$ and sweep backward. From the last step, $\partial J/\partial u_1=h^2\lambda_{2,\theta}=-0.0197$ and $\lambda_1=A_1^{\top}\lambda_2=(-0.427,\,-0.099)$; from the first,

$$\frac{\partial J}{\partial u_0}=h^2\lambda_{1,\theta}+h\,\lambda_{1,\omega}=-0.0368.$$

Both components are negative, so $-\nabla_u J$ raises both torques and drives $\theta_T$ toward $\pi$: the swing-up direction. And $\lvert\partial J/\partial u_0\rvert\approx 1.9\,\lvert\partial J/\partial u_1\rvert$, because the earlier control propagates through more steps and so has more leverage on the terminal state. One gradient step $u\leftarrow u-\alpha\nabla_u J$ with $\alpha=10$ gives $u=(0.368,\,0.197)$, a new $\theta_T=0.551$, and $J=1.854<1.870$.

iLQR replaces that fixed-$\alpha$ nudge with the LQR step $k_t$, which folds in the cost and dynamics curvature and converges in a handful of iterations rather than thousands, and it returns the gain $K_t$, so the output is a controller, not just an open-loop plan. One detail the toy exposes: at the exact bottom $\theta=0$, $\sin\theta=0$, so a zero-torque guess has zero gradient. A swing-up needs a non-trivial initial guess, which is why the demo seeds small random controls.

Swing-up by optimizing the controls

The demo runs the first-order version end to end: a torque-limited pendulum starting at rest at the bottom, $T=70$ steps, with $\nabla_u J$ computed by exact back-propagation through the two-state dynamics and plain gradient descent on the $70$ torques. Watch the replayed trajectory morph from a useless wiggle into a pump-and-swing as the cost falls.

swing-up by gradient descent on the controlsinteractive

trajectory · x = [θ, θ̇]

torque u = 0.29 / ±5

cost J vs. iteration

iteration 0 / 2500

OptimizeReset

final tip: 179.9° from upright

Optimize seeds a small random control sequence, then runs 2500 gradient steps u ← u − α ∇_uJ with ∇_uJ from back-propagation through time. The pendulum replays the current controls; the plot is cost per iteration.

Why it is the workhorse

iLQR is model-based: it needs $f$ and the Jacobians $A_t,B_t$. Minimal-coordinate engines supply exactly these in closed form, which is what makes differentiable simulation drop straight into the loop. One solve yields an open-loop plan together with local feedback gains; re-solve a short-horizon version every control step and the same machinery becomes model predictive control, the method underneath most modern legged and manipulation controllers. LQR gave the local model; iLQR iterates it into a plan for a nonlinear world.

Footnotes

1. Todorov & Li, A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems (ACC 2005). ↩

2. Tassa, Mansard & Todorov, Control-Limited Differential Dynamic Programming (ICRA 2014). ↩