Direct transcription and sequential convexification

A trajectory optimizer reports success: the cart starts at rest, moves one meter, and stops exactly. Yet halfway through the first sampling interval it crosses a position limit that every optimization constraint says it respects. The optimization can be correct for the finite model and wrong for the physical trajectory. This chapter develops the finite model carefully enough to locate that discrepancy.

The running system remains \(\dot p=v,\ \dot v=u\), where \(p\) is position in meters, \(v\) velocity in meters per second, and \(u\) acceleration in meters per second squared. The transfer starts at \(p(0)=v(0)=0\) and ends at \(p(T)=D,\ v(T)=0\), with duration \(T>0\) and displacement \(D\). Original calculations below connect the continuous optimum to finite optimization. The recording also uses a river-crossing example and finishes with sequential convex programming; those examples establish where a linear cart ceases to represent the numerical difficulty.

Based on AA203 lecture 6 and official slides. The first part completes the preceding lecture's free-time boundary-value computation. Source examples and original teaching constructions are identified separately. A solved discretized problem is not presented as a certificate for an unmodeled physical vehicle. The official slides add the course placement of direct methods, the general problem statement with a terminal set and control set, the solver setup of the free-time computation, the nonuniform-grid simultaneous and shooting programs, the displayed river programs and their plotted results, Hermite–Simpson, pseudospectral and higher-order shooting parameterizations, the general sequential-convex-programming loop, artificial unboundedness, augmented Lagrangian methods, and the closing local-versus-global comparison; where their notation differs, the text translates it, and where a slide formula is wrong, the text states the corrected form.

Make the unknown duration a variable on a fixed interval 0:05

A boundary-value solver normally receives a known interval. Free-final-time optimal control seems to violate that interface: the point at which the terminal boundary conditions apply is itself unknown. The issue is a coordinate choice. Introduce normalized time \(\tau=t/T\), so that \(\tau\in[0,1]\). For any differentiable trajectory \(z(t)\), the chain rule gives \(dz/d\tau=T\,dz/dt\). The factor multiplies every differential equation, including the costate equations; omitting it from one equation changes the problem. U. M. Ascher and R. D. Russell, “Reformulation of boundary value problems into ‘standard’ form,” SIAM Review 23(2), 238–254 (1981), collect this and related conversions of nonstandard boundary-value problems into the fixed-interval form that general solvers accept.

Use the source's earlier cart example, which starts at position \(D_0=10\) and finishes at zero, with zero endpoint velocities. Its objective is \(\alpha T^2/2+(\beta/2)\int_0^T u^2\,dt\), where \(\alpha,\beta>0\) convert time and squared acceleration into compatible cost units. With Hamiltonian \(H=\beta u^2/2+\lambda_p v+\lambda_v u\), the normal necessary conditions give \(u=-\lambda_v/\beta\), \(\dot\lambda_p=0\), and \(\dot\lambda_v=-\lambda_p\). Define a fifth state \(r(\tau)\) representing the constant duration. The fixed-interval differential system is

Five first-order unknown functions require five scalar boundary conditions. Four impose the endpoint positions and velocities. The fifth is free-time transversality. Because terminal velocity is zero and \(u=-\lambda_v/\beta\), the terminal Hamiltonian is \(-\lambda_v(1)^2/(2\beta)\). Thus

The zero derivative of \(r\) makes its terminal value equal to its value everywhere. The additional variable is not an additional physical actuator: it lets the solver adjust the time scale until the fifth boundary condition holds. The analytic check from chapter 4 is \(T^5=18\beta D_0^2/\alpha\), giving \(T\approx4.47769493\) when both weights equal one. Matching this number tests the time transformation and boundary residual jointly.

A concrete solver setup makes the check reproducible. Take \(\alpha=\beta=1\), an initial mesh of 21 equally spaced points \(\tau=0,0.05,\ldots,1\), and the following initial guess: position decreasing linearly from \(10\) to \(0\), velocity and both costates identically zero, and \(r\equiv1\). This guess satisfies the four endpoint conditions but neither the differential equations, since position changes while velocity is zero, nor the transversality residual, which evaluates to \(\alpha r(1)=1\). Its duration is also low by a factor of about \(4.5\). With SciPy's default tolerance of \(10^{-3}\), solve_bvp nonetheless reports convergence: it refines the mesh to 59 points, its largest per-interval relative residual is \(3.0\times10^{-5}\), and it returns \(r=4.47772\), which exceeds the analytic \(4.47769\) by \(2.7\times10^{-5}\). Tightening the tolerance to \(10^{-6}\) refines the mesh to 61 points and reduces the discrepancy to \(4\times10^{-10}\). The remaining gap at the default setting is therefore a tolerance effect, not a modeling error. Convergence from such a crude guess is a property of this linear-quadratic instance; it does not show that the same guess strategy works for nonlinear dynamics.

A numerical implementation must also enforce \(T>0\). An original alternative is to represent the duration by a scalar parameter \(q\in\mathbb R\) and set \(T=e^q\). A solver supporting unknown parameters can then solve four differential equations and five boundary residuals. The logarithm guarantees positivity, but it introduces an exponential scale: an extreme initial \(q\) can create poor conditioning. A convenient parameterization is not a substitute for a sensible scale and initial guess. SciPy's documented unknown-parameter interface specifies the required number of boundary residuals.

Choose which variables the optimizer is allowed to move 15:34

An indirect method first derives necessary conditions and then solves the resulting state-costate boundary equations. A direct method first represents trajectories with finitely many variables and then optimizes their cost subject to finite constraints. The resulting optimization still has its own first-order conditions, but the user need not manually derive continuous costates to formulate it.

The two routes therefore perform the same two operations in opposite orders. An indirect method optimizes and then discretizes: it states optimality conditions in continuous time, and a boundary-value solver discretizes time internally when it solves them. A direct method discretizes and then optimizes. Both belong to the open-loop branch of the course: each computes a control as a function of time for one specified initial state. The closed-loop branch, which begins with dynamic programming in the next chapter, computes a control for every state instead, and model predictive control, later in the course, connects the branches by re-solving an open-loop problem from each measured state.

The general problem that a direct method transcribes has state \(x(t)\in\mathbb R^n\), control \(u(t)\in\mathcal U\subseteq\mathbb R^m\), and a running cost \(\ell(x,u,t)\):

Here \(x_{\mathrm{init}}\) is the given initial state and \(\Psi:\mathbb R^n\to\mathbb R^{n_\Psi}\) defines the terminal set \(\{x:\Psi(x)=0\}\) with \(n_\Psi\) scalar conditions. Two simplifications cost no generality. A terminal cost \(\phi(x(T))\) can be added to the objective, and it becomes \(\phi(x_N)\) in every finite version below; it is omitted only to shorten the displays. A nonzero initial time can be shifted to zero. The cart is the instance with \(n=2\), \(m=1\), \(\ell=u^2\), \(x_{\mathrm{init}}=(0,0)^\mathsf T\), \(\Psi(x)=(p-D,\ v)^\mathsf T\), and \(\mathcal U=\mathbb R\). The official slides write \(g\) for the running cost, \(t_f\) for the final time, \(h\) for the terminal cost, and \(F(x)=0\) for the terminal set. This chapter keeps \(\ell\), \(T\), \(\phi\), and \(\Psi\), because \(h\) denotes the sampling interval and \(F_h\) the numerical transition map.

Fix a grid \(t_k=kh\), where \(N\) is the positive number of intervals, \(h=T/N\), and \(k=0,\ldots,N\). Denote sampled states by \(x_k=(p_k,v_k)^\mathsf T\) and controls by \(u_k\) for \(k=0,\ldots,N-1\). Suppose controls are held constant inside each interval. This zero-order-hold assumption specifies what an executable control sequence means between samples; it is more information than a vector of numbers at grid points.

In simultaneous transcription, both the state samples and controls are decision variables. A numerical transition map \(F_h\) gives equality constraints \(x_{k+1}-F_h(x_k,u_k)=0\), called dynamics defects. The optimizer can temporarily move state variables off a simulated trajectory while it searches, but a feasible final decision must have small defects. In single shooting, only controls are decision variables. Starting from the fixed initial state, recursion through \(F_h\) constructs every subsequent state. The state equations are satisfied by construction for that numerical map.

The two families carry several names, and the names are not used consistently across sources. Simultaneous transcription is also called a state and control parameterization method, because both trajectories are represented by decision variables, and, following the slides, a collocation method, a name that some authors reserve for the higher-order forms. Single shooting is a control parameterization method. The official slides call the simultaneous program “direct transcription” and the control-only program “direct shooting.” This chapter's title uses direct transcription in the broader sense of any conversion of the continuous problem into a finite program, which includes shooting. The classifying question in every case is the same: which quantities may the optimizer change independently, and which are computed from them?

Single shooting uses fewer decision variables, but removing the state variables does not remove the dynamics from the optimization. Their effect moves into derivatives of the recursively computed objective and constraints. Simultaneous transcription uses more variables and exposes local sparse coupling. The appropriate comparison is therefore not merely variable count: derivative structure, constraint handling, initialization, integration accuracy, and conditioning all change.

A defect equation defines a finite approximation 23:55

For a general continuous system \(\dot x=f(x,u,t)\), forward Euler uses \(F_h(x_k,u_k)=x_k+h f(x_k,u_k,t_k)\). For the cart this becomes \(p_{k+1}=p_k+h v_k,\ v_{k+1}=v_k+h u_k\). Exact integration with held input instead gives

The missing \(h^2u_k/2\) is the position accumulated because velocity changes within the interval. Exactness here concerns the mathematical double integrator under held controls. It does not account for drag, motor delay, or disturbances that the model omits.

A running cost \(\ell(x,u,t)\) is often discretized as \(h\sum_{k=0}^{N-1}\ell(x_k,u_k,t_k)\), a left-rectangle quadrature rule. Holding the control constant does not generally make \(\ell\) constant: the state and explicit time can change. For our input-only effort \(E=\int_0^T u(t)^2\,dt\), the expression \(h\sum u_k^2\) is exact under zero-order hold. For a position penalty \(p(t)^2\), it is an approximation. Dynamics integration and cost integration are separate choices.

Nothing in these constructions requires equal intervals. Choose times \(0=t_0\lt t_1\lt\cdots\lt t_N=T\) and write \(h_k=t_{k+1}-t_k\gt0\). With forward Euler dynamics, left-rectangle cost quadrature, and held controls, the general problem of the preceding section becomes the simultaneous nonlinear program

with the dynamics and control constraints imposed for \(k=0,\ldots,N-1\). The initial condition must appear as a constraint, or \(x_0\) must be fixed and removed from the decision vector; otherwise the program optimizes the starting state as well. The uniform grid of the cart is the case \(h_k=h=T/N\). A nonuniform grid lets short intervals resolve rapid changes, such as the corners of a saturated control, without refining the whole horizon.

The shooting program over the same grid has only the controls as decision variables:

where \(\mathbf u=(u_0,\ldots,u_{N-1})\), \(x_0(\mathbf u)=x_{\mathrm{init}}\), and \(x_{k+1}(\mathbf u)=x_k(\mathbf u)+h_kf(x_k(\mathbf u),u_k,t_k)\). The argument \(\mathbf u\) marks each state as a computed function rather than an independent variable. The official slides write these computed states as \(x(t_k)\); the notation must not be read as the exact continuous solution at \(t_k\), which the Euler recursion only approximates. In both programs \(x_k\) is a sample at the node \(t_k\). What happens between nodes is a separate statement: the control is held, the cost is evaluated at the left node, and the state, for Euler, is effectively the straight line between consecutive samples.

There is a particularly instructive cancellation in the rest-to-rest cart. Let \(p_k^{\rm exact}\) and \(p_k^{\rm Euler}\) use the same held controls and the same zero initial state. Velocity is identical in both maps. Summing the missing half-acceleration terms gives

At a terminal rest state, \(v_N=0\), so both methods produce the same final position. At an intermediate state they need not agree at all. For \(T=2,\ N=2,\ u_0=1,\ u_1=-1\), both finish at position one with zero velocity. At time one, Euler reports position zero while the continuous held-input trajectory is at position \(1/2\). An endpoint test alone misses the modeling error.

Even an exact transition map cannot certify arbitrary path constraints from endpoint samples. On one interval of length one, take initial position zero, initial velocity one, and held acceleration minus two. Then \(p(s)=s-s^2\) for local time \(s\in[0,1]\). Both endpoint positions are zero, yet \(p(1/2)=1/4\). A constraint \(p\leq0.1\) holds at both nodes and fails between them. For this quadratic position curve, an exact check evaluates the endpoints and any interior stationary point \(s=-v_k/u_k\). For a general nonlinear system, a denser independent integration provides evidence, while a certified bound requires additional analysis.

The river example separates a model from a solver anecdote 29:37

The source's Zermelo navigation example replaces the cart with a boat crossing a river. Let \(x\) measure downstream position, \(y\) cross-stream position, \(v_b>0\) the boat's constant speed relative to the water, and \(\theta\) its heading measured from the downstream axis in radians. If the known current speed is \(c(y)\), downstream and cross-stream dynamics are

The illustrated current is parabolic, vanishing at the two banks and strongest in the middle. One possible original parameterization is \(c(y)=4c_{\max}(y/L)(1-y/L)\), with river width \(L>0\) and maximum current \(c_{\max}\geq0\). This formula states the qualitative profile explicitly; it is not a claim about an unseen numerical constant in the demonstration code.

The source fixes start \((0,0)\), destination \((M,L)\), duration, state bounds, and a heading bound \(|\theta|\leq\theta_{\max}\). Its example uses \(M=10,\ L=5,\ T=10\) in the chosen units and minimizes the integral of squared heading. This is a preference for headings near the downstream direction. It is not automatically fuel use: the model fixes water-relative speed and supplies no propulsion-energy law. The objective also depends on the chosen angular origin, so treating it as a universal measure of steering effort would be misleading.

In simultaneous Euler transcription, the finite vector contains all boat positions and all headings. Endpoint conditions and nodewise state bounds act directly on decision coordinates. Each defect connects neighboring positions and one heading. The objective is \(h\sum\theta_k^2\), which is exact for held headings even though the state transition is approximate. Nonlinear trigonometric terms and the parabolic current make the resulting constraints nonconvex.

Written out on the uniform grid \(h=T/N\), the simultaneous program is

together with the bounds \(0\le x_k\le M\) and \(0\le y_k\le L\) on the sampled positions. In this section \(x_k\) and \(y_k\) are boat coordinates, not the cart state. The program has \(3N+2\) decision variables and \(2N+4\) equality constraints; with \(N=20\), these are 62 variables and 44 equalities. Any general nonlinear-programming routine that accepts equality constraints and bounds can be applied to it, for example MATLAB's fmincon or SciPy's scipy.optimize.minimize. The official slides use \(u\) for the heading, \(v\) for the boat's water-relative speed, \(\mathrm{flow}(y)\) for the current, \((M,\ell)\) for the destination, and \(u_{\max}\) for the heading bound. This chapter writes \(\theta\), \(v_b\), \(c(y)\), \((M,L)\), and \(\theta_{\max}\), because \(u\), \(v\), and \(\ell\) already denote the cart's acceleration, its velocity, and the running cost.

The demonstration contrasts a heading limit of one with a tighter limit of \(0.75\). A particular solve fails from the initial guess at the tighter limit, then succeeds when initialized from the looser solution. This illustrates continuation: solve an easier nearby instance and use its solution to initialize the harder instance. It does not show that every failed solve proves infeasibility, or that continuation must succeed. A solver's status must be interpreted with its residuals and its documented termination conditions.

The reported solutions explain why the two limits behave so differently. To plotting accuracy, the heading under the limit of one rises from near zero to a single peak of about \(0.8\) radians near the middle of the ten-second horizon and falls again before arrival. Because the peak stays below one, the bound is strictly inactive at every node. Near this solution the bounded and unbounded feasible sets then coincide, so it is also a local solution of the problem with no heading bound at all; in this sense the limit of one is effectively no constraint. The limit of \(0.75\) lies below that peak, so it must change the solution. The constrained heading rises as before, stays at exactly \(0.75\) over an interior arc from roughly \(3.5\) to \(7.5\) seconds, and then falls. This is the saturation structure of the bounded-control problems in chapter 5: away from the arc the bound is inactive, and on the arc it holds with equality. Its cost cannot be lower than the optimal value of the problem without a heading bound, because every heading sequence feasible under the tighter bound is feasible without it; comparing it with the plotted solution for the limit of one also requires that solution to be a global minimizer, which a local solver does not certify. The looser solution violates the tighter bound near its peak, so the warm start is an infeasible initial guess, which the transcription accepts.

Direct methods often permit an initially infeasible trajectory guess. That freedom helps initialization but is solver-dependent: function domains, finite evaluations, and algorithm-specific requirements still matter. A square root of a negative quantity in a dynamics function cannot be rescued merely by calling the guess “infeasible.” Kelly's direct-collocation tutorial provides a complementary primary treatment of finite trajectory representations and practical transcription choices. The derivations and numerical cart checks in this chapter are independent teaching constructions.

Eliminating states trades sparse constraints for long sensitivities 43:02

In single shooting, start from \(x_0\), evaluate \(x_1=F_h(x_0,u_0)\), and continue to \(x_N\). Terminal and path constraints become functions of the complete control vector. A state bound is therefore possible in shooting; it simply loses the form of a direct bound on an independent state coordinate.

The river problem shows the elimination explicitly. Unrolling the Euler recursion from \((x_0,y_0)=(0,0)\) gives

and the shooting program minimizes \(h\sum_k\theta_k^2\) over the \(N\) headings alone, subject to \(|\theta_k|\le\theta_{\max}\) and the two terminal equalities \(x_N=M\), \(y_N=L\). The upper summation limit for \(y_k\) is \(k-1\), not \(k\): the heading \(\theta_k\) first affects \(y_{k+1}\). With the limit \(k\), the formula would assign \(y_0=h\,v_b\sin\theta_0\), contradicting the initial condition unless \(\theta_0=0\), and \(y_N\) would require a heading \(\theta_N\) that does not exist. The unrolled form also exposes the coupling that the next paragraphs quantify. Each heading \(\theta_j\) enters \(x_N\) directly through \(\cos\theta_j\) and indirectly through the current \(c(y_k)\) at every later node \(k\gt j\), because each such \(y_k\) contains \(\sin\theta_j\).

To see the derivative cost, define \(A_k=\partial F_h/\partial x\) and \(B_k=\partial F_h/\partial u\), evaluated along a nominal rollout. With no direct dependence of later controls on earlier controls, the chain rule yields

Early controls influence many later states. Expanding directions in the \(A_k\) can amplify sensitivities; contracting directions can make them extremely small. Neither problem is guaranteed in every system, but the long product explains why eliminating variables can make a seemingly small optimization difficult. In simultaneous transcription, a defect's derivative involves only neighboring states and the current control. A suitable sparse solver can exploit that locality.

Multiple shooting divides the horizon into segments, introduces independent states at segment boundaries, integrates within each segment, and imposes matching constraints between segments. It interpolates between one long rollout and a fully simultaneous representation. More segment states shorten sensitivity products but increase the optimization dimension. Choosing segments is a numerical design decision, not a change to the intended continuous control problem.

The recording's tightly bounded river example succeeds under shooting from a guess that caused trouble in the illustrated simultaneous solve. That is evidence about that particular combination of model, discretization, solver, and initialization. It is not a theorem that shooting is easier to initialize. A fair comparison would hold tolerances, integration accuracy, and computational budget fixed and examine multiple initial guesses.

Both transcriptions report closely similar trajectories and headings: to plotting accuracy, a peak of about \(0.8\) radians under the limit of one, and under the limit of \(0.75\) a saturated arc from roughly \(3.5\) to \(7.5\) seconds in the simultaneous results and from roughly \(3.3\) to \(7.7\) seconds in the shooting results. The plotted headings change slope every \(0.5\) seconds in the simultaneous results and about every \(1/3\) second in the shooting results, so the two displayed runs appear to use different grids; their near-agreement reflects two Euler discretizations of the same continuous problem, not an identity between them. On a common grid the agreement would be exact, because both programs would then use the same Euler map, the same grid, and the same constraints. The simultaneous dynamics constraints determine every state uniquely from the controls, so the feasible control sequences of the two programs coincide and receive the same objective value. Because the computed states depend continuously on the controls, a local minimizer of one program also corresponds to a local minimizer of the other. The methods differ in the path a solver takes, not in the set of solutions it seeks: simultaneous iterates may violate the defects before convergence, while shooting iterates satisfy the dynamics exactly and may violate only the terminal and path conditions.

Higher-order transcriptions change the trajectory between nodes 59:28

Every transcription implies a continuous trajectory between its nodes, and the implied trajectory determines what a satisfied defect certifies. Forward Euler uses the slope \(f(x_k,u_k,t_k)\) over the whole interval, so its effective state trajectory is the piecewise-linear interpolant of the samples, paired with a zero-order-hold control. That trajectory is continuous, of class \(C^0\), but its slope generally jumps at every node, because the slope on each interval is evaluated at a different sample. It is also not the trajectory that the held control actually produces: for the cart the held-input position is quadratic within each interval, as the intersample example in the discretization section showed.

Hermite–Simpson collocation raises the polynomial degree of both trajectories. The control is a first-order hold: decision values \(u_0,\ldots,u_N\) at all \(N+1\) nodes, joined by straight lines, so the midpoint control is \(u_{k+1/2}=(u_k+u_{k+1})/2\). On each interval the state is the cubic Hermite polynomial that matches the samples \(x_k,\ x_{k+1}\) and the dynamics slopes \(f_k=f(x_k,u_k,t_k),\ f_{k+1}=f(x_{k+1},u_{k+1},t_{k+1})\) at both ends. Adjacent cubics share the slope \(f_k\) at node \(t_k\), so the effective state trajectory is continuously differentiable, of class \(C^1\). Matching the endpoints imposes nothing yet; the dynamics enter through one additional collocation point, the interval midpoint \(t_{k+1/2}=t_k+h_k/2\). Evaluating the cubic there gives its midpoint value and requires its midpoint slope to equal the dynamics:

with \(f_{k+1/2}=f(x_{k+1/2},u_{k+1/2},t_{k+1/2})\). The second equation is the rearranged midpoint condition: the cubic's midpoint slope is \(3(x_{k+1}-x_k)/(2h_k)-(f_k+f_{k+1})/4\), and setting it equal to \(f_{k+1/2}\) yields Simpson's quadrature rule for \(\int f\,dt\) over the interval. This is the form derived in Kelly's tutorial. The defect is implicit: \(x_{k+1}\) appears inside \(f_{k+1}\) and, through \(x_{k+1/2}\), inside \(f_{k+1/2}\). A simultaneous program handles this naturally, because the defect is simply an equality constraint among decision variables; no step of the transcription has to solve for \(x_{k+1}\) explicitly.

On the cart, Hermite–Simpson defects are satisfied exactly by the continuous trajectory under first-order hold. With a linear control on the interval, velocity is quadratic and position cubic. The cubic Hermite interpolant of a cubic or quadratic function reproduces it, and Simpson's rule integrates polynomials up to degree three exactly, so both defect components vanish. Take \(h=1\), \(p_k=v_k=0\), \(u_k=1\), \(u_{k+1}=-1\). The continuous trajectory reaches \(v_{k+1}=h(u_k+u_{k+1})/2=0\) and \(p_{k+1}=h^2(2u_k+u_{k+1})/6=1/6\). The interpolated midpoint velocity is \(0+\tfrac18(u_k-u_{k+1})=1/4\), which equals the exact \(v(1/2)=1\cdot\tfrac12-2\cdot\tfrac18=1/4\). The position defect is \(1/6-\tfrac16(0+4\cdot\tfrac14+0)=0\) and the velocity defect is \(0-\tfrac16(1+0-1)=0\). This exactness parallels the exact held-input map of the discretization section, under a different control parameterization; it does not extend to nonlinear dynamics, where the cubic is only an approximation.

For nonlinear dynamics, the gain is a higher order of accuracy. Consider the scalar test equation \(\dot\zeta=-\zeta^3+\sin t\) with \(\zeta(0)=1\), integrated to \(t=2\) with \(N=10,20,40,80\) uniform steps, and compare the endpoint against a reference solution computed with relative tolerance \(10^{-13}\). Forward Euler's errors are \(9.7\times10^{-3}\), \(4.3\times10^{-3}\), \(2.0\times10^{-3}\), and \(9.9\times10^{-4}\): each doubling of \(N\) divides the error by about two, the signature of first-order accuracy. Hermite–Simpson steps, each solved for \(\zeta_{k+1}\) by scalar root finding, give \(2.3\times10^{-6}\), \(1.4\times10^{-7}\), \(8.8\times10^{-9}\), and \(5.5\times10^{-10}\): each doubling divides the error by \(16.0\) to \(16.1\), consistent with fourth-order accuracy. At \(N=10\), the Hermite–Simpson error is already about \(4200\) times smaller than Euler's. This is an original numerical check on one smooth equation, not a convergence theorem; a kink in the control or the dynamics inside an interval reduces the observed order.

Shooting methods obtain higher order differently: they replace Euler inside the rollout with an explicit higher-order integrator, so the dynamics are enforced by explicit integration rather than by constraints. The classical fourth-order Runge–Kutta map with the control held at \(u_k\) is

with \(\kappa_1=f(x_k,u_k,t_k)\), \(\kappa_2=f(x_k+\tfrac h2\kappa_1,u_k,t_k+\tfrac h2)\), \(\kappa_3=f(x_k+\tfrac h2\kappa_2,u_k,t_k+\tfrac h2)\), and \(\kappa_4=f(x_k+h\kappa_3,u_k,t_k+h)\). On the same test equation its errors are \(5.2\times10^{-5}\), \(2.9\times10^{-6}\), \(1.7\times10^{-7}\), and \(1.0\times10^{-8}\), again falling by a factor of \(16\) to \(18\) per doubling. Each step costs four evaluations of \(f\) instead of one. In shooting, the integration grid may also be finer than the control grid, for example several Runge–Kutta substeps per held control: that refinement adds function evaluations and lengthens the sensitivity chains, but it adds no decision variables.

Pseudospectral methods replace piecewise polynomials with one global polynomial over the horizon, or over a few long segments. The state is represented by its values at a set of nodes, often the roots of an orthogonal polynomial or the endpoints together with such roots, and the derivative of the interpolating polynomial at each node is a fixed linear combination of all nodal values. The defect at node \(i\) equates that combination with \(f\) at node \(i\), so every defect couples every node, and the constraint Jacobian is dense rather than banded. For a solution that is analytic on the horizon, polynomial interpolation at such nodes can converge faster than any fixed power of the number of nodes. A corner removes that advantage: the \(0.75\)-bounded river heading has slope discontinuities where it enters and leaves its bound, and a single global polynomial converges only algebraically near such points. Practical implementations therefore divide the horizon into segments with one polynomial on each, which makes pseudospectral and piecewise collocation two ends of a single family.

These parameterizations therefore differ along four separate axes: the smoothness of the effective state trajectory (\(C^0\) for Euler, \(C^1\) for Hermite–Simpson, a single smooth polynomial per segment for pseudospectral methods), the control hold (zero order, first order, or polynomial), the way the dynamics are enforced (an explicit update, an implicit defect, or a global derivative identity), and the sparsity of the resulting program. Kelly's tutorial develops trapezoidal and Hermite–Simpson collocation in implementable detail; Anil V. Rao's 2009 A Survey of Numerical Methods for Optimal Control is a further survey of direct methods; and the course notes treat Hermite–Simpson collocation in their section 5.2.1. Multiple shooting combines these choices with the segment structure described in the shooting section.

Original worked case: solve both finite representations exactly

The linear cart allows a controlled experiment without nonlinear local minima. Under the exact held-input map, terminal velocity and position are linear functions of \(\mathbf u=(u_0,\ldots,u_{N-1})^\mathsf T\):

Let \(C\in\mathbb R^{2\times N}\) contain those position and velocity coefficients, and let \(b=(D,0)^\mathsf T\). Single shooting becomes the equality-constrained quadratic program \(\min h\mathbf u^\mathsf T\mathbf u\) subject to \(C\mathbf u=b\). For \(N\geq2\), the two rows of \(C\) are independent. Stationarity of \(h\mathbf u^\mathsf T\mathbf u+\nu^\mathsf T(C\mathbf u-b)\) gives \(2h\mathbf u+C^\mathsf T\nu=0\), where \(\nu\in\mathbb R^2\) is the endpoint multiplier. Substitution into the constraint yields

Strict convexity of the effort makes this feasible point the unique optimal control sequence. The formula uses a matrix inverse only as notation; numerical code solves the two-dimensional linear system. A single interval cannot move and stop from rest at a nonzero displacement because its velocity condition forces its only input to zero.

We can derive the discretization gap without a solver. Zero terminal velocity implies \(\sum u_i=0\), so subtracting the mean coefficient from every position weight changes nothing. Define \(r_i=(N-1)/2-i\). Then \(D=h^2\sum r_i u_i\), while \(\sum r_i^2=N(N^2-1)/12\). Cauchy–Schwarz gives \(D^2\leq h^4(\sum r_i^2)(\sum u_i^2)\), with equality when \(u_i\) is proportional to \(r_i\). Therefore

The finite optimum exceeds the continuous optimum because piecewise-constant controls form a restricted admissible class. The ratio is \(1/(1-N^{-2})\), so the relative gap is \(1/(N^2-1)\). For \(D=1,\ T=2\), the costs at \(N=2,4,8,16\) are \(2,\ 1.6,\ 1.52380952,\ 1.50588235\), approaching \(1.5\). Optimization residuals can be near machine precision while this representation gap remains appreciable.

For simultaneous transcription, stack all positions and velocities followed by the controls in a vector \(z\). Let \(Gz=d\) collect the initial, dynamics, and terminal equalities, and let \(Q\) be zero on state coordinates and \(2hI\) on controls. The objective is \(z^\mathsf TQz/2\). Although \(Q\) is not positive definite on the entire space, its quadratic form is positive on every nonzero feasible perturbation: zero control perturbation together with fixed initial state and dynamics forces every state perturbation to zero. The equality-constrained optimum is consequently unique. Solving

must reproduce the condensed solution. Here \(\eta\) contains one multiplier for each equality. This is an independent formulation check: agreement is stronger than comparing two calls to the same optimizer with slightly different tolerances.

Convex subproblems approximate a nonconvex trajectory problem 1:00:38

Suppose the cart acquires nonlinear drag, \(\dot p=v,\ \dot v=u-\gamma v^3\), with \(\gamma>0\) carrying the units needed to make \(\gamma v^3\) an acceleration. At outer iteration \(j\), let \((\bar x^{(j)},\bar u^{(j)})\) denote the nominal trajectory. A bar identifies a linearization point, not an initial condition. Suppress the superscript temporarily. With deviations \(\delta x=x-\bar x\) and \(\delta u=u-\bar u\), first-order Taylor expansion gives \(f(x,u)\approx f(\bar x,\bar u)+A\delta x+B\delta u\), where

The constant term is necessary. Dropping it would make the affine model disagree with the nonlinear dynamics at the point around which it was expanded. For this polynomial example the omitted acceleration is exactly \(-\gamma[3\bar v(\delta v)^2+(\delta v)^3]\). If \(|\delta v|\leq\Delta\), its magnitude is at most \(\gamma(3|\bar v|\Delta^2+\Delta^3)\). This explicit bound explains why a trust region limits the change from the nominal trajectory: local linearization is useful only where its remainder is controlled.

If the cost and the remaining constraints are convex, replacing the dynamics by affine equations yields a convex subproblem. A quadratic effort gives a quadratic objective; the subproblem is not generally a linear program. Solving a sequence of these problems and updating the nominal trajectory is sequential convex programming. Each convex subproblem may be solved globally without making the original nonlinear problem globally convex.

The drag computation is one instance of a general construction. For the general problem, the nonconvexity comes from the dynamics and possibly from the cost. Assume first that \(\ell\) is convex in \((x,u)\) at each time. Linearizing \(f\) along the nominal pair gives the time-varying affine model

where \(A^{(j)}(t)=\partial f/\partial x\) and \(B^{(j)}(t)=\partial f/\partial u\) are evaluated at \((\bar x^{(j)}(t),\bar u^{(j)}(t),t)\). For the drag model, \(A^{(j)}(t)\) is the matrix \(A\) above with \(\bar v=\bar v^{(j)}(t)\), so the Jacobian varies along the nominal trajectory even though the model is time-invariant. The nominal pair need not be feasible. It need not satisfy the dynamics, the boundary conditions, or the bounds; it serves only as the point of expansion, which is why an initial guess can be a crude interpolation. A nonconvex running cost is handled by the same device: replace it by a convex local model around the nominal, such as its first-order expansion. The official slides index iterations by a subscript and call the first nominal pair \((x_0(\cdot),u_0(\cdot))\), which collides with the initial state \(x_0\). This chapter writes the iteration as a superscript \((j)\) and marks nominal quantities with a bar for that reason.

Replacing \(f\) by \(f^{(j)}\) gives a continuous-time problem with affine dynamics. Discretizing it on the grid of the general simultaneous program, and taking the terminal condition in the affine form \(\Psi(x)=x-x_{\mathrm f}\) of a fixed target state \(x_{\mathrm f}\), gives the subproblem

It is a convex program when each \(\ell(\cdot,\cdot,t_k)\) is jointly convex, \(\mathcal U\) is a convex set, and every \(h_k\) is positive: the objective is then a positive combination of convex functions, and every equality is affine in the decision variables. Convexity of the cost alone is not enough; a nonconvex control set, such as a thrust magnitude bounded away from zero, keeps the subproblem nonconvex. A nonlinear terminal function \(\Psi\) must be linearized in the same way as \(f\). One outer iteration then has four steps: linearize \(f\) about \((\bar x^{(j)},\bar u^{(j)})\); form the affine continuous-time problem; discretize it into the convex program above; and solve that program, taking its solution as \((\bar x^{(j+1)},\bar u^{(j+1)})\). The loop repeats until a stopping test is met.

The order of linearization and discretization does not matter for forward Euler, provided the nominal trajectory is sampled at the nodes. Write \(\bar x_k=\bar x^{(j)}(t_k)\), \(\bar u_k=\bar u^{(j)}(t_k)\), \(\bar A_k=A^{(j)}(t_k)\), and \(\bar B_k=B^{(j)}(t_k)\). Linearizing the Euler map \(F_h(x,u)=x+h_kf(x,u,t_k)\) about \((\bar x_k,\bar u_k)\) gives \(\bar x_k+h_kf(\bar x_k,\bar u_k,t_k)+(I+h_k\bar A_k)(x-\bar x_k)+h_k\bar B_k(u-\bar u_k)\); its Jacobians \(I+h_k\bar A_k\) and \(h_k\bar B_k\) are the transition Jacobians of the shooting section for this map. The expression simplifies to \(x+h_k[f(\bar x_k,\bar u_k,t_k)+\bar A_k(x-\bar x_k)+\bar B_k(u-\bar u_k)]\), which is exactly the Euler discretization of \(f^{(j)}\). For Runge–Kutta or exact-hold maps the two orders need not coincide, because the discrete Jacobian then involves the continuous Jacobian at several points inside the interval, and the order becomes a design choice.

The source demonstrates Jacobian construction with automatic differentiation. Differentiation computes derivatives of the implemented function, not a guarantee that the function represents the intended physics. An analytic Jacobian for a small test model, followed by finite-difference checks, helps detect sign, coordinate, and scaling mistakes before using a larger differentiable model.

Malyuta and colleagues' trajectory-generation tutorial distinguishes lossless convexification from sequential convex approaches, including SCvx and GuSTO. That distinction matters here: an affine approximation plus repeated solves is not automatically an equivalent convex reformulation. Any convergence or feasibility claim must be tied to a specified algorithm and its assumptions, not borrowed from the word “convex.”

A trust region and a slack variable solve different problems 1:14:38

Linearization can create artificial infeasibility. Consider the scalar equality \(x^2=1\), which has two feasible points. Linearizing at \(\bar x=0\) yields \(0=1\), an inconsistent affine equation. A trust region alone cannot restore feasibility because restricting an empty feasible set leaves it empty. Adding a defect variable can make the subproblem solvable, but a nonzero defect means the original equation is still violated.

Linearization can also create the opposite defect, artificial unboundedness. Minimize \(w\) over scalars \(w\) and \(\omega\) subject to \(\omega=w^2\) and \(\omega\le1\). The true feasible set is \(w\in[-1,1]\), and the minimizer is \(w^\star=-1\). Linearizing the equality at \(\bar w\) gives \(\omega=\bar w^2+2\bar w(w-\bar w)\). At \(\bar w=0\) this is \(\omega=0\), which leaves \(w\) unconstrained, so the convex subproblem has no minimizer: its objective decreases without bound. The curvature that confined \(w\) to \([-1,1]\) has been linearized away. A trust region \(|w-\bar w|\le\Delta\) restores a bounded subproblem, whose solution is \(w=-\Delta\), and the linearization error at that point is exactly \((w-\bar w)^2=\Delta^2\). The radius bounds the model error; it does not make the step correct. With \(\Delta=2\), the step \(w=-2\) satisfies the linear model with \(\omega=0\), but \(w^2=4\) violates the true constraint, and a test against the original problem must reject it. Halving the radius to \(\Delta=1\) gives \(w=-1\). Relinearizing there gives \(\omega=1-2(w+1)\), so \(\omega\le1\) becomes \(w\ge-1\), and the next subproblem returns \(w=-1\) again: the iteration has reached a fixed point at the true minimizer. A trust region is the remedy for artificial unboundedness, as a slack or defect variable is the remedy for artificial infeasibility; neither substitutes for the other.

Likewise, relaxing a bound with a nonnegative slack and charging a penalty permits violations rather than eliminating them. The original scalar example

has \(u^\star=1+1/(1+\rho)\) and \(s^\star=1/(1+\rho)\). To derive this, observe that for \(u>1\) the cheapest feasible slack is \(u-1\); differentiating \((u-2)^2+\rho(u-1)^2\) gives the stated minimizer, which indeed lies above one. Every finite quadratic penalty leaves a positive violation. Increasing \(\rho\) reduces the violation but can worsen numerical scaling. This counterexample prevents a common interpretation error: a small objective does not imply a hard safety limit was respected.

Augmented Lagrangian methods remove the conflict between a finite penalty and a hard constraint without driving \(\rho\) to infinity. Eliminating the slack, whose cheapest value is \(\max\{0,u-1\}\), the penalty problem above minimizes \((u-2)^2+\rho\max\{0,u-1\}^2\). Augment it with a multiplier estimate \(\mu\ge0\):

At \(\mu=0\) this is exactly the penalty objective. For fixed \(\mu\) it is convex in \(u\). Setting its derivative \(2(u-2)+\mu+2\rho(u-1)\) to zero gives \(u=1+(2-\mu)/(2+2\rho)\), and at that point \(\mu+2\rho(u-1)=(2\mu+4\rho)/(2+2\rho)\gt0\) for every \(\mu\ge0\), so the stationary point lies on the branch where the maximum is positive and is the minimizer. The method alternates this minimization with the multiplier update \(\mu\leftarrow\max\{0,\mu+2\rho(u-1)\}\). Substituting the minimizer gives \(\mu^+-2=(\mu-2)/(1+\rho)\). Starting from \(\mu=0\), the \(j\)th minimization therefore returns \(u_j=1+(1+\rho)^{-j}\), and the \(j\)th update gives \(\mu_j=2-2(1+\rho)^{-j}\). The first iterate reproduces the penalty solution; each later one shrinks the violation by the factor \(1+\rho\) while \(\rho\) stays fixed.

With \(\rho=1\), the violations are \(0.5,\ 0.25,\ 0.125,\ldots\), and the first one below \(0.001\) occurs at the tenth minimization, \(2^{-10}\approx9.8\times10^{-4}\). The penalty alone needs \(\rho=999\) for a violation of \(0.001\), which raises the curvature \(2+2\rho\) of the penalized objective from \(4\) to \(2000\). The multiplier estimate converges to \(2\), the Lagrange multiplier of \(u\le1\) at the constrained optimum: stationarity of \((u-2)^2+\mu(u-1)\) at \(u=1\) requires \(\mu=2\). The estimate supplies the constraint's price, so the minimizer can satisfy the constraint without an unbounded penalty. This exact geometric convergence is a property of a convex scalar example. For nonconvex trajectory programs, the convergence guarantees of augmented Lagrangian solvers are local and conditional, and the final constraint residual must still be reported separately.

A defensible iteration evaluates the candidate in the original nonlinear dynamics, measures original constraint residuals and any artificial slack, and accepts or rejects the step using a specified criterion. The source's decreasing slacks illustrate progress in its example; they are not a general certificate that all slack will vanish. Trust-region policy, penalty updates, stopping tolerances, and final independent simulation form part of the algorithm.

A complete sequential-convex-programming method arranges these devices in a fixed loop. From an initial trajectory guess, each pass linearizes the nonconvexities about the current reference, adds the slack variables and trust region that guard against artificial infeasibility and artificial unboundedness, discretizes in time, and solves the resulting convex subproblem. A test against the original problem then either stops the method, declaring convergence, or updates the trust region, and the reference when the step is accepted, before the next pass. The first pass linearizes about the initial guess. Malyuta and colleagues' tutorial, published as “Convex Optimization for Trajectory Generation: A Tutorial on Generating Dynamically Feasible Trajectories Reliably and Efficiently” in IEEE Control Systems Magazine, vol. 42, no. 5, pp. 40–113, October 2022, organizes SCvx and GuSTO around this loop and discusses the assumptions behind their convergence guarantees.

Each device in the loop is a choice about the finite program, not a property of the control problem: the initial guess and continuation schedule, the slack and penalty weights, the trust-region policy, and the multiplier updates. Tedrake's trajectory-optimization chapter of Underactuated Robotics describes the resulting experience directly: “As you begin to play with these algorithms on your own problems, you might feel like you're on an emotional roller-coaster.” The checks in the final section separate what such tuning has achieved from what it has not.

Executable checks: condensed versus simultaneous optimization

The following original Python example requires NumPy. Its inputs are the interval count, displacement, and positive duration; its output is the exact-map held control sequence and its effort. It has no file or network effects. It rejects a grid too short to perform a nonzero rest-to-rest transfer and uses dense linear algebra for a small teaching problem. A production transcription should preserve sparse matrices and monitor conditioning.

import numpy as np

def condensed(n, distance=1.0, duration=2.0):
    if n < 2 or duration <= 0:
        raise ValueError("need at least two intervals and positive duration")
    h = duration / n
    C = np.vstack((h*h*(n-np.arange(n)-0.5), h*np.ones(n)))
    u = C.T @ np.linalg.solve(C @ C.T, [distance, 0.0])
    return u, h * (u @ u)

def simultaneous(n, distance=1.0, duration=2.0):
    h = duration / n
    A = np.array([[1.0, h], [0.0, 1.0]])
    B = np.array([h*h/2, h])
    nx, nz = 2*(n+1), 2*(n+1)+n
    G = np.zeros((2*n+4, nz))
    d = np.zeros(2*n+4)
    G[:2, :2] = np.eye(2)
    for k in range(n):
        rows = slice(2+2*k, 4+2*k)
        G[rows, 2*k:2*k+2] = -A
        G[rows, 2*k+2:2*k+4] = np.eye(2)
        G[rows, nx+k] = -B
    G[-2:, 2*n:2*n+2] = np.eye(2)
    d[-2] = distance
    Q = np.zeros((nz, nz))
    Q[nx:, nx:] = 2*h*np.eye(n)
    K = np.block([[Q, G.T], [G, np.zeros((len(d), len(d)))]])
    z = np.linalg.solve(K, np.r_[np.zeros(nz), d])[:nz]
    assert np.max(np.abs(G @ z - d)) < 1e-11
    return z[nx:], h*(z[nx:] @ z[nx:])

for n in (2, 4, 8, 16):
    u, effort = condensed(n)
    w, other_effort = simultaneous(n)
    assert np.allclose(u, w, atol=1e-11)
    assert abs(effort - 1.5/(1-1/n**2)) < 1e-11
    assert abs(effort - other_effort) < 1e-11
    print(n, round(effort, 9))

# A node-feasible trajectory can violate its continuous path constraint.
s = np.array([0.0, 0.5, 1.0])
position = s - s*s
assert np.all(position[[0, 2]] <= 0.1)
assert position[1] > 0.1

The displayed costs are \(2,\ 1.6,\ 1.523809524,\ 1.505882353\). These checks compare two mathematical formulations, an analytic value, and an explicit intersample counterexample. They do not test a nonlinear solver's behavior on the river problem or establish a global convergence theorem for sequential convex programming.

Exercises with worked solutions

What a successful computation establishes

Record at least three distinct checks. The optimization residual measures how well the finite decision satisfies its algebraic conditions. A mesh or integration comparison measures sensitivity to the finite approximation. An independent rollout and path-constraint check tests the proposed control against the specified continuous model. Agreement across these checks strengthens the numerical case while leaving model mismatch as a separate uncertainty.

The next chapter changes the computational question. Direct transcription computes an input sequence for one initial state. Dynamic programming computes how the best remaining cost depends on the current state, leading to feedback laws that can respond when the realized state differs from the plan. The two viewpoints will meet again: a Riccati recursion can solve the same linear-quadratic finite problem by exploiting its temporal structure.

The two branches trade along three separate axes. The first is the scope of optimality. A direct method returns one trajectory from one initial state, and for a nonconvex program a local solver returns at best a point that satisfies local optimality conditions; the river's failed and continued solves show that the result depends on the starting point. Dynamic programming returns a value and a control for every state of its model, optimal globally over that model, so the picture changes from one curve ending at the target to a field of level sets of the value with a control arrow at every point. The second axis is computation, and the third is state dimension, which governs the computation. A simultaneous program with \(n\) states, \(m\) controls, and \(N\) intervals has \((N+1)n+Nm\) decision variables, linear in \(n\); the simultaneous river program, whose plotted node spacing of \(0.5\) seconds corresponds to \(N=20\), has \(n=2\), \(m=1\), and 62 decision variables. A value table with 101 grid values per state coordinate has \(101^n\) entries: \(10{,}201\) for the river, \(1{,}061{,}520{,}150{,}601\) for \(n=6\), and about \(1.13\times10^{24}\) for \(n=12\). Direct methods therefore reach high-dimensional systems at the price of local answers for one initial state, while tabulated dynamic programming gives global answers for low-dimensional systems. The next chapter's complexity section develops the dimension count, and model predictive control, later in the course, obtains feedback from repeated local solves.

Primary sources

• AA203 lecture 6 recording and official lecture 6 slides; the opening free-time computation continues material in lecture 5's deck.
• Matthew Kelly, An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation, SIAM Review, 2017.
• Danylo Malyuta and colleagues, Convex Optimization for Trajectory Generation, preprint and subsequent IEEE Control Systems tutorial.
• SciPy solve_bvp documentation, boundary residual and unknown-parameter interfaces.
• Russ Tedrake, Underactuated Robotics, chapter 10: Trajectory Optimization, MIT course notes; source of the quoted remark on the practical sensitivity of direct methods.
• Anil V. Rao, A Survey of Numerical Methods for Optimal Control, 2009; cited by the official slides as a resource for direct methods.
• Stanford Autonomous Systems Lab, AA203 course notes, chapter 5 on direct methods and section 5.2.1 on Hermite–Simpson collocation, cited by the official slides.