Trajectory optimization

Search and sampling find a path that works; trajectory optimization finds one that is good. Instead of exploring a graph, an optimizer starts from an initial trajectory, often a straight line or the output of a sampling planner, and repeatedly adjusts it to reduce a cost that rewards smoothness and penalizes closeness to obstacles, subject to constraints such as joint limits and the start and goal. The results are the smooth, natural-looking motions that people expect from robots, and the same machinery extends to dynamics, torque limits and task constraints that search methods handle poorly. The price is locality: an optimizer improves the trajectory it is given and can get stuck in a local minimum, for example on the wrong side of an obstacle. Practical systems therefore combine optimization with search, using one to find the right homotopy class and the other to refine within it.

This lesson formulates trajectory optimization, builds the standard smoothness and obstacle costs, works through gradient descent on a simple path, and then surveys the influential methods: CHOMP, STOMP, TrajOpt and model predictive approaches. ROB-300 lesson 8 covered time scaling of a given path; here the shape of the path itself is the variable.

Formulating the problem

Discretize the trajectory into N waypoints \( \xi = (q_1, \ldots, q_N) \) between a fixed start \( q_0 \) and goal \( q_{N+1} \), equally spaced in time. The optimization problem is

where \( \mathcal{F}_{\text{smooth}} \) measures how much the trajectory bends or accelerates, \( \mathcal{F}_{\text{obs}} \) measures how close it comes to obstacles, and λ weights the two. Additional constraints can enforce velocity and acceleration limits, keep a held cup upright, or require the end effector to stay on a line. With N waypoints in a d-dimensional configuration space, the decision variable has \( N d \) entries, a few hundred for a typical arm trajectory, small enough for gradient-based methods to run in tens of milliseconds.

Smoothness costs

The simplest smoothness cost is the sum of squared differences between consecutive waypoints, a discrete version of the integral of squared velocity:

Because it is quadratic, its gradient is linear, \( A\xi + b \), where A is a tridiagonal finite-difference matrix with 2 on the diagonal and −1 beside it. Minimizing it alone gives evenly spaced waypoints on a straight line between start and goal. Penalizing squared accelerations, using second differences, gives smoother curves and is preferred for arms because it reduces jerky changes of speed; its matrix is pentadiagonal. Either way the smoothness cost is convex and well understood, and the difficulty lies entirely in the obstacle term.

Obstacle costs

The obstacle term uses the signed distance field of lesson 8. Represent the robot by a set of body points or spheres; for each waypoint and each point, look up the distance d to the nearest obstacle and apply a cost that is zero when d exceeds a safety distance ε and grows as d shrinks. CHOMP uses a smooth version of this hinge:

Its gradient points along the distance field's gradient, away from the nearest obstacle, and its magnitude grows as the point approaches or penetrates the obstacle. Summing over body points and waypoints, weighted by how fast each point is moving so that slow, lingering contact costs more than a brief graze, gives \( \mathcal{F}_{\text{obs}} \). For an arm, the gradient with respect to the joint angles comes from the point's position Jacobian: \( \nabla_q c = J^{\mathsf{T}} \nabla_x c \), the same transpose-Jacobian mapping from forces to torques seen in ROB-300.

Gradient descent on a path

The simplest optimizer repeats \( \xi \gets \xi - \alpha \nabla \mathcal{U}(\xi) \) with a step size α, holding the start and goal fixed. Plain gradient descent works but converges slowly because the smoothness term is badly conditioned: moving one waypoint alone is cheap, while moving the whole trajectory together is what reduces the obstacle cost. CHOMP's key idea, covariant gradient descent, premultiplies the gradient by the inverse of the smoothness matrix, \( \xi \gets \xi - \tfrac{1}{\eta} A^{-1} \nabla \mathcal{U} \). This spreads each local correction smoothly along the whole trajectory, so a push away from an obstacle at one waypoint becomes a gentle bend over many, and convergence is dramatically faster. The code below runs this on a 2-D example.

import numpy as np
# 2-D path from (0, 0) to (10, 0) past a disc obstacle at (5, 0.3) radius 1.0; CHOMP-style covariant descent
N = 30; eps = 0.6; lam = 4.0
start, goal = np.array([0.0, 0.0]), np.array([10.0, 0.0])
xi = np.linspace(start, goal, N + 2)[1:-1].copy()        # straight line through the obstacle
xi[:, 1] += 0.05                                          # tiny nudge so the gradient picks a side
A = 2 * np.eye(N) - np.eye(N, k=1) - np.eye(N, k=-1)      # velocity finite differences
b = np.zeros((N, 2)); b[0] -= start; b[-1] -= goal
Ainv = np.linalg.inv(A)
c, R = np.array([5.0, 0.3]), 1.0
def obs(p):
    v = p - c; r = np.linalg.norm(v, axis=1, keepdims=True); d = r - R
    cost = np.where(d < 0, -d + eps / 2, np.where(d < eps, (d - eps) ** 2 / (2 * eps), 0))
    grad_d = np.where(d < 0, -1, np.where(d < eps, (d - eps) / eps, 0))
    return cost.sum(), grad_d * v / r
def clearance(p): return (np.linalg.norm(p - c, axis=1) - R).min()
def length(p):
    full = np.vstack([start, p, goal]); return np.linalg.norm(np.diff(full, axis=0), axis=1).sum()
print("iter  0: min clearance %+.2f m, length %.2f m" % (clearance(xi), length(xi)))
for it in range(1, 61):
    oc, og = obs(xi)
    grad = (A @ xi + b) + lam * og
    xi -= 0.03 * Ainv @ grad
    if it in (10, 30, 60):
        print("iter %2d: min clearance %+.2f m, length %.2f m" % (it, clearance(xi), length(xi)))

iter  0: min clearance -0.70 m, length 10.01 m
iter 10: min clearance +1.58 m, length 11.12 m
iter 30: min clearance +0.59 m, length 10.35 m
iter 60: min clearance +0.62 m, length 10.35 m

Local minima and initialization

An optimizer only follows the gradient, so its result depends on where it starts. A straight-line initialization that passes exactly through the middle of an obstacle has a symmetric gradient and may stall, or be pushed to whichever side a small numerical asymmetry favors; one that starts on the far side of a wall cannot cross it. Each distinct way of going around obstacles is a homotopy class, and gradient methods stay within the class they start in. Practical remedies include restarting from several initial trajectories and keeping the best, initializing from a sampling planner's path, which is already in a feasible class, and adding random perturbations, as in CHOMP's Hamiltonian Monte Carlo variant. In MoveIt 2, CHOMP and STOMP are often run as post-processors on an OMPL path for exactly this reason.

CHOMP, STOMP and TrajOpt

CHOMP (Covariant Hamiltonian Optimization for Motion Planning; Ratliff, Zucker, Bagnell and Srinivasa, 2009) is the method sketched above: smoothness plus SDF-based obstacle cost, minimized by covariant gradient descent. STOMP (Stochastic Trajectory Optimization for Motion Planning; Kalakrishnan, Chitta, Theodorou, Pastor and Schaal, 2011) needs no gradients: it generates noisy variations of the current trajectory, evaluates their costs, and combines them weighted by how well they did. This lets it handle costs that are not differentiable, such as torque limits computed by simulation or discontinuous constraints. TrajOpt (Schulman and colleagues, 2013 and 2014) formulates the problem as sequential convex optimization: at each iteration it linearizes collision constraints using signed distances between convex shapes (computed by GJK and EPA), solves a convex quadratic program within a trust region, and repeats. Treating collision avoidance as a constraint rather than a penalty, and checking continuous swept volumes between waypoints, makes TrajOpt reliable at producing truly collision-free results.

Constraints and dynamics

Optimizers handle constraints naturally. Joint limits are bounds on the variables. Velocity and acceleration limits become constraints on finite differences of waypoints once the time step is fixed, or the time step itself can be a variable for time-optimal problems. Task constraints, such as keeping an end effector's orientation within 5° of vertical, become nonlinear equality or inequality constraints on forward kinematics. Including the full dynamics, \( M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \) from ROB-300, with torque limits gives direct transcription and collocation methods, the basis of trajectory planning for legged robots and fast arms. Model predictive control applies the same optimization on a short, receding horizon at control rates, which lesson 11 uses for dynamic environments.