In motion planning, a trajectory is a time-parameterized curve in configuration or state space, typically denoted by \( \mathbf{q}(t) \) (joint-space) or \( \mathbf{x}(t) \) (state-space). Trajectory optimization treats the entire function \( t \mapsto \mathbf{x}(t), \mathbf{u}(t) \) as a decision variable and searches for the best trajectory according to a cost functional, subject to dynamics and constraints.

For an \(n\)-DOF manipulator, we define the state and control as

\[ \mathbf{x}(t) = \begin{bmatrix} \mathbf{q}(t) \\ \dot{\mathbf{q}}(t) \end{bmatrix} \in \mathbb{R}^{2n}, \qquad \mathbf{u}(t) \in \mathbb{R}^{m}, \]

where \( \mathbf{q}(t) \) are joint coordinates and \( \mathbf{u}(t) \) are control inputs (e.g., torques or commanded velocities). The goal of trajectory optimization is to choose \( \mathbf{x}(\cdot) \) and \( \mathbf{u}(\cdot) \) that minimize a cost functional while satisfying:

• Robot dynamics (from the kinematics and dynamics course you already know).
• Boundary conditions (initial and terminal states/poses).
• Path constraints (joint limits, torque bounds, collision avoidance, etc.).

flowchart TD
  A["Task specification: start, goal, environment"] --> B["Choose state x(t) and control u(t)"]
  B --> C["Write robot dynamics: x_dot = f(x,u)"]
  C --> D["Define cost functional J[x(.), u(.)]"]
  D --> E["Discretize time grid and parameterize trajectory"]
  E --> F["Form finite-dimensional optimization problem"]
  F --> G["Call numerical solver"]
  G --> H["Feasible, (locally) optimal trajectory"]
        

From classical robotics, an \(n\)-DOF rigid manipulator obeys the joint-space dynamics

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{u} + \mathbf{J}(\mathbf{q})^{\top}\mathbf{f}_{c}, \]

where \( \mathbf{M}(\mathbf{q}) \) is the mass matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) captures Coriolis/centrifugal effects, \( \mathbf{g}(\mathbf{q}) \) gravity, and \( \mathbf{J}(\mathbf{q})^{\top}\mathbf{f}_{c} \) the effect of contact forces \( \mathbf{f}_{c} \). For simplicity in this lesson we often consider contact-free motion with \( \mathbf{f}_{c} = \mathbf{0} \).

Writing this second-order system as a first-order ODE in the state \( \mathbf{x}(t) = [\mathbf{q}(t)^{\top}, \dot{\mathbf{q}}(t)^{\top}]^{\top} \) gives:

\[ \dot{\mathbf{x}}(t) = \begin{bmatrix} \dot{\mathbf{q}}(t) \\ \mathbf{M}(\mathbf{q}(t))^{-1} \bigl(\mathbf{u}(t) - \mathbf{C}(\mathbf{q}(t),\dot{\mathbf{q}}(t))\dot{\mathbf{q}}(t) - \mathbf{g}(\mathbf{q}(t))\bigr) \end{bmatrix} = \mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)). \]

In trajectory optimization, these dynamics become equality constraints that must hold for all \( t \) (in continuous time) or all time steps \( k \) (after discretization).

A basic fixed-horizon trajectory optimization problem for a robot can be written as:

\[ \begin{aligned} \min_{\mathbf{x}(\cdot),\mathbf{u}(\cdot)} \quad & J(\mathbf{x}(\cdot),\mathbf{u}(\cdot)) := \phi(\mathbf{x}(T)) + \int_{0}^{T} \ell\bigl(\mathbf{x}(t),\mathbf{u}(t),t\bigr)\,\mathrm{d}t \\ \text{s.t.} \quad & \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)), \quad 0 < t < T, \\ & \mathbf{c}\bigl(\mathbf{x}(t),\mathbf{u}(t),t\bigr) = \mathbf{0}, \quad 0 < t < T, \\ & \mathbf{d}\bigl(\mathbf{x}(t),\mathbf{u}(t),t\bigr) \leq \mathbf{0}, \quad 0 < t < T, \\ & \mathbf{x}(0) = \mathbf{x}_{0}, \\ & \mathbf{\psi}\bigl(\mathbf{x}(T)\bigr) = \mathbf{0}, \quad \mathbf{\eta}\bigl(\mathbf{x}(T)\bigr) \leq \mathbf{0}. \end{aligned} \]

Here:

• \( \ell \) is a running cost (e.g., control effort, smoothness, proximity to obstacles).
• \( \phi \) is a terminal cost (e.g., distance to goal configuration).
• \( \mathbf{c} \) are additional equality path constraints (e.g., closed kinematic chains).
• \( \mathbf{d} \) are inequality path constraints (e.g., joint limits, distance to obstacles).
• \( \mathbf{\psi}, \mathbf{\eta} \) encode terminal constraints, such as reaching a desired pose set.

In many robotic problems, we restrict controls and states to admissible sets \( \mathbf{u}(t) \in \mathcal{U} \), \( \mathbf{x}(t) \in \mathcal{X} \), but for brevity we absorb such bounds into \( \mathbf{d} \).

Typical cost terms in robotic trajectory optimization reflect smoothness, control effort, and safety margins. A common quadratic cost combines tracking, effort, and acceleration:

\[ \ell(\mathbf{x}(t),\mathbf{u}(t),t) = \tfrac{1}{2} \bigl\| \mathbf{q}(t) - \mathbf{q}_{\text{ref}}(t) \bigr\|_{\mathbf{Q}}^{2} + \tfrac{1}{2} \bigl\|\mathbf{u}(t)\bigr\|_{\mathbf{R}}^{2} + \tfrac{1}{2} \bigl\|\ddot{\mathbf{q}}(t)\bigr\|_{\mathbf{S}}^{2}, \]

where \( \mathbf{Q}, \mathbf{R}, \mathbf{S} \) are positive semi-definite weighting matrices. The acceleration term promotes smooth motion without jerk. Collision avoidance can be modeled via “soft” penalty terms that grow when the robot approaches obstacles, e.g.

\[ \ell_{\text{obs}}(\mathbf{x}(t)) = \sum_{o} \rho\Bigl(d_{\text{safe}} - d_{o}(\mathbf{x}(t))\Bigr), \]

where \( d_{o}(\mathbf{x}) \) is the signed distance from the robot to obstacle \(o\), \( d_{\text{safe}} \) is a safety margin, and \( \rho(\cdot) \) is a convex penalty that is zero when the argument is non-positive and increases smoothly when the robot penetrates the safety region.

Numerical solvers do not operate directly on infinite-dimensional function spaces, so we discretize time. Let \( 0 = t_{0} < t_{1} < \dots < t_{N} = T \) be a grid with step size \( h = T/N \). We introduce decision variables \( \mathbf{x}_{k} \approx \mathbf{x}(t_{k}) \) and \( \mathbf{u}_{k} \approx \mathbf{u}(t_{k}) \) and approximate the dynamics by, e.g., forward Euler:

\[ \mathbf{x}_{k+1} = \mathbf{x}_{k} + h\, \mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k}), \quad k = 0,\dots,N-1. \]

These become algebraic equality constraints of the form

\[ \mathbf{g}_{k}(\mathbf{x}_{k},\mathbf{x}_{k+1},\mathbf{u}_{k}) := \mathbf{x}_{k+1} - \mathbf{x}_{k} - h\, \mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k}) = \mathbf{0}. \]

Integrals are approximated by numerical quadrature, such as the rectangular rule:

\[ J \approx \phi(\mathbf{x}_{N}) + h \sum_{k=0}^{N-1} \ell(\mathbf{x}_{k},\mathbf{u}_{k},t_{k}). \]

Collect all decision variables into a single vector \( \mathbf{z} \in \mathbb{R}^{n_{z}} \), e.g. \( \mathbf{z} = (\mathbf{x}_{0},\dots,\mathbf{x}_{N},\mathbf{u}_{0},\dots,\mathbf{u}_{N-1}) \). The discretized trajectory optimization problem becomes a finite-dimensional nonlinear program (NLP):

\[ \begin{aligned} \min_{\mathbf{z}} \quad & J(\mathbf{z}) \\ \text{s.t.} \quad & \mathbf{g}(\mathbf{z}) = \mathbf{0}, \\ & \mathbf{h}(\mathbf{z}) \leq \mathbf{0}, \\ & \mathbf{z}^{\text{L}} \leq \mathbf{z} \leq \mathbf{z}^{\text{U}}, \end{aligned} \]

where \( \mathbf{g} \) stacks dynamics and boundary constraints, \( \mathbf{h} \) stacks inequality constraints, and \( \mathbf{z}^{\text{L}}, \mathbf{z}^{\text{U}} \) are simple bounds. The Jacobian and Hessian matrices of this NLP have a sparse, banded structure because each dynamic constraint only couples neighboring time steps.

flowchart TD
  Z["Decision vector z = (x_0,...,x_N,u_0,...,u_{N-1})"] --> G0["Dynamics equality constraints between k and k+1"]
  G0 --> C0["Path and boundary constraints at each time step"]
  C0 --> OBJ["Cost as sum of per-step terms"]
  OBJ --> NLP["Sparse nonlinear program for a generic solver"]
        

A standard minimal example is the one-dimensional double integrator, representing motion of a point mass along a line with control as acceleration. The state and control are:

\[ \mathbf{x}(t) = \begin{bmatrix} p(t) \\ v(t) \end{bmatrix}, \quad \mathbf{u}(t) = a(t), \quad \dot{\mathbf{x}}(t) = \begin{bmatrix} v(t) \\ a(t) \end{bmatrix}. \]

Consider the minimum-effort problem:

\[ \begin{aligned} \min_{\mathbf{x}(\cdot),\mathbf{u}(\cdot)} \quad & \tfrac{1}{2}\int_{0}^{T} a(t)^{2}\,\mathrm{d}t \\ \text{s.t.} \quad & \dot{p}(t) = v(t), \\ & \dot{v}(t) = a(t), \\ & \mathbf{x}(0) = \begin{bmatrix} p_{0} \\ v_{0} \end{bmatrix}, \quad \mathbf{x}(T) = \begin{bmatrix} p_{f} \\ v_{f} \end{bmatrix}, \\ & |a(t)| \leq a_{\max}, \quad 0 \leq t \leq T. \end{aligned} \]

Discretizing with step size \( h \) and variables \( p_{k}, v_{k}, a_{k} \) yields:

\[ \begin{aligned} & p_{k+1} = p_{k} + h v_{k}, \\ & v_{k+1} = v_{k} + h a_{k}, \\ & J \approx \tfrac{1}{2} h \sum_{k=0}^{N-1} a_{k}^{2}, \\ & p_{0} = p_{0}^{\text{given}}, \quad v_{0} = v_{0}^{\text{given}}, \\ & p_{N} = p_{f}, \quad v_{N} = v_{f}, \\ & -a_{\max} \leq a_{k} \leq a_{\max}. \end{aligned} \]

This simple structure is already representative of many robotic trajectory optimization problems, but with higher-dimensional states and more complex dynamics.

Python, together with casadi for symbolic/NLP modeling and packages like pinocchio or roboticstoolbox for dynamics, is widely used in trajectory optimization. Below is a minimal setup for the double integrator using CasADi.

import casadi as ca
import numpy as np

# Discretization
N = 40
T = 2.0
h = T / N

nx = 2  # [p, v]
nu = 1  # [a]

# Continuous-time dynamics f(x,u)
x = ca.MX.sym("x", nx)
u = ca.MX.sym("u", nu)

p = x[0]
v = x[1]
a = u[0]

f = ca.vertcat(v, a)  # x_dot = [v; a]

# Decision variables
X = ca.MX.sym("X", nx, N + 1)  # states over horizon
U = ca.MX.sym("U", nu, N)      # controls over horizon

w   = []  # stacked decision vars
lbw = []  # lower bounds
ubw = []  # upper bounds
g   = []  # constraint expressions
lbg = []  # lower bounds on constraints
ubg = []  # upper bounds on constraints

J = 0  # objective

# Boundary conditions
p0 = 0.0
v0 = 0.0
pf = 1.0
vf = 0.0
amax = 2.0

# Initial state fixed
w.append(X[:, 0])
lbw += [p0, v0]
ubw += [p0, v0]

# Loop over horizon
for k in range(N):
    # Control at step k
    w.append(U[:, k])
    lbw += [-amax]
    ubw += [ amax]

    # Cost: 0.5 * a^2 * h
    J = J + 0.5 * h * ca.mtimes(U[:, k].T, U[:, k])

    # State at step k
    xk = X[:, k]

    # Integrate dynamics with forward Euler
    x_next = xk + h * f.subs({x: xk, u: U[:, k]})

    # Add next-state variable
    w.append(X[:, k + 1])
    # No bound yet; we will set for final state after the loop
    lbw += [-ca.inf, -ca.inf]
    ubw += [ ca.inf,  ca.inf]

    # Dynamics equality constraint
    g.append(X[:, k + 1] - x_next)
    lbg += [0.0, 0.0]
    ubg += [0.0, 0.0]

# Terminal state fixed
lbw[-2] = pf
ubw[-2] = pf
lbw[-1] = vf
ubw[-1] = vf

# Stack decision variables and constraints
w = ca.vertcat(*[ca.reshape(wi, -1, 1) for wi in w])
g = ca.vertcat(*g)

prob = {"f": J, "x": w, "g": g}
solver = ca.nlpsol("solver", "ipopt", prob,
                   {"ipopt.print_level": 0, "print_time": 0})

sol = solver(lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
w_opt = sol["x"].full().flatten()

print("Optimal cost:", float(sol["f"]))
      

For a full manipulator, one would call a dynamics function produced by a robotics library (e.g., pinocchio or roboticstoolbox) inside f, and add additional constraints (joint limits, collision margins) based on forward kinematics and distance queries.

In C++, it is common to use Eigen for linear algebra and libraries such as Pinocchio or KDL for rigid-body dynamics. Below is a minimal structural example for the double integrator problem setup; the actual connection to an NLP solver like IPOPT or SNOPT is omitted, but the interface is designed with that in mind.

#include <Eigen/Dense>
#include <vector>
#include <cmath>

struct TrajectoryOptimizationProblem {
  int N;
  double T;
  double h;

  TrajectoryOptimizationProblem(int N_, double T_)
      : N(N_), T(T_), h(T_ / N_) {}

  // x = [p, v], u = [a]
  Eigen::Vector2d dynamics(const Eigen::Vector2d& x,
                           double u) const {
    Eigen::Vector2d xdot;
    xdot(0) = x(1);
    xdot(1) = u;
    return xdot;
  }

  // Decision vector w layout:
  // [x0 (2), u0 (1), x1 (2), u1 (1), ..., u_{N-1} (1), xN (2)]
  // This helper extracts state and control at step k.
  void getIndex(int k, int& idx_xk, int& idx_uk) const {
    // x0 at index 0
    // pattern: x0, u0, x1, u1, ...
    idx_xk = 3 * k;      // each stage adds 3 variables: 2 (x) + 1 (u)
    idx_uk = idx_xk + 2;
  }

  double cost(const Eigen::VectorXd& w) const {
    double J = 0.0;
    for (int k = 0; k < N; ++k) {
      int idx_xk, idx_uk;
      getIndex(k, idx_xk, idx_uk);
      double ak = w(idx_uk);
      J += 0.5 * h * ak * ak;
    }
    return J;
  }

  // Computes dynamic constraint residuals: x_{k+1} - (x_k + h * f(x_k, u_k))
  void dynamicsResiduals(const Eigen::VectorXd& w,
                         Eigen::VectorXd& g) const {
    g.setZero(2 * N);
    for (int k = 0; k < N; ++k) {
      int idx_xk, idx_uk;
      getIndex(k, idx_xk, idx_uk);
      Eigen::Vector2d xk = w.segment<2>(idx_xk);
      double uk = w(idx_uk);
      Eigen::Vector2d xkp1 = w.segment<2>(idx_xk + 3);  // x_{k+1}
      Eigen::Vector2d x_next_euler = xk + h * dynamics(xk, uk);
      g.segment<2>(2 * k) = xkp1 - x_next_euler;
    }
  }
};

int main() {
  int N = 40;
  double T = 2.0;
  TrajectoryOptimizationProblem prob(N, T);

  // Example: allocate decision vector w with some initial guess.
  Eigen::VectorXd w(3 * N + 2);
  w.setZero();

  // Set initial state and final state through bounds in the solver.
  // Those bounds are not shown here, but conceptually they are:
  // x0 = [p0, v0], xN = [pf, vf].

  // Evaluate cost and dynamics residuals for debugging
  double J = prob.cost(w);
  Eigen::VectorXd g(2 * N);
  prob.dynamicsResiduals(w, g);

  return 0;
}
      

For a full manipulator, the method dynamics would call a dynamics library (e.g., Pinocchio) to compute joint accelerations from torques and then construct a first-order state update as in Section 2.

Java is sometimes used in industrial control stacks or educational simulators. Here we illustrate a simple array-based representation of the same double integrator problem. A numerical optimizer (e.g., a Java port of an SQP method) would call cost and dynamicsResiduals.

public class DoubleIntegratorTrajOpt {

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

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

    // Layout: w = [x0(2), u0(1), x1(2), u1(1), ..., u_{N-1}(1), xN(2)]
    private int idxX(int k) {
        return 3 * k;
    }

    private int idxU(int k) {
        return 3 * k + 2;
    }

    public double cost(double[] w) {
        double J = 0.0;
        for (int k = 0; k < N; ++k) {
            int iu = idxU(k);
            double ak = w[iu];
            J += 0.5 * h * ak * ak;
        }
        return J;
    }

    public void dynamicsResiduals(double[] w, double[] g) {
        // g has length 2 * N
        for (int k = 0; k < N; ++k) {
            int ix = idxX(k);
            int iu = idxU(k);

            double pk = w[ix];
            double vk = w[ix + 1];
            double ak = w[iu];

            double pkp1 = w[ix + 3];
            double vkp1 = w[ix + 4];

            double pNext = pk + h * vk;
            double vNext = vk + h * ak;

            g[2 * k]     = pkp1 - pNext;
            g[2 * k + 1] = vkp1 - vNext;
        }
    }

    public static void main(String[] args) {
        int N = 40;
        double T = 2.0;
        DoubleIntegratorTrajOpt problem = new DoubleIntegratorTrajOpt(N, T);

        double[] w = new double[3 * N + 2];
        double[] g = new double[2 * N];

        double J = problem.cost(w);
        problem.dynamicsResiduals(w, g);

        System.out.println("Initial cost: " + J);
    }
}
      

In Java-based robotic systems, linear algebra libraries such as EJML or Apache Commons Math can be used to handle higher-dimensional states and to interface with external optimization backends.

10.1 MATLAB Script with fmincon

In MATLAB, the double integrator problem can be posed as a nonlinear program using fmincon. Robotics-oriented problems can additionally use Robotics System Toolbox for kinematics/dynamics and Simulink for simulation.

function traj_opt_double_integrator
    N = 40;
    T = 2.0;
    h = T / N;

    p0 = 0; v0 = 0;
    pf = 1; vf = 0;
    amax = 2.0;

    % Decision vector w: [p0 v0 a0 p1 v1 a1 ... a_{N-1} pN vN]'
    nz = 3 * N + 2;

    w0 = zeros(nz, 1);   % initial guess
    lb = -inf(nz, 1);
    ub = inf(nz, 1);

    % Fix initial state
    lb(1:2) = [p0; v0];
    ub(1:2) = [p0; v0];

    % Fix final state
    lb(end-1:end) = [pf; vf];
    ub(end-1:end) = [pf; vf];

    % Acceleration bounds
    for k = 0:N-1
        iu = 3 * k + 3;
        lb(iu) = -amax;
        ub(iu) =  amax;
    end

    opts = optimoptions("fmincon", "Display", "iter", ...
        "Algorithm", "sqp");

    w_opt = fmincon(@(w) cost_fun(w, N, h), w0, [], [], [], [], ...
                    lb, ub, @(w) dyn_constraints(w, N, h), opts);

    % Extract trajectory
    p = w_opt(1:3:end);
    v = w_opt(2:3:end);
    t = linspace(0, T, N+1);
    plot(t, p); xlabel("t"); ylabel("p(t)"); grid on;
end

function J = cost_fun(w, N, h)
    J = 0;
    for k = 0:N-1
        iu = 3 * k + 3;
        ak = w(iu);
        J = J + 0.5 * h * ak^2;
    end
end

function [c, ceq] = dyn_constraints(w, N, h)
    ceq = zeros(2 * N, 1);
    for k = 0:N-1
        ix = 3 * k + 1;
        iu = 3 * k + 3;

        pk = w(ix);
        vk = w(ix + 1);
        ak = w(iu);

        pkp1 = w(ix + 3);
        vkp1 = w(ix + 4);

        pNext = pk + h * vk;
        vNext = vk + h * ak;

        ceq(2 * k + 1) = pkp1 - pNext;
        ceq(2 * k + 2) = vkp1 - vNext;
    end
    c = [];
end
      

In Simulink, one typically builds a continuous-time model of the robot and uses a custom block or MATLAB Function block to evaluate cost and constraints along a parameterized trajectory (e.g., piecewise polynomials) during optimization.

10.2 Mathematica Sketch with NMinimize

In Wolfram Mathematica, one can define a discretized problem similarly and call NMinimize:

N = 40;
T = 2.0;
h = T/N;

(* Variables: p[0..N], v[0..N], a[0..N-1] *)
vars = Flatten@Join[
    Array[p, 0 ;; N],
    Array[v, 0 ;; N],
    Array[a, 0 ;; N - 1]
];

(* Cost *)
J = (h/2) * Sum[a[k]^2, {k, 0, N - 1}];

(* Dynamics constraints *)
dynEqns = Flatten@Table[
    {
      p[k + 1] == p[k] + h v[k],
      v[k + 1] == v[k] + h a[k]
    },
    {k, 0, N - 1}
];

(* Boundary conditions *)
bcEqns = {
  p[0] == 0, v[0] == 0,
  p[N] == 1, v[N] == 0
};

(* Acceleration bounds *)
ineqs = Flatten@Table[
    {
      -2 <= a[k] <= 2
    },
    {k, 0, N - 1}
];

sol = NMinimize[
  {J, Join[dynEqns, bcEqns, ineqs]},
  vars
];

sol[[1]]
      

For more complex robotic systems, Mathematica can symbolically derive dynamics equations (via Lagrangian mechanics) and then discretize and optimize them similarly.

Problem 1 (Formulating a Manipulator Trajectory Optimization Problem). Consider an \(n\)-DOF manipulator with state \( \mathbf{x}(t) = [\mathbf{q}(t)^{\top}, \dot{\mathbf{q}}(t)^{\top}]^{\top} \), control \( \mathbf{u}(t) \), and dynamics \( \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)) \). You want to move from configuration \( \mathbf{q}_{0} \) with zero velocity to \( \mathbf{q}_{f} \) with zero velocity in time \( T \), minimizing the integral of squared torques while respecting joint and torque limits. Write down the continuous-time optimal control problem.

Solution: The state and control are as in the statement. We impose boundary and inequality constraints:

\[ \begin{aligned} \min_{\mathbf{x}(\cdot),\mathbf{u}(\cdot)} \quad & \tfrac{1}{2}\int_{0}^{T} \|\mathbf{u}(t)\|^{2}\,\mathrm{d}t \\ \text{s.t.} \quad & \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)), \quad 0 < t < T, \\ & \mathbf{x}(0) = \begin{bmatrix} \mathbf{q}_{0} \\ \mathbf{0} \end{bmatrix}, \quad \mathbf{x}(T) = \begin{bmatrix} \mathbf{q}_{f} \\ \mathbf{0} \end{bmatrix}, \\ & \mathbf{q}_{\min} \leq \mathbf{q}(t) \leq \mathbf{q}_{\max}, \\ & \mathbf{u}_{\min} \leq \mathbf{u}(t) \leq \mathbf{u}_{\max}, \quad 0 \leq t \leq T. \end{aligned} \]

This is precisely of the form described in Section 3 with quadratic running cost, dynamics constraints, box constraints on states and controls, and fixed boundary conditions.

Problem 2 (Forward Euler Discretization for a General System). Let \( \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t),\mathbf{u}(t)) \) with \( \mathbf{x} \in \mathbb{R}^{n} \), \( \mathbf{u} \in \mathbb{R}^{m} \). Using a uniform grid \( t_{k} = k h \), derive the discrete-time dynamics constraint used in direct transcription with forward Euler, and write it as an equality constraint function \( \mathbf{g}_{k}(\mathbf{x}_{k},\mathbf{x}_{k+1},\mathbf{u}_{k}) = \mathbf{0} \).

Solution: Integrating over a small interval \([t_{k}, t_{k+1}]\) we have formally \( \mathbf{x}(t_{k+1}) = \mathbf{x}(t_{k}) + \int_{t_{k}}^{t_{k+1}} \mathbf{f}(\mathbf{x}(s),\mathbf{u}(s))\,\mathrm{d}s \). Approximating the integral by \( h\,\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k}) \) yields

\[ \mathbf{x}_{k+1} \approx \mathbf{x}_{k} + h\,\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k}). \]

Moving all terms to the left, the equality constraint is

\[ \mathbf{g}_{k}(\mathbf{x}_{k},\mathbf{x}_{k+1},\mathbf{u}_{k}) := \mathbf{x}_{k+1} - \mathbf{x}_{k} - h\,\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k}) = \mathbf{0}. \]

Problem 3 (Smoothness Cost and Discrete Acceleration). For a scalar joint trajectory \( q(t) \) sampled at times \( t_{k} = k h \), define the discrete second difference \( \Delta^{2} q_{k} := q_{k+1} - 2 q_{k} + q_{k-1} \). Show that if \( q(t) \) is twice continuously differentiable and \( h \) is small, then \( \Delta^{2} q_{k} \approx h^{2} \ddot{q}(t_{k}) \). Conclude that the discrete cost \( J_{\text{disc}} = \sum_{k} \|\Delta^{2} q_{k}\|^{2} \) approximates \( \int_{0}^{T} \|\ddot{q}(t)\|^{2}\,\mathrm{d}t \) up to scaling.

Solution: By Taylor expansion around \( t_{k} \):

\[ \begin{aligned} q(t_{k+1}) &= q(t_{k}) + h \dot{q}(t_{k}) + \tfrac{1}{2} h^{2} \ddot{q}(t_{k}) + \mathcal{O}(h^{3}), \\ q(t_{k-1}) &= q(t_{k}) - h \dot{q}(t_{k}) + \tfrac{1}{2} h^{2} \ddot{q}(t_{k}) + \mathcal{O}(h^{3}). \end{aligned} \]

Substituting into \( \Delta^{2} q_{k} = q_{k+1} - 2 q_{k} + q_{k-1} \) gives

\[ \Delta^{2} q_{k} = \bigl(q(t_{k}) + h \dot{q}(t_{k}) + \tfrac{1}{2} h^{2} \ddot{q}(t_{k}) + \mathcal{O}(h^{3})\bigr) - 2 q(t_{k}) + \\ \bigl(q(t_{k}) - h \dot{q}(t_{k}) + \tfrac{1}{2} h^{2} \ddot{q}(t_{k}) + \mathcal{O}(h^{3})\bigr) = h^{2} \ddot{q}(t_{k}) + \mathcal{O}(h^{3}). \]

Thus, for small \( h \), we have \( \Delta^{2} q_{k} \approx h^{2} \ddot{q}(t_{k}) \), and \( \|\Delta^{2} q_{k}\|^{2} \approx h^{4} \|\ddot{q}(t_{k})\|^{2} \). Hence \( h \sum_{k} \|\Delta^{2} q_{k}\|^{2} \) is proportional to \( \int_{0}^{T} \|\ddot{q}(t)\|^{2}\,\mathrm{d}t \) up to constants, so discrete second-difference penalties approximate continuous acceleration penalties.

Problem 4 (Structure of the NLP Jacobian). Consider the discretized dynamics constraints \( \mathbf{g}_{k}(\mathbf{x}_{k},\mathbf{x}_{k+1},\mathbf{u}_{k}) = \mathbf{0} \) for \( k = 0,\dots,N-1 \). Argue that the Jacobian of all dynamic constraints with respect to all variables \( \mathbf{z} = (\mathbf{x}_{0},\dots,\mathbf{x}_{N},\mathbf{u}_{0},\dots,\mathbf{u}_{N-1}) \) has a block-banded sparsity pattern.

Solution: The constraint at time index \(k\) involves only the variables \( \mathbf{x}_{k} \), \( \mathbf{x}_{k+1} \), and \( \mathbf{u}_{k} \). Therefore, the partial derivatives \( \partial \mathbf{g}_{k} / \partial \mathbf{x}_{j} \) are nonzero only when \( j \in \{k, k+1\} \), and \( \partial \mathbf{g}_{k} / \partial \mathbf{u}_{j} \) is nonzero only when \( j = k \). If we order the variables as \( (\mathbf{x}_{0},\mathbf{u}_{0},\mathbf{x}_{1},\mathbf{u}_{1},\dots,\mathbf{x}_{N},\mathbf{u}_{N-1}) \), then each row block corresponding to \( \mathbf{g}_{k} \) touches only a small band of neighboring variable blocks.

This yields a block-banded structure, which is crucial for efficient sparse linear algebra in large-scale trajectory optimization.

Problem 5 (Decision Flow for Problem Setup). Sketch a conceptual flow for deciding how to set up a trajectory optimization problem for a new robotic task: how do you choose state/control variables, dynamics, cost, and constraints?

Solution (conceptual flow):

flowchart TD
  S["Start with task description (start, goal, environment)"] --> XU["Choose state x and control u"]
  XU --> DYN["Specify dynamics model x_dot = f(x,u)"]
  DYN --> COST["Select cost terms: effort, smoothness, tracking, safety"]
  COST --> CONSTR["Add path and boundary constraints (limits, collisions)"]
  CONSTR --> DISC["Choose time grid and discretization scheme"]
  DISC --> SOLVER["Formulate NLP and select numerical solver"]
        

In this lesson we formalized trajectory optimization as an optimal control problem for robotic systems. Starting from standard manipulator dynamics, we defined continuous-time cost functionals, path and boundary constraints, and then discretized time to obtain a sparse nonlinear program with states and controls as decision variables. The double integrator example illustrated the full pipeline from dynamics equations to an NLP formulation. We also sketched implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica to emphasize the language-agnostic structure of the problem setup.

Subsequent lessons in this chapter will build on this formalization to introduce specific optimization-based planners such as CHOMP/STOMP, sequential convex optimization (TrajOpt), and direct collocation methods.