We consider a robot with state \( x(t)\in\mathbb{R}^{n_x} \) (e.g. joint positions and velocities) and control \( u(t)\in\mathbb{R}^{n_u} \), governed by dynamics

\[ \dot{x}(t) = f(x(t),u(t),t), \quad t \in [0,T]. \]

A standard fixed-horizon optimal control problem (OCP) is

\[ \min_{x(\cdot),u(\cdot)} \; J = \phi\big(x(T)\big) + \int_0^T \ell\big(x(t),u(t),t\big)\,\mathrm{d}t \]

\[ \text{s.t.}\quad \dot{x}(t) = f(x(t),u(t),t),\; x(0) = x_0,\; x(T) \in \mathcal{X}_T,\; g\big(x(t),u(t),t\big) \leq 0. \]

In robotics, \( x(t) \) often stacks joint coordinates and velocities, while \( u(t) \) are joint torques or commanded accelerations. Direct methods approximate \( x(\cdot), u(\cdot) \) by a finite set of parameters, yielding a finite-dimensional NLP that can be solved by generic solvers (SQP, interior-point, etc.).

Let \( N \) be the number of time intervals and define a grid \( t_k = k h \) with \( k = 0,\dots,N \) and \( h = T/N \). Direct transcription methods introduce decision variables for values of \( x(t),u(t) \) on this grid, then impose constraints that approximate the dynamics.

A generic direct transcription introduces

\[ z = \big(x_0,\dots,x_N,u_0,\dots,u_{N-1}\big) \in \mathbb{R}^{(N+1)n_x + N n_u}, \]

and constructs an NLP of the form

\[ \min_{z} \; J_h(z) \quad \text{s.t.}\quad c_h(z) = 0,\; d_h(z) \leq 0, \]

where \( c_h \) encodes dynamics and boundary constraints, and \( d_h \) encodes inequality constraints (joint limits, torque bounds, etc.).

flowchart TD
  A["Continuous-time OCP"] --> B["Time grid t_k"]
  B --> C["Choose parameterization: shooting or collocation"]
  C --> D["Finite-dimensional NLP (cost + constraints)"]
  D --> E["Sparse NLP solver"]
  E --> F["Optimal discrete state-control sequence"]
  F --> G["Reconstruct continuous trajectory"]
        

The rest of this lesson compares shooting (single and multiple) with direct collocation, both from a mathematical and implementation perspective.

3.1 Single Shooting

Single shooting parameterizes only the control (and possibly some boundary states) and obtains states by numerical integration. Let the decision vector be

\[ \theta = \big(u_0,\dots,u_{N-1}\big) \in \mathbb{R}^{N n_u}, \]

with a chosen time-discretization scheme \( \Psi_h \) (e.g. Runge–Kutta) such that

\[ x_{k+1}(\theta) = \Psi_h\big(x_k(\theta),u_k\big), \quad x_0(\theta) = x_0. \]

The terminal state is a differentiable function \( F(\theta) = x_N(\theta) \), and the OCP is approximated by

\[ \min_{\theta} \; J_h(\theta) = \phi\big(F(\theta)\big) + \sum_{k=0}^{N-1} h\,\ell\big(x_k(\theta),u_k,t_k\big) \]

\[ \text{s.t.}\quad F(\theta) \in \mathcal{X}_T,\; g\big(x_k(\theta),u_k,t_k\big) \leq 0. \]

Gradient-based optimization requires sensitivities \( \partial x_k/\partial \theta \), obtained by integrating the variational equations or by automatic differentiation. For unstable dynamics, these sensitivities can grow roughly like exponentials, which may lead to ill-conditioned Jacobians.

3.2 Multiple Shooting

Multiple shooting partitions the horizon into segments and introduces explicit state variables at the segment boundaries. For simplicity, consider segment boundaries at the same grid points \( t_k \). The decision vector becomes

\[ z = \big(x_0,\dots,x_N,u_0,\dots,u_{N-1}\big), \]

but states \( x_k \) are now independent optimization variables. Dynamics are enforced by matching each state to the result of integrating from the previous state:

\[ c_k^{\mathrm{ms}}(z) = x_{k+1} - \Psi_h(x_k,u_k) = 0, \quad k = 0,\dots,N-1. \]

The NLP is larger than in single shooting, but Jacobians are typically better conditioned: perturbations in a given segment do not exponentially propagate through the entire horizon inside the decision variables; they appear in local constraints, which solvers exploit via sparse linear algebra.

Direct collocation introduces state and control decision variables at grid points and enforces the dynamics through integral consistency constraints that mimic high-order integration schemes.

4.1 Trapezoidal Collocation

Let \( x_k \approx x(t_k) \) and \( u_k \approx u(t_k) \). Trapezoidal collocation enforces

\[ c_k^{\mathrm{trap}}(x_k,x_{k+1},u_k,u_{k+1}) = x_{k+1} - x_k - \frac{h}{2}\Big(f(x_k,u_k,t_k) + f(x_{k+1},u_{k+1},t_{k+1})\Big) = 0, \]

for all \( k = 0,\dots,N-1 \). The discrete cost is typically

\[ J_h = \phi(x_N) + \sum_{k=0}^{N-1} h\,\ell(x_k,u_k,t_k), \]

or a higher-order quadrature (e.g. Simpson) if higher accuracy for the integral cost is needed.

4.2 Hermite–Simpson Collocation

Hermite–Simpson collocation adds midpoint variables \( x_{k+\frac{1}{2}}, u_{k+\frac{1}{2}} \) and uses a cubic Hermite interpolant plus Simpson integration:

\[ c_k^{\mathrm{HS}} = x_{k+1} - x_k - \frac{h}{6} \Big(f_k + 4 f_{k+\frac{1}{2}} + f_{k+1}\Big) = 0, \]

where \( f_k = f(x_k,u_k,t_k) \) and \( f_{k+\frac{1}{2}} = f(x_{k+\frac{1}{2}},u_{k+\frac{1}{2}},t_k + h/2) \). This scheme achieves higher order accuracy and is widely used in high-precision robot trajectory optimization.

Assume the exact solution \( x(t) \) to the ODE is sufficiently smooth (e.g. three times continuously differentiable). Define the local truncation error for trapezoidal collocation as

\[ \tau_k = x(t_{k+1}) - x(t_k) - \frac{h}{2} \Big(f\big(x(t_k),u(t_k),t_k\big) + f\big(x(t_{k+1}),u(t_{k+1}),t_{k+1}\big)\Big). \]

Expanding \( x(t_{k+1}) \) and \( f\big(x(t_{k+1}),u(t_{k+1}),t_{k+1}\big) \) in a Taylor series around \( t_k \) and using the ODE to substitute derivatives yields

\[ \|\tau_k\| \leq C h^3, \]

for some constant \( C \) depending on bounds of higher derivatives. Standard numerical analysis arguments then show that the global discretization error in the state trajectory is \( \mathcal{O}(h^2) \), and under mild regularity conditions the discrete optimal solution converges to the continuous-time optimum as \( h \to 0 \).

Hermite–Simpson collocation corresponds to a higher-order Runge–Kutta scheme and yields local truncation error of order \( \mathcal{O}(h^5) \), hence global error \( \mathcal{O}(h^4) \). This higher accuracy can significantly reduce the grid size needed for precise manipulator trajectories.

From an optimization perspective, the KKT conditions of the collocation NLP approximate the continuous-time first-order optimality conditions of the OCP, with the discrete multipliers playing the role of co-states.

For direct collocation with uniform grid and \( n_x \) states, \( n_u \) controls, and \( N \) intervals:

\[ n_z^{\mathrm{coll}} = (N+1)n_x + N n_u \]

\[ m^{\mathrm{coll}} = N n_x + n_{\text{bnd}}, \]

where \( n_{\text{bnd}} \) counts boundary constraints. The Jacobian of \( c_h(z) \) exhibits a banded sparse structure: each collocation constraint couples only \( x_k,x_{k+1},u_k,u_{k+1} \). Modern solvers (e.g. SNOPT, IPOPT, WORHP) exploit this sparsity to handle trajectories for high-DOF manipulators.

In contrast, single shooting has a much smaller decision vector, \( n_z^{\mathrm{ss}} = N n_u \), but the Jacobian of the terminal constraints is typically dense and can be badly conditioned. Multiple shooting and collocation thus trade more variables for more favorable sparsity and conditioning.

Proper scaling of states, controls, and time (for example, normalizing joint angles and torques to order one) is crucial. Poor scaling can destroy the advantages of sparsity by making the linear systems in KKT solvers numerically unstable.

We illustrate trapezoidal collocation on a simple double integrator model \( x = (p,v) \) with dynamics \( \dot{p} = v \), \( \dot{v} = u \). The goal is to drive the system from \( (p(0),v(0)) = (0,0) \) to \( (p(T),v(T)) = (1,0) \) with minimal quadratic control effort.

import numpy as np
from scipy.optimize import minimize

# Problem parameters
N = 40          # number of intervals
T = 1.0
h = T / N
n_x = 2
n_u = 1

def unpack(z):
    """Split decision vector into X (states) and U (controls)."""
    X_flat = z[: (N + 1) * n_x]
    U_flat = z[(N + 1) * n_x :]
    X = X_flat.reshape((N + 1, n_x))
    U = U_flat.reshape((N, n_u))
    return X, U

def objective(z):
    X, U = unpack(z)
    # Integral approximation J = sum h * u^2
    return h * np.sum(U[:, 0] ** 2)

def constraints(z):
    X, U = unpack(z)
    cons = []

    # Initial state constraints: x_0 = (0, 0)
    cons.extend(X[0] - np.array([0.0, 0.0]))

    # Terminal state constraints: x_N = (1, 0)
    cons.extend(X[-1] - np.array([1.0, 0.0]))

    # Trapezoidal collocation constraints
    for k in range(N):
        xk = X[k]
        xkp1 = X[k + 1]
        uk = U[k, 0]

        # Double integrator dynamics: dot p = v, dot v = u
        fk = np.array([xk[1], uk])
        # piecewise constant control, approximate f at k+1 with same uk
        fkp1 = np.array([xkp1[1], uk])

        cons.extend(xkp1 - xk - 0.5 * h * (fk + fkp1))

    return np.array(cons)

# Initial guess: straight-line in position, zero velocity and control
z0 = np.zeros((N + 1) * n_x + N * n_u)
X0 = np.zeros((N + 1, n_x))
for k in range(N + 1):
    t = k * h
    X0[k, 0] = t
z0[: (N + 1) * n_x] = X0.reshape(-1)

con_dict = {
    "type": "eq",
    "fun": constraints,
}

res = minimize(
    objective,
    z0,
    method="SLSQP",
    constraints=[con_dict],
    options={"maxiter": 200, "ftol": 1e-6},
)

X_opt, U_opt = unpack(res.x)
print("Optimization success:", res.success)
print("Terminal state:", X_opt[-1])
print("First few controls:", U_opt[:5, 0])
      

In practice, robotics toolkits such as pinocchio, drake, or casadi-based frameworks are used to compute dynamics \( f(x,u) \) and their derivatives, which plug directly into the above structure.

In C++, robotics trajectory optimization is frequently implemented using Eigen for linear algebra and Ipopt or similar solvers for sparse NLP. Below is a schematic implementation of trapezoidal collocation inside an Ipopt::TNLP subclass for the same double integrator.

#include <IpIpoptApplication.hpp>
#include <IpTNLP.hpp>
#include <Eigen/Dense>

using Eigen::VectorXd;

class DoubleIntegratorCollocationNLP : public Ipopt::TNLP {
public:
    DoubleIntegratorCollocationNLP(int N_in, double T_in)
    : N(N_in), T(T_in), h(T_in / N_in), n_x(2), n_u(1) {}

    virtual bool get_nlp_info(
        Ipopt::Index& n,
        Ipopt::Index& m,
        Ipopt::Index& nnz_jac_g,
        Ipopt::Index& nnz_h_lag,
        IndexStyleEnum& index_style)
    {
        n = (N + 1) * n_x + N * n_u;     // decision variables
        m = 2 * n_x + N * n_x;           // initial, terminal, collocation
        nnz_jac_g = m * n;               // dense for simplicity (sparse in practice)
        nnz_h_lag = 0;                   // let Ipopt approximate Hessian
        index_style = TNLP::C_STYLE;
        return true;
    }

    virtual bool eval_f(
        Ipopt::Index n,
        const Ipopt::Number* x,
        bool new_x,
        Ipopt::Number& obj_value)
    {
        obj_value = 0.0;
        int offset_u = (N + 1) * n_x;
        for (int k = 0; k < N; ++k) {
            double uk = x[offset_u + k * n_u];
            obj_value += h * uk * uk;
        }
        return true;
    }

    virtual bool eval_g(
        Ipopt::Index n,
        const Ipopt::Number* x,
        bool new_x,
        Ipopt::Index m,
        Ipopt::Number* g)
    {
        int row = 0;

        // Initial state: x_0 = (0, 0)
        for (int i = 0; i < n_x; ++i) {
            g[row++] = x[i] - 0.0;
        }

        // Terminal state: x_N = (1, 0)
        int idx_final = N * n_x;
        g[row++] = x[idx_final] - 1.0;     // position
        g[row++] = x[idx_final + 1] - 0.0; // velocity

        int offset_u = (N + 1) * n_x;

        // Trapezoidal collocation constraints
        for (int k = 0; k < N; ++k) {
            int idx_xk = k * n_x;
            int idx_xkp1 = (k + 1) * n_x;
            double uk = x[offset_u + k * n_u];

            double pk = x[idx_xk];
            double vk = x[idx_xk + 1];
            double pkp1 = x[idx_xkp1];
            double vkp1 = x[idx_xkp1 + 1];

            double fk_p = vk;
            double fk_v = uk;
            double fkp1_p = vkp1;
            double fkp1_v = uk;

            g[row++] = pkp1 - pk - 0.5 * h * (fk_p + fkp1_p);
            g[row++] = vkp1 - vk - 0.5 * h * (fk_v + fkp1_v);
        }
        return true;
    }

    // Other virtual methods (bounds, Jacobian, etc.) omitted for brevity.

private:
    int N;
    int n_x;
    int n_u;
    double T;
    double h;
};

int main()
{
    Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
    Ipopt::SmartPtr<DoubleIntegratorCollocationNLP> nlp =
        new DoubleIntegratorCollocationNLP(40, 1.0);

    app->Initialize();
    app->OptimizeTNLP(nlp);
    return 0;
}
      

For high-DOF robots, one typically replaces the hand-coded double integrator dynamics with calls into a rigid-body dynamics library and provides sparse Jacobian/Hessian structures.

Java is less common in trajectory optimization for manipulators, but the same principles apply. Below we implement a crude finite-difference gradient descent for single shooting on the double integrator. In practice, you would plug the cost and constraint functions into an external optimization library (or a JVM binding to a C/C++ solver).

import java.util.Arrays;

public class DirectShootingDoubleIntegrator {

    private final int N;
    private final double T;
    private final double h;

    public DirectShootingDoubleIntegrator(int N, double T) {
        this.N = N;
        this.T = T;
        this.h = T / N;
    }

    // Simulate terminal position for a control sequence u
    private double simulateTerminal(double[] u) {
        double p = 0.0;
        double v = 0.0;
        for (int k = 0; k < N; ++k) {
            double uk = u[k];
            p = p + h * v;
            v = v + h * uk;
        }
        return p;
    }

    // Cost: integral of u^2 plus terminal penalty
    private double cost(double[] u) {
        double J = 0.0;
        for (int k = 0; k < N; ++k) {
            J += h * u[k] * u[k];
        }
        double pT = simulateTerminal(u);
        double terminalPenalty = 1000.0 * (pT - 1.0) * (pT - 1.0);
        return J + terminalPenalty;
    }

    // Very simple gradient descent with finite differences
    public double[] gradientDescent(int maxIter, double alpha) {
        double[] u = new double[N];
        Arrays.fill(u, 0.0);
        double eps = 1e-6;

        for (int it = 0; it < maxIter; ++it) {
            double J = cost(u);
            double[] grad = new double[N];

            for (int k = 0; k < N; ++k) {
                double delta = 1e-5;
                double old = u[k];

                u[k] = old + delta;
                double Jp = cost(u);

                u[k] = old - delta;
                double Jm = cost(u);

                u[k] = old;
                grad[k] = (Jp - Jm) / (2.0 * delta);
            }

            double norm = 0.0;
            for (int k = 0; k < N; ++k) {
                norm += grad[k] * grad[k];
            }
            norm = Math.sqrt(norm);
            if (norm < eps) {
                break;
            }

            for (int k = 0; k < N; ++k) {
                u[k] -= alpha * grad[k];
            }
            System.out.println("iter " + it + " cost " + J);
        }
        return u;
    }

    public static void main(String[] args) {
        DirectShootingDoubleIntegrator solver =
            new DirectShootingDoubleIntegrator(40, 1.0);
        double[] u = solver.gradientDescent(200, 1e-2);
        System.out.println("First few controls:");
        for (int k = 0; k < 5; ++k) {
            System.out.println(u[k]);
        }
    }
}
      

For robotic systems, this pattern is generalized by replacing the double integrator with forward dynamics of the robot and shaping the terminal penalty and path costs according to the task (e.g. end-effector pose tracking).

MATLAB's fmincon can solve the collocation NLP, while Simulink and Simscape Multibody provide simulation and visualization for the resulting trajectory. Below is a script for the same double integrator example using trapezoidal collocation.

% Direct collocation for double integrator using fmincon
N   = 40;
T   = 1.0;
h   = T / N;
n_x = 2;
n_u = 1;

% Decision vector: [X(0)...X(N), U(0)...U(N-1)]
z0 = zeros((N + 1) * n_x + N * n_u, 1);

% Initial guess: straight-line position
X0 = zeros(N + 1, n_x);
for k = 1:(N + 1)
    t = (k - 1) * h;
    X0(k, 1) = t;
end
z0(1:(N + 1) * n_x) = X0(:);

% Objective
function J = objective(z, N, h, n_x, n_u)
    X = reshape(z(1:(N + 1) * n_x), N + 1, n_x);
    U = reshape(z((N + 1) * n_x + 1:end), N, n_u);
    J = 0.0;
    for k = 1:N
        uk = U(k, 1);
        J = J + h * uk * uk;
    end
end

% Nonlinear equality constraints: boundary + collocation
function [c, ceq] = constraints(z, N, h, n_x, n_u)
    X = reshape(z(1:(N + 1) * n_x), N + 1, n_x);
    U = reshape(z((N + 1) * n_x + 1:end), N, n_u);

    ceq = [];
    % initial state
    ceq = [ceq;
           X(1, 1) - 0.0;
           X(1, 2) - 0.0];
    % terminal state
    ceq = [ceq;
           X(end, 1) - 1.0;
           X(end, 2) - 0.0];

    % trapezoidal collocation
    for k = 1:N
        pk   = X(k, 1);
        vk   = X(k, 2);
        pkp1 = X(k + 1, 1);
        vkp1 = X(k + 1, 2);
        uk   = U(k, 1);

        fk_p   = vk;
        fk_v   = uk;
        fkp1_p = vkp1;
        fkp1_v = uk;

        ceq = [ceq;
               pkp1 - pk - 0.5 * h * (fk_p + fkp1_p);
               vkp1 - vk - 0.5 * h * (fk_v + fkp1_v)];
    end
    c = [];
end

options = optimoptions("fmincon", ...
    "Algorithm", "sqp", ...
    "MaxIterations", 200, ...
    "Display", "iter");

problem.objective = @(z) objective(z, N, h, n_x, n_u);
problem.x0       = z0;
problem.nonlcon  = @(z) constraints(z, N, h, n_x, n_u);
problem.lb       = [];
problem.ub       = [];
problem.options  = options;
problem.solver   = "fmincon";

[z_opt, J_opt] = fmincon(problem);

X_opt = reshape(z_opt(1:(N + 1) * n_x), N + 1, n_x);
U_opt = reshape(z_opt((N + 1) * n_x + 1:end), N, n_u);

t_grid = linspace(0, T, N + 1);
figure;
subplot(2, 1, 1);
plot(t_grid, X_opt(:, 1), "-o");
xlabel("t"); ylabel("p");
subplot(2, 1, 2);
stairs(t_grid(1:end-1), U_opt(:, 1));
xlabel("t"); ylabel("u");
      

In Simulink, one can feed the optimized control sequence into a discrete-time double integrator block (or a full rigid-body plant model) and verify tracking and robustness properties under modeling errors.

Mathematica provides symbolic and numeric tools that fit naturally with direct collocation. We again solve the double integrator problem with trapezoidal collocation, using NMinimize with equality constraints.

(* Direct collocation for double integrator in Mathematica *)
N  = 20;
T  = 1.0;
h  = T/N;

xVars = Table[Array[x[k, #] &, 2], {k, 0, N}];
uVars = Table[u[k], {k, 0, N - 1}];

vars = Flatten[{xVars, uVars}];

cost = h*Sum[u[k]^2, {k, 0, N - 1}];

dynConstraints = Flatten@Table[
   Module[{pk, vk, pkp1, vkp1, uk, fk, fkp1},
    pk   = x[k, 1];
    vk   = x[k, 2];
    pkp1 = x[k + 1, 1];
    vkp1 = x[k + 1, 2];
    uk   = u[k];
    fk   = {vk, uk};
    fkp1 = {vkp1, uk};
    {
     pkp1 - pk - 0.5*h*(fk[[1]] + fkp1[[1]]),
     vkp1 - vk - 0.5*h*(fk[[2]] + fkp1[[2]])
     }
    ],
   {k, 0, N - 1}
   ];

bndConstraints = {
   x[0, 1] == 0.0,
   x[0, 2] == 0.0,
   x[N, 1] == 1.0,
   x[N, 2] == 0.0
   };

allEq = Join[bndConstraints, Thread[dynConstraints == 0]];

sol = NMinimize[{cost, allEq}, vars];

solVars = vars /. Last[sol];

ListLinePlot[
 Table[{k*h, (x[k, 1] /. solVars)}, {k, 0, N}],
 AxesLabel -> {"t", "p"},
 PlotMarkers -> Automatic
 ]
      

For more complex robotic systems (e.g. manipulator dynamics derived symbolically), the above pattern extends naturally to high-dimensional state and control vectors.

Problem 1 (Local Truncation Error of Trapezoidal Collocation): Consider a scalar ODE \( \dot{x}(t) = f(x(t),t) \) with \( f \) smooth. Show that the local truncation error \( \tau_k \) of the trapezoidal collocation constraint

\[ x_{k+1} - x_k - \frac{h}{2}\Big(f(x_k,t_k) + f(x_{k+1},t_{k+1})\Big) = 0 \]

satisfies \( |\tau_k| \leq C h^3 \) for some constant \( C \), assuming the exact solution \( x(t) \) is three times continuously differentiable.

Solution: Substitute the exact solution evaluated at \( t_k \) and \( t_{k+1} \) into the collocation formula:

\[ \tau_k = x(t_{k+1}) - x(t_k) - \frac{h}{2} \Big(f(x(t_k),t_k) + f(x(t_{k+1}),t_{k+1})\Big). \]

Expand \( x(t_{k+1}) \) in a Taylor series around \( t_k \), using \( \dot{x}(t_k) = f(x(t_k),t_k) \) and similar expansions for \( f(x(t_{k+1}),t_{k+1}) \). Collecting terms shows that the leading non-vanishing term in \( \tau_k \) is proportional to \( h^3 x^{(3)}(\xi) \) for some \( \xi \in (t_k,t_{k+1}) \), hence \( |\tau_k| \leq C h^3 \) for a suitable bound \( C \) on the third derivative.

Problem 2 (Conditioning of Single Shooting): Consider the scalar linear system \( \dot{x}(t) = a x(t) + b u(t) \) with constant control \( u(t) \equiv u \), horizon \( T \), and \( a > 0 \). Suppose the decision variable is \( u \) and the terminal constraint is \( x(T;u) = x_T \). Compute \( \partial x(T;u)/\partial u \) and discuss how its magnitude scales with \( T \).

Solution: Solving the ODE gives

\[ x(T;u) = \mathrm{e}^{a T} x_0 + \int_0^T \mathrm{e}^{a (T-s)} b u \,\mathrm{d}s = \mathrm{e}^{a T} x_0 + b u \frac{\mathrm{e}^{a T} - 1}{a}. \]

Differentiating with respect to \( u \) yields

\[ \frac{\partial x(T;u)}{\partial u} = b \frac{\mathrm{e}^{a T} - 1}{a}. \]

For large \( T \), this grows like \( \mathcal{O}(\mathrm{e}^{a T}) \), which can make the Jacobian of the terminal constraint very large in magnitude. This illustrates the ill-conditioning of single shooting for unstable dynamics, motivating multiple shooting or collocation.

Problem 3 (Dimension of the NLP): Let a robotic system have \( n_x \) states and \( n_u \) controls, and use \( N \) time intervals. Compare the number of decision variables for: (a) single shooting, (b) multiple shooting with state variables at each grid point, and (c) collocation with state and control at each grid point.

Solution: For single shooting, controls are parameterized at each interval:

\[ n_z^{\mathrm{ss}} = N n_u. \]

For multiple shooting, states at each grid point are decision variables, together with controls:

\[ n_z^{\mathrm{ms}} = (N+1)n_x + N n_u. \]

For collocation with piecewise constant control per interval and state at each grid point, the count is the same:

\[ n_z^{\mathrm{coll}} = (N+1)n_x + N n_u. \]

Thus shooting methods use fewer variables but yield dense and potentially ill-conditioned Jacobians, whereas collocation and multiple shooting yield larger but structured sparse NLPs.

Problem 4 (Discrete Euler–Lagrange for Double Integrator): Consider the double integrator with discrete dynamics \( x_{k+1} = x_k + h f(x_k,u_k) \) (forward Euler) and cost

\[ J_h = \phi(x_N) + \sum_{k=0}^{N-1} h\,\ell(x_k,u_k). \]

Derive the discrete optimality conditions with Lagrange multipliers \( \lambda_k \) enforcing the dynamics and show that they correspond to a backward recursion on \( \lambda_k \).

Solution: Form the Lagrangian

\[ \mathcal{L} = \phi(x_N) + \sum_{k=0}^{N-1} \Big( h\,\ell(x_k,u_k) + \lambda_{k+1}^{\top}\big(x_{k+1} - x_k - h f(x_k,u_k)\big) \Big). \]

Stationarity with respect to \( x_k \) for \( k = 1,\dots,N-1 \) yields

\[ 0 = h \frac{\partial \ell(x_k,u_k)}{\partial x_k} - \lambda_{k+1} + \lambda_k - h \Big( \frac{\partial f(x_k,u_k)}{\partial x_k} \Big)^{\top} \lambda_{k+1}, \]

which can be rearranged as a backward recursion \( \lambda_k = \lambda_{k+1} - h\big(\partial \ell/\partial x_k + (\partial f/\partial x_k)^{\top}\lambda_{k+1}\big) \), with terminal condition \( \lambda_N = \partial \phi(x_N)/\partial x_N \). Stationarity in \( u_k \) gives the discrete analog of the optimality condition for the Hamiltonian. These discrete relations approximate the continuous-time adjoint equations.

Problem 5 (Method Selection Flow): For a manipulator planning problem, you need to choose among single shooting, multiple shooting, and direct collocation. Describe a qualitative decision flow based on horizon length, stiffness of dynamics, and the need for path constraints.

Solution (flow): A simple decision flow is:

flowchart TD
  S["Start"] --> H["Long horizon or \nstiff dynamics?"]
  H -->|yes| C["Prefer collocation \n(high accuracy, sparse NLP)"]
  H -->|no| G1["Are strong path \nconstraints important?"]
  G1 -->|yes| C2["Multiple shooting \nor collocation"]
  G1 -->|no| SS["Single shooting \nis acceptable"]
  C --> OUT["Formulate sparse \nconstrained NLP"]
  C2 --> OUT
  SS --> OUT
        

For high-DOF manipulators with torque and collision constraints along the entire trajectory, collocation or multiple shooting are usually preferred; single shooting is mainly used for simpler systems with short horizons or when reusing an existing simulator is a priority.

We formulated trajectory optimization for robotic systems as continuous-time OCPs and introduced direct transcription methods that convert them into sparse NLPs. Single shooting parameterizes only controls and integrates dynamics, while multiple shooting adds state variables at grid points to improve conditioning. Direct collocation enforces dynamics via quadrature-based constraints and yields high-order accurate approximations whose KKT conditions converge to the continuous-time optimality conditions as the mesh is refined. Implementation patterns in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrate how these methods are realized in practice and provide a foundation for subsequent lessons on collision handling and smoothness constraints in trajectory optimization.