Michael Brenndoerfer | Data & AI, Private Equity, Technology

Michael Brenndoerfer

Classical Planning and Optimal Control › Trajectory Optimization and Constraints

Michael BrenndoerferJune 10, 20267 min read

World Models HandbookAll chapters

Part 2 of 12Inference, Decisions, and Control

Chapter 10Classical Planning and Optimal Control › Trajectory Optimization and Constraints

Reading section 6 of 10 · Trajectory Optimization and Constraints · 7 minSection 6 of 10 · 7 min

Section 6 of 10 · 7 min read

Classical Planning and Optimal Control › Trajectory Optimization and ConstraintsOne section · Sections in this chapter6 / 10 ↓

Sections in this chapter10

Reading tools Definitions & inline links

Choose your expertise level to adjust how many terms are explained. Beginners see more tooltips, experts see fewer to maintain reading flow. Hover over underlined terms for instant definitions.

Article links

Make inline references clickable

LQR gives optimality by exploiting linear-quadratic structure. With nonlinear dynamics, non-quadratic costs, or constraints, trajectory optimization states a finite-horizon problem directly and solves it numerically. These methods still exploit substantial structure: dynamics couple only neighboring time steps, derivatives are sparse, and specialized shooting or collocation solvers use that organization. The result is a flexible framework used throughout robotics and aerospace, with guarantees that depend on the solver, formulation, initialization, and model.

The formulation is direct. Treat the entire future as decision variables:

\min_{x_1,\dots,x_T,\,u_0,\dots,u_{T-1}} \; \sum_{t=0}^{T-1} \ell(x_t, u_t) + \ell_f(x_T)

subject to the dynamics acting as equality constraints,

x_{t+1} = f(x_t, u_t), \quad t = 0, \dots, T-1,

and the initial condition

x_0=x_{\mathrm{init}},

plus whatever task constraints apply, for example $u_{\min} \le u_t \le u_{\max}$ from actuator limits or obstacle-avoidance inequalities on the states. The symbols in these two expressions are:

$\ell(x_t, u_t)$ : the running cost incurred at step $t$ , encoding objectives such as tracking error and control effort
$\ell_f(x_T)$ : the terminal cost on the final state $x_T$
$f(x_t, u_t)$ : the model's dynamics, predicting the next state from the current state and control
$x_1, u_0, \dots, x_T, u_{T-1}$ : the optimized state-and-control trajectory; $x_0$ is fixed by the initial condition

Search and DP asked for a path or value function; LQR produced a feedback law. Open-loop trajectory optimization instead returns one nominal state-control trajectory. It can be used offline with a tracking controller, or online inside receding-horizon MPC when computation permits. Both pairings add feedback that the nominal trajectory alone does not provide.

For the nonlinear problems emphasized here, two widely used direct formulations are shooting and collocation.

Direct shooting keeps only the controls $u_0, \dots, u_{T-1}$ as decision variables and simulates the model to produce the states. The dynamics constraints are satisfied by construction, and gradients are obtained by differentiating through the rollout. The downside: the map from early controls to late states becomes long, thin, and poorly conditioned, so gradients through long rollouts can vanish or explode. Intuitively, an early control affects a late state only through a long chain of intermediate states, and the Jacobian of that chain is a product of many Jacobians whose singular values multiply, so the gradient magnitude can shrink or grow exponentially in the horizon length.
Direct collocation keeps states and controls as joint decision variables and enforces discretized dynamics as explicit constraints at collocation points. This exposes sparse structure and often improves conditioning, at the cost of a larger optimization problem. An NLP solver drives the discrete constraint residuals toward its numerical tolerances; whether the resulting continuous trajectory is feasible also depends on the transcription resolution and model fidelity.

Hard versus soft constraints

A hard constraint belongs to the feasible set, whereas a soft constraint adds a penalty to the objective. A constrained solver enforces the discretized hard constraint only to numerical tolerance, and real-system satisfaction additionally depends on discretization and model error. A penalty merely discourages violation. This distinction matters in safety-critical settings, where robust margins and independent safety mechanisms may still be needed. We return to it in Part XII.

To make this concrete, let's solve a nonlinear task by direct shooting with automatic differentiation and Adam: swinging up a bounded-input pendulum. We use normalized angular dynamics

\ddot\theta = -\omega_g^2\sin\theta - c\,\dot\theta + u,

where:

$\theta$ : the pendulum angle, with $\theta = 0$ hanging down and $\theta = \pi$ the upright target
$\dot\theta$ , $\ddot\theta$ : the angular velocity and the angular acceleration
$\omega_g^2$ : the normalized gravity coefficient, with units of inverse seconds squared; $\omega_g^2\sin\theta$ has angular-acceleration units
$c$ : the damping rate
$u$ : the commanded angular acceleration after normalizing motor torque by inertia

In the simulation below the normalized gravity coefficient is $3.0$ while the input bound is $2.0$ . A constant saturated input therefore cannot hold the pendulum at horizontal, and the optimizer must exploit momentum to reach the upright. We optimize $u_0,\dots,u_{T-1}$ by reverse-mode differentiation through the discretized rollout, with the input bound represented by clamping.

In[25]:

Code

import torch

torch.manual_seed(0)
gravity_coeff, c_damp, dt_p, T_p = 3.0, 0.25, 0.05, 120
u_max = 2.0  # below the normalized gravity term at horizontal


def rollout(u_seq):
    theta = torch.zeros(())
    omega = torch.zeros(())
    thetas, omegas = [theta], [omega]
    for t in range(T_p):
        tau = torch.clamp(u_seq[t], -u_max, u_max)
        alpha = -gravity_coeff * torch.sin(theta) - c_damp * omega + tau
        omega = omega + dt_p * alpha
        theta = theta + dt_p * omega
        thetas.append(theta)
        omegas.append(omega)
    return torch.stack(thetas), torch.stack(omegas)


u = torch.zeros(T_p, requires_grad=True)
opt = torch.optim.Adam([u], lr=0.05)
losses = []
for it in range(600):
    opt.zero_grad()
    th, om = rollout(u)
    loss = (
        ((th[:-1] - torch.pi) ** 2 + 0.05 * om[:-1] ** 2).mean()
        + 10.0 * (th[-1] - torch.pi) ** 2
        + 1.0 * om[-1] ** 2
        + 0.001 * (u**2).mean()
    )
    loss.backward()
    opt.step()
    losses.append(loss.item())

The cost penalizes angle and velocity along the trajectory, applies extra terminal penalties at $x_T$ , and mildly discourages large inputs. The running term provides gradient information at every simulated time step; a terminal-only objective would still have a gradient here, but it can be harder to optimize because all influence must pass through the full rollout. For this discretized, piecewise-differentiable PyTorch program, loss.backward() computes reverse-mode derivatives of the implemented objective with respect to all 120 controls. At clamp boundaries, the returned derivative follows PyTorch's chosen subgradient convention.

In[26]:

Code

with torch.no_grad():
    th_final, om_final = rollout(u)
th_np = th_final.numpy()
u_np = u.detach().numpy()

Out[27]:

Console

Final angle:           +3.1071 rad   (target: pi = 3.1416)
Final angular velocity: +0.16096 rad/s
Initial loss: 108.57   final loss: 6.5452
Controls at torque limit: 65% of the time

The optimized trajectory ends near the upright, with a small residual angular velocity reported in the printed output above. The angle makes a backswing rather than rising monotonically because the bounded input cannot hold the pendulum at horizontal. That momentum-building maneuver is outside what an LQR linearized around the upright can discover from the downward initial state. The example shows how differentiating through a nonlinear model can expose a useful open-loop strategy without encoding the maneuver by hand.

Out[28]:

Visualization

Line chart of pendulum angle making a backswing and then approaching pi at the end of the horizon. — Optimized pendulum swing-up trajectory. After a backswing, the angle approaches the upright target of pi radians with a small residual angular velocity.

Log-scale line chart of loss decreasing steeply then leveling off over 600 iterations. — Trajectory optimization loss versus Adam iteration on a log scale. The objective drops rapidly and then improves more gradually.

The loss drops rapidly and then improves more gradually. The curve shows optimization progress, but it does not identify which physical strategy Adam discovered at each iteration; that would require inspecting intermediate trajectories. The later, slower phase is consistent with local refinement of a nonlinear shooting solution, not proof of a universal signature for shooting methods.

This example compresses the chapter's arc into one picture. We had a model, used it predictively to simulate rollouts, and obtained an open-loop control sequence by optimizing the model's outputs. No explicit value table or search tree was needed. The pendulum has one mechanical degree of freedom and a two-dimensional state $(\theta,\dot\theta)$ . A comparable transcription gains variables as state and control dimension increase; conditioning, constraint difficulty, and local optima depend on the model, coordinates, objective, and constraint structure. When a nonconvex problem has separate basins, a local optimizer may settle on one gait even if a lower-cost gait lies in another. Better initialization, multiple starts, sampling-based exploration, and learned trajectory priors are common responses.