From a path to a motion
The sampling-based planners return collision-free paths, but nobody would want to execute them as they come: they zig-zag between random samples, pass needlessly close to obstacles, and say nothing about speed. Shortcutting helped, but only a little. What a robot needs is a trajectory: positions as a function of time, smooth enough for its motors, fast enough to be useful, respecting joint limits, velocity limits, torque limits and dynamics, and clear of every obstacle by a margin.
All of these wishes can be written as numbers. Smoothness is a sum of squared accelerations; clearance is a penalty that grows as the robot approaches an obstacle; speed is the total time; limits are inequalities; dynamics are equations linking each instant to the next. Planning then becomes optimisation: choose the trajectory that minimises the cost while satisfying the constraints. This is the same idea as model predictive control, which re-solves a short trajectory optimisation many times a second; here we look at the longer, harder problems of planning a whole motion around obstacles.
Optimisation has a price. These problems are almost never convex, so solvers find local optima: good trajectories near where they started, not necessarily the best one, and sometimes not a collision-free one at all. This lesson builds three optimisers small enough to read in full, shows where each succeeds, and shows one of them failing because of its starting point, which is the central practical fact of the field.
A path as a flexible object
Analogy, not a formal construction
Lay a stretched rubber band on a table between two pins, and put a few coins on the table in its way. Each coin, if it could, would push the band aside; the band's tension pulls it back towards a straight line. The band settles where the pushes and the pull balance: short, smooth, and clear of the coins. Start it on the other side of a coin, though, and it settles on that side, even if going round the other way would be shorter. Trajectory optimisation behaves much like the band.
Discretise a trajectory as waypoints between a fixed start and goal (for an arm, each is a vector of joint angles; for our drawings, a point in the plane). Two forces act on it. A smoothness cost
is small when neighbouring waypoints are close and evenly spaced, like the band's tension. An obstacle cost uses the signed distance field of the previous lesson: is zero far from obstacles and grows as the distance falls below a margin , and keeps growing inside them. Gradient descent on moves each waypoint downhill, and since the SDF's gradient points away from obstacles, waypoints inside an obstacle are pushed out.
One subtlety makes all the difference. Plain gradient descent moves each waypoint independently: a waypoint inside an obstacle jumps outwards while its neighbours stay put, creating a kink, which the smoothness term then slowly irons out over many iterations. CHOMP (Nathan Ratliff, Matt Zucker, J. Andrew Bagnell and Siddhartha Srinivasa, 2009) measures the size of an update in the smoothness metric instead. Writing , where is the finite-difference matrix of the path, it takes the step
Multiplying by spreads a push at one waypoint smoothly over the whole path, the way pressing on the middle of a rubber band moves all of it.
Definitions
Definition— trajectory optimisation problem
Given dynamics with state and control , a trajectory optimisation problem is
where is the running cost (effort, smoothness, time), a final cost or goal, and collects inequality constraints: joint and torque limits, and collision avoidance written as . A computer solves a discretised version with finitely many variables.
Definition— single shooting
Single shooting makes the controls the only decision variables; the states are obtained by simulating the dynamics forward from . Multiple shooting also makes the state at the start of each of several segments a variable, simulates each segment separately, and adds constraints that each segment ends where the next begins.
Definition— direct collocation
Direct collocation makes the states and controls at all knot times decision variables and imposes the dynamics as algebraic equality constraints between neighbouring knots, the defects, for example by the trapezoidal rule
The dynamics hold only when the solver has driven every defect to zero; during the solve, the trajectory may be dynamically impossible.
Definition— time parameterisation
A time parameterisation of a geometric path , , is a function saying where along the path the robot is at each time. Time-optimal path parameterisation (TOPP) finds the one that traverses the path fastest while respecting velocity, acceleration or torque limits.
A CHOMP-style optimiser
The program optimises a 40-waypoint path across a 3 m × 2 m workspace with a large round obstacle in the way and a smaller one just below it. The signed distance field is sampled on a 2 cm grid, interpolated bilinearly, and differentiated with np.gradient. The obstacle cost uses CHOMP's shape: in the margin (here cm) and inside obstacles, so its slope is continuous. It compares plain and covariant gradient descent, each with about the largest step that stays stable for it, and then starts the covariant method from two different initial guesses.
import numpy as np
# Workspace 3 m x 2 m, two round obstacles; a signed distance field sampled on a 2 cm grid.
CIRCLES = [((1.5, 1.0), 0.35), ((1.55, 0.35), 0.2)]
RES = 0.02
gy, gx = (np.indices((101, 151)) * RES)
D = np.min([np.hypot(gx - cx, gy - cy) - r for (cx, cy), r in CIRCLES], axis=0)
DY, DX = np.gradient(D, RES) # the field's gradient, also on the grid
def sample(F, p):
"""Bilinear interpolation of a grid field F at points p (n x 2, metres)."""
x, y = p[:, 0] / RES, p[:, 1] / RES
j, i = np.clip(x.astype(int), 0, 149), np.clip(y.astype(int), 0, 99)
fx, fy = x - j, y - i
return ((1 - fy) * ((1 - fx) * F[i, j] + fx * F[i, j + 1]) +
fy * ((1 - fx) * F[i + 1, j] + fx * F[i + 1, j + 1]))
EPS = 0.15 # start paying 15 cm from obstacles
def obstacle_cost_grad(P):
d = sample(D, P)
g = np.stack([sample(DX, P), sample(DY, P)], axis=1)
cost = np.where(d < 0, -d + EPS / 2, np.where(d < EPS, (EPS - d) ** 2 / (2 * EPS), 0.0))
dc_dd = np.where(d < 0, -1.0, np.where(d < EPS, (d - EPS) / EPS, 0.0))
return cost.sum(), dc_dd[:, None] * g # chain rule: dc/dp = dc/dd * grad d
N = 40 # interior waypoints; the ends are fixed
start, goal = np.array([0.2, 1.0]), np.array([2.8, 1.0])
K = np.eye(N) - np.eye(N, k=-1) # finite differences between neighbours
A = K.T @ K; A[-1, -1] += 1 # smoothness metric: sum of squared steps
b = np.zeros((N, 2)); b[0] = -start; b[-1] = -goal
def total_cost(P, lam=10.0):
obst, _ = obstacle_cost_grad(P)
full = np.vstack([start, P, goal])
return obst + lam * 0.5 * np.sum(np.diff(full, axis=0) ** 2)
def optimise(P, iters, covariant, lam=10.0):
"""Gradient descent; the covariant (CHOMP) version preconditions by the inverse of A.
Each uses roughly the largest step size that is stable for it."""
Ainv, eta = np.linalg.inv(A), (400.0 if covariant else 40.0)
for _ in range(iters):
_, g_obs = obstacle_cost_grad(P)
grad = g_obs + lam * (A @ P + b) # gradient of obstacle + smoothness terms
step = Ainv @ grad if covariant else grad
P = P - step / eta
return P
def report(name, P):
full = np.vstack([start, P, goal])
length = np.sum(np.linalg.norm(np.diff(full, axis=0), axis=1))
bend = np.max(np.linalg.norm(np.diff(full, 2, axis=0), axis=1))
print(f"{name:38} clearance {sample(D, P).min():+.3f} m length {length:.3f} m "
f"max bend {bend:.3f} m cost {total_cost(P):.3f}")
t = np.linspace(0, 1, N + 2)[1:-1, None]
straight = start + t * (goal - start)
report("straight line (initial guess)", straight)
for iters in (10, 100):
report(f"plain gradient, {iters} steps", optimise(straight + [0, 0.01], iters, False))
report(f"covariant (CHOMP), {iters} steps", optimise(straight + [0, 0.01], iters, True))
# Local minima: start from a guess bent above the big obstacle, or below it.
bump = np.sin(np.pi * t) * [0, 0.25]
report("covariant from a guess bent upwards", optimise(straight + bump, 100, True))
report("covariant from a guess bent downwards", optimise(straight - bump, 100, True))The straight line starts 31.8 cm deep in the obstacle. After 10 iterations, plain gradient descent is still 21.2 cm inside, with a visible kink (a "bend" of 7.3 cm between consecutive steps); the covariant method is already clear by 12.6 cm with a smooth path, and after 100 iterations it has converged (cost 0.949) while plain descent is still at 1.079. Both start from the straight line nudged upwards by 1 cm, which decides which way round the obstacle they go. That is the warning in the last two lines: started above the obstacle, the covariant method finds the cost-0.949 path; started below, it settles into the 10 cm gap between the two obstacles, at cost 1.132 and only 1.8 cm from their surfaces. Both are local minima. Gradient descent cannot know that the other side is better, because it only looks downhill from where it stands.
Shooting and collocation
CHOMP optimises geometry only. When dynamics matter, as for a fast arm, a drone or a walking robot, the optimiser must respect , and there are two families of ways to do it.
Single shooting has few variables and always produces a trajectory that obeys the dynamics, but the end state depends on early controls through the whole simulation, and for unstable systems that dependence explodes. Collocation has many more variables but every constraint involves only two neighbouring knots, so its matrices are sparse and well conditioned, and the solver can start from a guess that is not dynamically consistent at all, such as a straight line in state space. Large nonlinear solvers handle the sparsity easily, which is why collocation (and its close relative, multiple shooting) dominates offline trajectory optimisation for robots.
The program solves the classic test: move a double integrator (, a mass pushed by a force) from rest at 0 to rest at 1 m in 2 s with the least effort . With the trapezoidal rule, the cost is quadratic and the defects are linear, so the problem is an equality-constrained quadratic programme, solved exactly by one linear system (the KKT equations). The exact answer is known: , a force that falls linearly from push to brake. The program compares, then shows why shooting struggles with an unstable system.
import numpy as np
# Move a 1-D double integrator (x'' = u) from rest at x = 0 to rest at x = 1 m in T = 2 s,
# minimising the effort integral of u^2, by trapezoidal direct collocation.
T = 2.0
def collocate(n):
"""Decision variables z = [x_0..x_n, v_0..v_n, u_0..u_n]; returns the knot values."""
h, m = T / n, n + 1
X, V, U = 0, m, 2 * m # where each block starts in z
rows, rhs = [], []
def row(entries, value=0.0):
r = np.zeros(3 * m)
for index, coeff in entries:
r[index] += coeff
rows.append(r); rhs.append(value)
for k in range(n): # dynamics as equality constraints
row([(X + k + 1, 1), (X + k, -1), (V + k, -h / 2), (V + k + 1, -h / 2)])
row([(V + k + 1, 1), (V + k, -1), (U + k, -h / 2), (U + k + 1, -h / 2)])
row([(X, 1)], 0.0); row([(V, 1)], 0.0) # start at rest at 0
row([(X + n, 1)], 1.0); row([(V + n, 1)], 0.0) # end at rest at 1
C, d = np.array(rows), np.array(rhs)
w = np.full(m, h); w[0] = w[-1] = h / 2 # trapezoid weights for the cost
H = np.zeros((3 * m, 3 * m)); H[U:, U:] = 2 * np.diag(w)
H += 1e-12 * np.eye(3 * m) # x and v do not appear in the cost
KKT = np.block([[H, C.T], [C, np.zeros((len(d), len(d)))]])
sol = np.linalg.solve(KKT, np.concatenate([np.zeros(3 * m), d]))
z = sol[:3 * m]
return z[X:X + m], z[V:V + m], z[U:U + m], float(w @ z[U:U + m] ** 2)
def exact(t): # the minimum-effort solution
return 3 * t**2 / T**2 - 2 * t**3 / T**3, 6 * t / T**2 - 6 * t**2 / T**3, 6 / T**2 - 12 * t / T**3
print(f"exact: effort {12 / T**3:.4f}, peak speed {exact(T / 2)[1]:.4f} m/s, u(0) = {exact(0)[2]:.4f} m/s^2")
print(f"{'knots':>6}{'effort':>10}{'max |x error|':>15}{'max |u error|':>15}")
for n in (4, 8, 16, 32):
x, v, u, J = collocate(n)
t = np.linspace(0, T, n + 1)
xe, ve, ue = exact(t)
print(f"{n + 1:6d}{J:10.4f}{np.abs(x - xe).max():15.2e}{np.abs(u - ue).max():15.2e}")
# Why not single shooting? For an unstable system the end state is hypersensitive to early inputs.
lam, n = 3.0, 30 # x' = lam * x + u, 3 s horizon
h = 3.0 / n
sens = [h * (1 + lam * h) ** (n - 1 - k) for k in range(n)] # d x_n / d u_k, explicit Euler
print(f"\nunstable system x' = 3x + u over 3 s: d x(T) / d u_k ranges from {min(sens):.2f} to {max(sens):.0f}")
print("in collocation every constraint links only neighbouring knots, with coefficients near 1 and h")The effort converges to the exact 1.5 as knots are added (1.882 with 5 knots, 1.506 with 33), and the position error falls by a factor approaching 4 with each doubling of the knots, as a second-order method's should. The control's error is largest at the two end knots, where the trapezoid rule is least accurate; higher-order schemes such as Hermite–Simpson collocation reduce it. The last lines show the shooting problem: for over 3 s, nudging the first control moves the final state about 2000 times more than nudging the last one. A solver adjusting those controls faces a badly scaled problem, and for longer horizons or nonlinear dynamics, a simulation that diverges. Collocation never simulates over the whole horizon, so this cannot happen.
Time-optimal parameterisation
A common division of labour in industry is: plan a geometric path (by sampling, optimisation or a teach pendant), then decide how fast to move along it. With the path fixed, the only freedom is the speed along it, and the limits become limits on and . For a vehicle on a planar path with curvature , staying within a sideways acceleration requires , a speed limit in every bend; a tangential acceleration limit bounds how fast can change with distance, since .
The time-optimal profile has a simple structure, found by James Bobrow, Steven Dubowsky and John Gibson, and independently by Kang Shin and Neil McKay, in 1985: at every point the robot is either accelerating as hard as it can, braking as hard as it can, or riding a speed limit. For limits of this simple kind a forward pass (accelerate as hard as allowed, never exceeding the limit) followed by a backward pass (brake in time for every slow section ahead) computes it exactly. The program applies this to a 6.7 m path with a tight bend and a wide one.
import numpy as np
# A path for a mobile robot: 2 m straight, a 90-degree bend of radius 0.4 m, 1.5 m straight,
# a 90-degree bend of radius 1.0 m the other way, then 1 m straight. Curvature along the path:
ds = 0.005
pieces = [(2.0, 0.0), (np.pi / 2 * 0.4, 1 / 0.4), (1.5, 0.0), (np.pi / 2 * 1.0, 1 / 1.0), (1.0, 0.0)]
kappa = np.concatenate([np.full(int(round(L / ds)), k) for L, k in pieces])
s = np.arange(len(kappa) + 1) * ds
kappa = np.append(kappa, 0.0) # curvature at each of the len(s) points
V_MAX, A_TAN, A_LAT = 1.5, 0.8, 1.0 # m/s, m/s^2 along the path, m/s^2 sideways
v_lim = np.minimum(V_MAX, np.sqrt(A_LAT / np.maximum(kappa, 1e-9)))
v = v_lim.copy()
v[0] = v[-1] = 0.0 # start and stop at rest
for i in range(len(s) - 1): # forward pass: accelerate as hard as allowed
v[i + 1] = min(v[i + 1], np.sqrt(v[i] ** 2 + 2 * A_TAN * ds))
for i in range(len(s) - 1, 0, -1): # backward pass: brake in time for what comes
v[i - 1] = min(v[i - 1], np.sqrt(v[i] ** 2 + 2 * A_TAN * ds))
def travel_time(v):
return float(np.sum(2 * ds / (v[1:] + v[:-1])))
print(f"path length {s[-1]:.2f} m; speed limits: {V_MAX} m/s, tight bend {np.sqrt(A_LAT * 0.4):.3f} m/s, "
f"wide bend {np.sqrt(A_LAT * 1.0):.3f} m/s")
print(f"time-optimal profile: {travel_time(v):.2f} s, top speed {v.max():.2f} m/s at s = {s[v.argmax()]:.2f} m")
# A simpler schedule for comparison: one speed, with the same limits for starting and stopping.
def flat(limit):
w = np.full_like(s, limit); w[0] = w[-1] = 0.0
for i in range(len(s) - 1):
w[i + 1] = min(w[i + 1], np.sqrt(w[i] ** 2 + 2 * A_TAN * ds))
for i in range(len(s) - 1, 0, -1):
w[i - 1] = min(w[i - 1], np.sqrt(w[i] ** 2 + 2 * A_TAN * ds))
return w
print(f"one speed everywhere, safe for the tight bend: {travel_time(flat(np.sqrt(A_LAT * 0.4))):.2f} s")
print("\nspeed along the path (each row 0.25 m; '|' marks the limit, '*' the speed):")
for i in range(0, len(s), 50):
bar = [" "] * 32
bar[min(int(v_lim[i] / V_MAX * 30), 31)] = "|"
bar[int(v[i] / V_MAX * 30)] = "*"
print(f"s = {s[i]:4.2f} m v = {v[i]:4.2f} " + "".join(bar))The robot accelerates along the first straight and must start braking for the tight bend before reaching its 1.5 m/s limit; it takes the bend at 0.632 m/s, accelerates out of it, and reaches its top speed of 1.38 m/s on the middle straight. The whole path takes 8.13 s, against 11.38 s at a single speed safe for the tightest bend. The profile shows the three regimes, with each speed either on its limit or on a maximum acceleration or braking curve between limits.
For arms the same idea applies with joint-space limits: every joint's velocity, acceleration and torque limit turns into bounds on and that depend on . TOPP-RA (Hung Pham and Quang-Cuong Pham, 2018) solves the general version robustly with a sequence of small linear programmes, and MoveIt applies time-optimal parameterisation to every planned path by default. Profiles that are time-optimal under acceleration limits have jumps in acceleration, which excite vibrations; production systems usually also limit jerk (the rate of change of acceleration), for example with the open-source Ruckig library.
Trajectory optimisation in practice
TrajOpt (John Schulman, Pieter Abbeel and colleagues, 2013–14) made optimisation a serious alternative to sampling for arms. It solves a sequence of convex subproblems (sequential convex programming), each with a trust region, and writes collision avoidance as constraints on signed distances computed by GJK and EPA between convex shapes, including the convex hull of a link's positions at consecutive waypoints, so that the motion between waypoints is checked as well. STOMP (Mrinal Kalakrishnan and colleagues, 2011) instead perturbs the trajectory with smooth random noise and keeps the improvements, which needs no gradients. CHOMP and STOMP are both available in MoveIt.
Solvers and toolkits. Most robotics groups do not write solvers; they formulate problems in a modelling tool and pass them to a general solver. Widely used, as of 2026: CasADi (KU Leuven) for modelling with automatic differentiation; IPOPT (an open-source interior-point solver from the COIN-OR project) and SNOPT (a commercial sequential quadratic programming solver) for large sparse nonlinear problems; OSQP for quadratic programmes; acados (University of Freiburg and partners) for fast embedded MPC; Crocoddyl (LAAS-CNRS) for differential dynamic programming on legged robots; and Drake (MIT and Toyota Research Institute), which wraps many solvers behind one interface. The code below shows the shape of a collocation problem in CasADi; it is not run here.
# CasADi (not run here): min-effort double integrator by trapezoidal collocation.
import casadi as ca
N, T = 32, 2.0
opti = ca.Opti()
x, v, u = opti.variable(N + 1), opti.variable(N + 1), opti.variable(N + 1)
h = T / N
for k in range(N):
opti.subject_to(x[k + 1] == x[k] + h / 2 * (v[k] + v[k + 1]))
opti.subject_to(v[k + 1] == v[k] + h / 2 * (u[k] + u[k + 1]))
opti.subject_to([x[0] == 0, v[0] == 0, x[N] == 1, v[N] == 0])
opti.subject_to(opti.bounded(-2.0, u, 2.0)) # an inequality: force limits
opti.minimize(h * ca.sumsqr(u))
opti.solver("ipopt")
solution = opti.solve()Parallelism and learning. GPU tools such as NVIDIA's cuRobo run many trajectory optimisations in parallel from different initial guesses and keep the best, which attacks local minima by brute force. Learned models trained on solved problems can propose initial trajectories (including, since about 2022, diffusion models that generate whole trajectories), which an optimiser then refines and checks.
Global structure. Graphs of convex sets (Tobia Marcucci, Russ Tedrake and colleagues, published in Science Robotics in 2023) take another route: decompose the free space into convex regions offline, then solve one convex optimisation that chooses which regions to pass through and the smooth trajectory within them at the same time. Its convex relaxation is often tight, so it finds near-globally-optimal trajectories without depending on an initial guess, at the cost of computing the decomposition.
Local minima, and why the initial guess matters
The rubber band and the CHOMP experiment showed the basic fact: an optimiser follows the local slope, so it converges to a minimum near where it starts. In planning, different minima usually correspond to different ways around obstacles (paths in different homotopy classes, which cannot be deformed into each other without crossing an obstacle), and no amount of local improvement turns one into another. Worse, a local minimum can still be in collision, if the obstacle cost's push is balanced by the smoothness pull, for example on a path through a thin wall whose SDF gradient points in opposite directions on its two sides.
Practitioners use several defences, often together:
- Good seeds. Run a sampling-based planner first and optimise its path: the sampler finds the right homotopy class, the optimiser makes it good. This is the most common combination in arm planning.
- Many seeds. Start from several different guesses (straight lines through different intermediate points, random perturbations) and keep the best, which GPUs make cheap.
- Warm starts. In MPC and replanning, start from the previous solution shifted forward in time: the world has changed little, so the old answer is an excellent guess.
- Continuation. Solve an easier version first (large margin, no dynamics, coarse grid) and use its answer to seed the harder one.
- Global methods where they fit, such as GCS or a search over homotopy classes.
Common misconceptions
Misconception
"Optimisation finds the best trajectory." It finds a locally best one, near its starting guess, as the two CHOMP runs that went different ways round the obstacle showed. Global optimality needs global methods, many seeds, or a convex formulation.
Misconception
"Collocation trajectories obey the dynamics." Only at convergence, and only at the knots and to the accuracy of the integration rule. Between knots, and with coarse knots, the real system can deviate; check by simulating the controls, and refine the mesh where the defects or the error estimate are large.
Misconception
"Time-optimal is always what you want." Time-optimal profiles ride the limits, leaving no margin for model error, payload changes or disturbances, and their acceleration jumps shake the robot. Real systems plan with scaled-down limits and jerk limits, trading a little time for accuracy, wear and safety.
Misconception
"Sampling and optimisation are competing approaches." In practice they are layers of one pipeline: search or sampling finds a feasible route, optimisation makes it smooth and fast, and time parameterisation and a tracking controller (Trajectories and tracking) execute it.
Exercises
The Python exercises use NumPy and run in your browser.
1. Trajectory optimisation
Warm-upHumanA quadrotor's planar model has 6 states and 2 controls. A trapezoidal direct collocation problem uses 21 knots, with the states and the controls at every knot as decision variables. How many decision variables are there?
2. Trajectory optimisation
CoreHumanA warehouse robot drives a straight 10 m aisle, starting and ending at rest. Its speed limit is 1.0 m/s and it can accelerate or brake at 0.5 m/s². What is the shortest time, in seconds, to drive the aisle?
3. Trajectory optimisation
CoreHumanA CHOMP-style optimiser is started from a straight-line path that passes exactly through the centre of a single round obstacle, symmetric above and below. What is likely to happen?
4. Trajectory optimisation
CoreHumanIn the lesson's experiment, covariant gradient descent (CHOMP's update ) was clear of the obstacle after 10 iterations, while plain gradient descent was still 21 cm inside it. What is the main reason?
5. Trajectory optimisation
StretchHumanWhich statements about single shooting and direct collocation are true? Select all that apply.
6. Trajectory optimisation
CoreComputationalWrite defects(x, v, u, h) for the double integrator , , using the trapezoidal rule. x, v, u are arrays (or lists) of the knot values and h is the time between knots. Return an array of shape (n, 2) whose row holds
- the position defect , and
- the velocity defect .
A collocation solver drives all of these to zero.
7. Trajectory optimisation
CoreComputationalWrite speed_profile(v_lim, ds, a_max). v_lim holds a speed limit at each of points spaced ds apart along a path. Return the fastest speeds v (a new NumPy array of the same length) such that
v[0] = v[-1] = 0(start and stop at rest),v[i] <= v_lim[i]everywhere, and- neighbouring speeds respect the acceleration limit: .
Use a forward pass and a backward pass.
Further reading and resources
- CHOMP: Gradient optimization techniques for efficient motion planning · Nathan Ratliff, Matt Zucker, J. Andrew Bagnell and Siddhartha Srinivasa · 2009The covariant gradient method implemented in this lesson; an extended journal version followed in 2013.
- Motion planning with sequential convex optimization and convex collision checking · John Schulman and colleagues · 2014TrajOpt, in the International Journal of Robotics Research; the first version appeared at Robotics Science and Systems in 2013.
- An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation · Matthew Kelly · 2017A clear tutorial in SIAM Review on trapezoidal and Hermite–Simpson collocation, with worked examples.
- Underactuated Robotics · Russ TedrakeFree online text whose trajectory optimisation chapters cover shooting, collocation and solvers, with Drake examples.
- A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis · Hung Pham and Quang-Cuong Pham · 2018TOPP-RA, a robust and fast method for the problem solved in this lesson's third program; implemented in the open-source toppra library.
- Motion Planning around Obstacles with Convex Optimization · Tobia Marcucci, Mark Petersen, David von Wrangel and Russ Tedrake · 2023Graphs of convex sets (GCS), a globally informed alternative to local trajectory optimisation, published in Science Robotics.
- Ruckig · Lars Berscheid and Torsten KrögerOpen-source jerk-limited online trajectory generation, used in MoveIt 2 and by robot manufacturers.