The goal of this lab is to compute a dynamically feasible, smooth trajectory for a revolute-joint manipulator between given boundary configurations using direct collocation. You will:

• Formulate a continuous-time optimal control problem for a manipulator.
• Discretize the dynamics via trapezoidal direct collocation on a fixed grid.
• Assemble the resulting nonlinear program (NLP) with state and control decision variables.
• Implement and solve the NLP in multiple programming environments.

We assume familiarity with rigid-body manipulator dynamics and basic constrained optimization from prior courses. The new material in this lab is the discretization via collocation and its practical implementation for trajectory planning.

Consider an n-DOF manipulator with joint positions \( q(t)\in\mathbb{R}^n \) and joint velocities \( v(t)=\dot{q}(t) \). The standard rigid-body dynamics are

\[ \mathbf{M}(q)\,\dot{v} + \mathbf{C}(q,v)\,v + \mathbf{g}(q) = u, \]

where \( \mathbf{M}(q) \) is the positive-definite inertia matrix, \( \mathbf{C}(q,v) \) collects Coriolis/centrifugal terms, \( \mathbf{g}(q) \) is the gravity vector, and \( u(t)\in\mathbb{R}^n \) is the vector of joint torques.

We define the state \( x(t) = \begin{bmatrix} q(t) \\ v(t)\end{bmatrix} \in \mathbb{R}^{2n} \) and write the dynamics as a first-order system:

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

A typical minimum-effort trajectory optimization problem over horizon \( [0,T] \) is

\[ \begin{aligned} \min_{x(\cdot),u(\cdot)}\quad & J = \int_0^T \frac{1}{2}\,u(t)^\top \mathbf{R}\,u(t)\,\mathrm{d}t \\ \text{s.t.}\quad & \dot{x}(t) = f(x(t),u(t)), \\ & x(0) = x_{\mathrm{init}},\quad x(T) = x_{\mathrm{goal}}, \\ & q_{\min} \le q(t) \le q_{\max}, \\ & v_{\min} \le v(t) \le v_{\max}, \\ & u_{\min} \le u(t) \le u_{\max}, \end{aligned} \]

where \( \mathbf{R} \succ 0 \) penalizes control effort and the bound constraints encode joint limits and actuator limits.

We discretize time with a uniform grid of \( N \) segments: \( t_k = k h \) for \( k=0,\dots,N \) and \( h = T/N \). At each node we introduce decision variables \( x_k \approx x(t_k) \) and \( u_k \approx u(t_k) \). For the ODE \( \dot{x}=f(x,u) \), the trapezoidal collocation constraint is

\[ x_{k+1} - x_k = \frac{h}{2}\Bigl(f(x_k,u_k) + f(x_{k+1},u_{k+1})\Bigr), \quad k=0,\dots,N-1. \]

These defect equations enforce that the discrete trajectory is consistent with the dynamics up to the local truncation error of the trapezoidal rule. The discrete approximation of the cost is, for example,

\[ J_h \approx \sum_{k=0}^{N} \frac{h}{2}\Bigl( u_k^\top \mathbf{R} u_k + u_{k+1}^\top \mathbf{R} u_{k+1} \Bigr). \]

Local error order. Using a Taylor expansion of \( x(t) \) and \( f(x(t),u(t)) \) about \( t_k \), one can show that the trapezoidal update matches the exact integral \( x(t_{k+1}) = x(t_k) + \int_{t_k}^{t_{k+1}} f(x(t),u(t))\,\mathrm{d}t \) with an error of order \( \mathcal{O}(h^3) \). Thus, the global integration error across the horizon is of order \( \mathcal{O}(h^2) \), which is sufficient for many trajectory-planning applications.

For a planar 2-DOF arm, we take \( x_k = [q_{1,k}, q_{2,k}, v_{1,k}, v_{2,k}]^\top \) and \( u_k = [u_{1,k}, u_{2,k}]^\top \). The discrete decision vector stacks all states and controls:

\[ z = \bigl(x_0,u_0,x_1,u_1,\dots,x_N,u_N\bigr). \]

The NLP is

\[ \begin{aligned} \min_{z}\quad & J_h(z) = \sum_{k=0}^{N} \frac{h}{2}\Bigl(u_k^\top \mathbf{R}u_k + u_{k+1}^\top \mathbf{R}u_{k+1}\Bigr) \\ \text{s.t.}\quad & x_0 = x_{\mathrm{init}},\quad x_N = x_{\mathrm{goal}}, \\ & x_{k+1} - x_k - \tfrac{h}{2}\bigl(f(x_k,u_k) + f(x_{k+1},u_{k+1})\bigr) = 0,\quad k=0,\dots,N-1, \\ & q_{\min} \le q_k \le q_{\max},\quad v_{\min} \le v_k \le v_{\max},\quad u_{\min} \le u_k \le u_{\max}. \end{aligned} \]

A modern nonlinear programming solver (e.g. IPOPT, SNOPT, fmincon or similar) can then be used to solve for \( z^\star \), which directly yields the desired discrete-time trajectory.

flowchart TD
  C0["Specify dynamics xdot = f(x,u) and cost J"] --> C1["Choose horizon T and number of segments N"]
  C1 --> C2["Define decision variables {x_k, u_k} for k = 0..N"]
  C2 --> C3["Add boundary constraints x_0 = x_init, x_N = x_goal"]
  C3 --> C4["Add trapezoidal collocation constraints for each segment"]
  C4 --> C5["Add bound constraints on q, v, u"]
  C5 --> C6["Pass objective J_h and constraints to NLP solver"]
  C6 --> C7["Obtain optimal discrete trajectory {x_k*, u_k*}"]
        

We now implement the collocation-based trajectory optimization for a 2-DOF planar arm in Python using casadi, which provides symbolic expressions and interfaces to NLP solvers such as IPOPT.

import casadi as ca
import numpy as np
import matplotlib.pyplot as plt

# Problem setup
nq = 2
nv = 2
nx = nq + nv
nu = 2

T = 2.0           # horizon [s]
N = 40            # number of segments
h = T / N

# Simple planar 2-link arm parameters (one possible parameterization)
m1, m2 = 1.0, 1.0
l1, l2 = 1.0, 1.0
lc1, lc2 = 0.5, 0.5
I1, I2 = 0.12, 0.12
g = 9.81

def two_link_dynamics(x, u):
    """
    x = [q1, q2, v1, v2]
    u = [u1, u2]
    returns xdot = [v1, v2, a1, a2]
    """
    q1, q2, v1, v2 = x[0], x[1], x[2], x[3]
    u1, u2 = u[0], u[1]

    s2 = ca.sin(q2)
    c2 = ca.cos(q2)

    # Inertia matrix M(q)
    M11 = I1 + I2 + m2 * l1**2 + 2 * m2 * l1 * lc2 * c2
    M12 = I2 + m2 * l1 * lc2 * c2
    M21 = M12
    M22 = I2
    M = ca.vertcat(
        ca.hcat([M11, M12]),
        ca.hcat([M21, M22])
    )

    # Coriolis/centrifugal matrix C(q, v)
    h_c = m2 * l1 * lc2 * s2
    C11 = 0.0
    C12 = -2.0 * h_c * v2
    C21 = h_c * v1
    C22 = 0.0
    C = ca.vertcat(
        ca.hcat([C11, C12]),
        ca.hcat([C21, C22])
    )

    # Gravity vector g(q)
    g1 = (m1 * lc1 + m2 * l1) * g * ca.cos(q1) + m2 * lc2 * g * ca.cos(q1 + q2)
    g2 = m2 * lc2 * g * ca.cos(q1 + q2)
    G = ca.vertcat(g1, g2)

    v = ca.vertcat(v1, v2)
    tau = ca.vertcat(u1, u2)

    # M(q) * a + C(q, v) * v + g(q) = tau
    a = ca.solve(M, tau - C @ v - G)

    return ca.vertcat(v, a)

# Collocation optimization with CasADi Opti
opti = ca.Opti()

X = opti.variable(nx, N + 1)  # states at nodes
U = opti.variable(nu, N + 1)  # controls at nodes

# Boundary conditions
q_init = np.array([0.0, 0.0])
v_init = np.array([0.0, 0.0])
q_goal = np.array([np.pi / 2.0, 0.0])
v_goal = np.array([0.0, 0.0])

x_init = np.concatenate([q_init, v_init])
x_goal = np.concatenate([q_goal, v_goal])

opti.subject_to(X[:, 0] == x_init)
opti.subject_to(X[:, N] == x_goal)

# Cost (control effort)
R = np.eye(nu)
J = 0
for k in range(N):
    uk = U[:, k]
    uk1 = U[:, k + 1]
    J += 0.5 * h * (ca.mtimes([uk.T, R, uk]) + ca.mtimes([uk1.T, R, uk1]))
opti.minimize(J)

# Collocation constraints (trapezoidal)
for k in range(N):
    xk = X[:, k]
    xk1 = X[:, k + 1]
    uk = U[:, k]
    uk1 = U[:, k + 1]

    fk = two_link_dynamics(xk, uk)
    fk1 = two_link_dynamics(xk1, uk1)

    opti.subject_to(xk1 == xk + 0.5 * h * (fk + fk1))

# Optional simple box constraints on states and controls
q_min = -np.pi
q_max = np.pi
v_max = 4.0
u_max = 10.0

q = X[0:2, :]
v = X[2:4, :]

opti.subject_to(q >= q_min)
opti.subject_to(q <= q_max)
opti.subject_to(v >= -v_max)
opti.subject_to(v <= v_max)
opti.subject_to(U >= -u_max)
opti.subject_to(U <= u_max)

# Initial guess
opti.set_initial(X, np.tile(x_init.reshape((-1, 1)), (1, N + 1)))
opti.set_initial(U, 0.0)

# Solver
opti.solver("ipopt", {"print_time": False}, {"max_iter": 200})

sol = opti.solve()
X_opt = sol.value(X)
U_opt = sol.value(U)

# Plot joint trajectories
t_grid = np.linspace(0.0, T, N + 1)
q1_opt = X_opt[0, :]
q2_opt = X_opt[1, :]

plt.figure()
plt.plot(t_grid, q1_opt, label="q1")
plt.plot(t_grid, q2_opt, label="q2")
plt.xlabel("time [s]")
plt.ylabel("joint position [rad]")
plt.legend()
plt.grid(True)
plt.show()
      

The structure of this code mirrors the mathematical formulation: decision variables for the state and control at all nodes, collocation constraints for each segment, and a quadratic torque penalty as objective. The CasADi backend automatically generates derivatives for the NLP solver.

In C++, one typically implements the NLP by deriving from the Ipopt::TNLP interface, using Eigen for matrix computations. Below is a schematic implementation for the same 2-DOF planar arm. Only key parts of the interface are shown; the pattern directly follows the collocation equations from Section 3.

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

using Eigen::VectorXd;
using Eigen::MatrixXd;

class TwoLinkCollocationNLP : public Ipopt::TNLP {
public:
    TwoLinkCollocationNLP(int N_in, double T_in)
    : N(N_in), T(T_in), h(T_in / N_in), nx(4), nu(2)
    {
        // total variables: (N+1)*nx + (N+1)*nu
        n_var = (N + 1) * (nx + nu);
        n_con_eq = N * nx + 2 * nx; // collocation + boundary
        // inequality bounds encoded via variable bounds
    }

    virtual bool get_nlp_info(Ipopt::Index& n, Ipopt::Index& m,
                              Ipopt::Index& nnz_jac_g,
                              Ipopt::Index& nnz_h_lag,
                              Ipopt::TNLP::IndexStyleEnum& index_style) override {
        n = n_var;
        m = n_con_eq;
        // Sparse pattern sizes (here we give simple dense estimates)
        nnz_jac_g = n * m;
        nnz_h_lag = n * n;
        index_style = Ipopt::TNLP::C_STYLE;
        return true;
    }

    virtual bool get_bounds_info(Ipopt::Index n, double* x_l, double* x_u,
                                 Ipopt::Index m, double* g_l, double* g_u) override {
        // Variable bounds: q, v, u
        const double q_min = -M_PI;
        const double q_max =  M_PI;
        const double v_max =  4.0;
        const double u_max = 10.0;

        for (int k = 0; k <= N; ++k) {
            int offset = k * (nx + nu);
            // q1, q2
            x_l[offset + 0] = q_min; x_u[offset + 0] = q_max;
            x_l[offset + 1] = q_min; x_u[offset + 1] = q_max;
            // v1, v2
            x_l[offset + 2] = -v_max; x_u[offset + 2] = v_max;
            x_l[offset + 3] = -v_max; x_u[offset + 3] = v_max;
            // u1, u2
            x_l[offset + 4] = -u_max; x_u[offset + 4] = u_max;
            x_l[offset + 5] = -u_max; x_u[offset + 5] = u_max;
        }

        // Equality constraints: collocation and boundary
        for (int i = 0; i < m; ++i) {
            g_l[i] = 0.0;
            g_u[i] = 0.0;
        }
        return true;
    }

    virtual bool get_starting_point(Ipopt::Index n, bool init_x, double* x,
                                    bool init_z, double* z_L, double* z_U,
                                    Ipopt::Index m, bool init_lambda,
                                    double* lambda) override {
        if (!init_x) return false;
        // initialize all states at x_init and controls at zero
        VectorXd x_init(nx);
        x_init << 0.0, 0.0, 0.0, 0.0;
        for (int k = 0; k <= N; ++k) {
            int offset = k * (nx + nu);
            for (int i = 0; i < nx; ++i) x[offset + i] = x_init[i];
            x[offset + 4] = 0.0; // u1
            x[offset + 5] = 0.0; // u2
        }
        return true;
    }

    virtual bool eval_f(Ipopt::Index n, const double* x, bool new_x,
                        double& obj_value) override {
        // Quadratic torque cost approximation
        obj_value = 0.0;
        for (int k = 0; k <= N; ++k) {
            int offset = k * (nx + nu);
            double u1 = x[offset + 4];
            double u2 = x[offset + 5];
            obj_value += 0.5 * h * (u1*u1 + u2*u2);
        }
        return true;
    }

    virtual bool eval_g(Ipopt::Index n, const double* x, bool new_x,
                        Ipopt::Index m, double* g) override {
        // Boundary constraints: x_0 and x_N
        VectorXd x_init(nx), x_goal(nx);
        x_init << 0.0, 0.0, 0.0, 0.0;
        x_goal << M_PI / 2.0, 0.0, 0.0, 0.0;

        int row = 0;
        // x_0 - x_init = 0
        for (int i = 0; i < nx; ++i) {
            g[row++] = x[i] - x_init[i];
        }

        // x_N - x_goal = 0
        int offsetN = N * (nx + nu);
        for (int i = 0; i < nx; ++i) {
            g[row++] = x[offsetN + i] - x_goal[i];
        }

        // Collocation constraints
        for (int k = 0; k < N; ++k) {
            int offk  = k * (nx + nu);
            int offk1 = (k + 1) * (nx + nu);

            Eigen::Map<const VectorXd> xk(&x[offk], nx);
            Eigen::Map<const VectorXd> xk1(&x[offk1], nx);

            Eigen::Vector2d uk(x[offk + 4],  x[offk + 5]);
            Eigen::Vector2d uk1(x[offk1 + 4], x[offk1 + 5]);

            VectorXd fk  = two_link_dynamics(xk,  uk);
            VectorXd fk1 = two_link_dynamics(xk1, uk1);

            VectorXd defect = xk1 - xk - 0.5 * h * (fk + fk1);
            for (int i = 0; i < nx; ++i) {
                g[row++] = defect[i];
            }
        }
        return true;
    }

    // eval_grad_f, eval_jac_g, eval_h and other methods must be implemented.

private:
    int N;
    double T, h;
    int nx, nu;
    int n_var, n_con_eq;

    VectorXd two_link_dynamics(const VectorXd& x, const Eigen::Vector2d& u) const {
        // Same equations as in Python version, but using Eigen.
        // Returns xdot = [v1, v2, a1, a2]
        // ...
        VectorXd xdot(4);
        // fill xdot here
        return xdot;
    }
};
      

The core idea is identical to the Python implementation: states and controls at all nodes become entries of the decision vector, collocation equations appear in eval_g, and box constraints are encoded via variable bounds. Analytical Jacobians and Hessians can be supplied for robustness and efficiency.

In Java, we can implement the collocation constraints and objective as functions passed to a generic nonlinear optimizer (for example, in scientific computing libraries or custom solvers). Here is a simplified template that constructs the collocation residuals for a planar arm.

public class TwoLinkCollocationProblem {

    private final int N;
    private final double T;
    private final double h;
    private final int nx = 4;
    private final int nu = 2;

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

    // Map decision vector z to (x_k, u_k)
    private int indexX(int k, int i) {
        // x_k: i = 0..3
        return k * (nx + nu) + i;
    }

    private int indexU(int k, int i) {
        // u_k: i = 0..1
        return k * (nx + nu) + nx + i;
    }

    public double objective(double[] z) {
        double cost = 0.0;
        for (int k = 0; k <= N; ++k) {
            double u1 = z[indexU(k, 0)];
            double u2 = z[indexU(k, 1)];
            cost += 0.5 * h * (u1 * u1 + u2 * u2);
        }
        return cost;
    }

    // Equality constraints: boundary + collocation
    public double[] constraints(double[] z) {
        int m = (N + 1) * nx; // upper bound; some rows unused if needed
        double[] g = new double[m];
        int row = 0;

        double[] xInit = {0.0, 0.0, 0.0, 0.0};
        double[] xGoal = {Math.PI / 2.0, 0.0, 0.0, 0.0};

        // x_0 - xInit = 0
        for (int i = 0; i < nx; ++i) {
            g[row++] = z[indexX(0, i)] - xInit[i];
        }

        // x_N - xGoal = 0
        for (int i = 0; i < nx; ++i) {
            g[row++] = z[indexX(N, i)] - xGoal[i];
        }

        // Collocation constraints
        for (int k = 0; k < N; ++k) {
            double[] xk = new double[nx];
            double[] xk1 = new double[nx];
            double[] uk = new double[nu];
            double[] uk1 = new double[nu];

            for (int i = 0; i < nx; ++i) {
                xk[i] = z[indexX(k, i)];
                xk1[i] = z[indexX(k + 1, i)];
            }
            for (int i = 0; i < nu; ++i) {
                uk[i] = z[indexU(k, i)];
                uk1[i] = z[indexU(k + 1, i)];
            }

            double[] fk = twoLinkDynamics(xk, uk);
            double[] fk1 = twoLinkDynamics(xk1, uk1);

            for (int i = 0; i < nx; ++i) {
                double defect = xk1[i] - xk[i] - 0.5 * h * (fk[i] + fk1[i]);
                g[row++] = defect;
            }
        }

        // If the library expects g.length == number of constraints,
        // one can truncate or size m accordingly.
        return java.util.Arrays.copyOf(g, row);
    }

    private double[] twoLinkDynamics(double[] x, double[] u) {
        // Same dynamics as in Python example, implemented in Java.
        double q1 = x[0], q2 = x[1];
        double v1 = x[2], v2 = x[3];
        double u1 = u[0], u2 = u[1];

        double m1 = 1.0, m2 = 1.0;
        double l1 = 1.0, l2 = 1.0;
        double lc1 = 0.5, lc2 = 0.5;
        double I1 = 0.12, I2 = 0.12;
        double g = 9.81;

        double s2 = Math.sin(q2);
        double c2 = Math.cos(q2);

        double M11 = I1 + I2 + m2 * l1 * l1 + 2.0 * m2 * l1 * lc2 * c2;
        double M12 = I2 + m2 * l1 * lc2 * c2;
        double M21 = M12;
        double M22 = I2;

        double hC = m2 * l1 * lc2 * s2;
        double C11 = 0.0;
        double C12 = -2.0 * hC * v2;
        double C21 = hC * v1;
        double C22 = 0.0;

        double g1 = (m1 * lc1 + m2 * l1) * g * Math.cos(q1)
                    + m2 * lc2 * g * Math.cos(q1 + q2);
        double g2 = m2 * lc2 * g * Math.cos(q1 + q2);

        double v1dot, v2dot;
        // Solve 2x2 linear system M * a = tau - C * v - g
        double detM = M11 * M22 - M12 * M21;
        double v1c = C11 * v1 + C12 * v2;
        double v2c = C21 * v1 + C22 * v2;

        double rhs1 = u1 - v1c - g1;
        double rhs2 = u2 - v2c - g2;

        v1dot = ( M22 * rhs1 - M12 * rhs2 ) / detM;
        v2dot = (-M21 * rhs1 + M11 * rhs2 ) / detM;

        return new double[] {v1, v2, v1dot, v2dot};
    }
}
      

This Java template isolates the discretization logic; any Java-based NLP solver that accepts an objective function and constraint vector can then be used to numerically solve the problem.

In MATLAB, fmincon can be used to solve the collocation NLP. States and controls are stacked into a single vector z, and nonlinear equality constraints implement the collocation equations and boundary conditions. Simulink can then be used to validate the resulting torque sequence on a continuous-time model.

function collocation_two_link_lab
    N = 40;
    T = 2.0;
    h = T / N;
    nx = 4; nu = 2;

    nvar = (N+1) * (nx + nu);

    % Initial guess: rest configuration, zero control
    x_init = [0; 0; 0; 0];
    x_goal = [pi/2; 0; 0; 0];

    z0 = zeros(nvar, 1);
    for k = 0:N
        offset = k * (nx + nu);
        z0(offset + (1:nx)) = x_init;
        z0(offset + nx + (1:nu)) = [0; 0];
    end

    % Variable bounds
    q_min = -pi; q_max = pi;
    v_max = 4.0;
    u_max = 10.0;
    lb = -inf(nvar, 1);
    ub =  inf(nvar, 1);
    for k = 0:N
        offset = k * (nx + nu);
        % q1, q2
        lb(offset + 1) = q_min; ub(offset + 1) = q_max;
        lb(offset + 2) = q_min; ub(offset + 2) = q_max;
        % v1, v2
        lb(offset + 3) = -v_max; ub(offset + 3) = v_max;
        lb(offset + 4) = -v_max; ub(offset + 4) = v_max;
        % u1, u2
        lb(offset + 5) = -u_max; ub(offset + 5) = u_max;
        lb(offset + 6) = -u_max; ub(offset + 6) = u_max;
    end

    % Objective and constraints as nested functions
    fun = @(z)objective_fun(z, N, h, nx, nu);
    nonlcon = @(z)collocation_constraints(z, N, h, nx, nu, x_init, x_goal);

    options = optimoptions("fmincon", ...
        "Algorithm", "interior-point", ...
        "SpecifyObjectiveGradient", false, ...
        "SpecifyConstraintGradient", false, ...
        "MaxIterations", 200);

    [z_star, ~] = fmincon(fun, z0, [], [], [], [], lb, ub, nonlcon, options);

    % Extract optimal states and controls
    X = zeros(nx, N+1);
    U = zeros(nu, N+1);
    for k = 0:N
        offset = k * (nx + nu);
        X(:, k+1) = z_star(offset + (1:nx));
        U(:, k+1) = z_star(offset + nx + (1:nu));
    end

    t_grid = linspace(0, T, N+1);
    figure; plot(t_grid, X(1,:), t_grid, X(2,:));
    xlabel("time [s]"); ylabel("joint angles [rad]");
    legend("q1", "q2"); grid on;

    % Export U to workspace for Simulink block "From Workspace"
    assignin("base", "torque_sequence", [t_grid.' U.']);
end

function J = objective_fun(z, N, h, nx, nu)
    J = 0;
    for k = 0:N
        offset = k * (nx + nu);
        u = z(offset + nx + (1:nu));
        J = J + 0.5 * h * (u.' * u);
    end
end

function [c, ceq] = collocation_constraints(z, N, h, nx, nu, x_init, x_goal)
    c = []; % no inequality constraints here
    ceq = [];

    % Boundary constraints
    x0 = z(1:nx);
    xN = z(N * (nx + nu) + (1:nx));
    ceq = [ceq;
           x0 - x_init;
           xN - x_goal];

    % Collocation constraints
    for k = 0:(N-1)
        offk  = k * (nx + nu);
        offk1 = (k+1) * (nx + nu);

        xk  = z(offk  + (1:nx));
        uk  = z(offk  + nx + (1:nu));
        xk1 = z(offk1 + (1:nx));
        uk1 = z(offk1 + nx + (1:nu));

        fk  = two_link_dynamics(xk,  uk);
        fk1 = two_link_dynamics(xk1, uk1);

        defect = xk1 - xk - 0.5 * h * (fk + fk1);
        ceq = [ceq; defect];
    end
end

function xdot = two_link_dynamics(x, u)
    q1 = x(1); q2 = x(2);
    v1 = x(3); v2 = x(4);
    u1 = u(1); u2 = u(2);

    m1 = 1.0; m2 = 1.0;
    l1 = 1.0; l2 = 1.0;
    lc1 = 0.5; lc2 = 0.5;
    I1 = 0.12; I2 = 0.12; g = 9.81;

    s2 = sin(q2); c2 = cos(q2);

    M11 = I1 + I2 + m2 * l1^2 + 2 * m2 * l1 * lc2 * c2;
    M12 = I2 + m2 * l1 * lc2 * c2;
    M21 = M12;
    M22 = I2;
    M = [M11, M12; M21, M22];

    hC = m2 * l1 * lc2 * s2;
    C = [0,  -2 * hC * v2;
         hC * v1, 0];

    g1 = (m1 * lc1 + m2 * l1) * g * cos(q1) + m2 * lc2 * g * cos(q1 + q2);
    g2 = m2 * lc2 * g * cos(q1 + q2);
    G = [g1; g2];

    v = [v1; v2];
    a = M \ (u - C * v - G);

    xdot = [v; a];
end
      

In Simulink, a continuous-time model of the 2-DOF arm can be built (for example using a joint-space rigid-body dynamics block). The optimized torque sequence torque_sequence can then be fed through a From Workspace block to verify the resulting motion under the continuous dynamics.

Mathematica offers symbolic and numeric capabilities that make it well-suited to direct collocation. Below is an example for a simplified single-DOF joint with dynamics \( \ddot{q} = u \). The extension to the 2-DOF arm follows by stacking vector states and controls and using the manipulator dynamics \( f(x,u) \).

(* Discretization parameters *)
n = 40;
t0 = 0.0;
T = 2.0;
h = (T - t0)/n;

(* Decision variables: q[k], v[k], u[k] for k = 0..n *)
q = Array[q, 0 ;; n];
v = Array[v, 0 ;; n];
u = Array[u, 0 ;; n];

(* Boundary conditions *)
qInit = 0.0; vInit = 0.0;
qGoal = Pi/2; vGoal = 0.0;

bcEqns = {
  q[0] == qInit,
  v[0] == vInit,
  q[n] == qGoal,
  v[n] == vGoal
};

(* Trapezoidal collocation for single-DOF: x = (q, v), xdot = (v, u) *)
collocationEqns =
  Flatten@Table[
    {
      q[k + 1] - q[k] == (h/2) (v[k] + v[k + 1]),
      v[k + 1] - v[k] == (h/2) (u[k] + u[k + 1])
    },
    {k, 0, n - 1}
  ];

(* Quadratic control effort objective *)
J =
  Sum[(h/2) (u[k]^2 + u[k + 1]^2), {k, 0, n - 1}];

vars = Flatten[{Table[q[k], {k, 0, n}],
                Table[v[k], {k, 0, n}],
                Table[u[k], {k, 0, n}]}];

sol = NMinimize[
  {J, Join[bcEqns, collocationEqns]},
  vars,
  Method -> {"AugmentedLagrangian"}][[2]];

qOpt = Table[q[k] /. sol, {k, 0, n}];
vOpt = Table[v[k] /. sol, {k, 0, n}];
uOpt = Table[u[k] /. sol, {k, 0, n}];

tGrid = Table[t0 + k h, {k, 0, n}];

ListLinePlot[
  {Transpose[{tGrid, qOpt}],
   Transpose[{tGrid, vOpt}]},
  PlotLegends -> {"q(t)", "v(t)"},
  AxesLabel -> {"t", "state"}
]
      

For a 2-DOF arm, the state can be defined as x[k] = {q1[k], q2[k], v1[k], v2[k]} and u[k] = {u1[k], u2[k]}, and the dynamics f[x[k], u[k]] can be implemented using Mathematica's vector operations. The collocation equations and cost follow the same pattern as in this scalar example.

Before running large-scale manipulator trajectory optimizations, it is helpful to use a structured checklist to avoid common pitfalls (such as inconsistent units, ill-conditioned dynamics, or infeasible boundary conditions).

flowchart TD
  S["Start lab"] --> M["Verify manipulator model M(q), C(q,v), g(q)"]
  M --> B["Check boundary states x_init, x_goal are dynamically reachable"]
  B --> D["Choose N and T; compute step h"]
  D --> G["Form collocation constraints and box bounds"]
  G --> I["Provide good initial guess for x_k, u_k"]
  I --> O["Solve NLP and inspect diagnostics"]
  O --> V["Validate trajectory with forward simulation"]
  V --> R["Refine grid size or weights if needed"]
        

This flow mirrors what you will implement in each language: model verification, discrete formulation, optimization, and post-solution validation.

Problem 1 (Consistency of Trapezoidal Collocation): Consider a scalar ODE \( \dot{x}(t) = f(x(t)) \) with sufficiently smooth \( f \). Show that the trapezoidal update

\[ x_{k+1} = x_k + \frac{h}{2}\Bigl(f(x_k) + f(x_{k+1})\Bigr) \]

has local truncation error of order \( \mathcal{O}(h^3) \).

Solution: Let \( x(t) \) be the exact solution. Expanding around \( t_k \), we have \( x(t_{k+1}) = x(t_k) + h \dot{x}(t_k) + \tfrac{h^2}{2}\ddot{x}(t_k) + \mathcal{O}(h^3) \). Since \( \dot{x}(t) = f(x(t)) \), \( \ddot{x}(t) = f'(x(t)) f(x(t)) \). The exact integral form is

\[ x(t_{k+1}) = x(t_k) + \int_{t_k}^{t_{k+1}} f(x(s))\,\mathrm{d}s. \]

Applying the scalar trapezoidal rule to the integral yields \( \int_{t_k}^{t_{k+1}} f(x(s))\,\mathrm{d}s = \tfrac{h}{2}\bigl(f(x(t_k)) + f(x(t_{k+1}))\bigr) + \mathcal{O}(h^3) \). Substituting back, we obtain

\[ x(t_{k+1}) = x(t_k) + \frac{h}{2}\bigl(f(x(t_k)) + f(x(t_{k+1}))\bigr) + \mathcal{O}(h^3), \]

which shows that the trapezoidal update reproduces the exact solution up to \( \mathcal{O}(h^3) \) at each step, hence the local truncation error is of that order.

Problem 2 (Linear Dynamics and Collocation Sparsity): Suppose the manipulator is linearized about a nominal trajectory so that \( \dot{x}(t) = \mathbf{A}x(t) + \mathbf{B}u(t) \) with constant matrices. Show that trapezoidal collocation leads to linear equality constraints in the decision variables \( x_k,u_k \), and write them explicitly.

Solution: Substituting \( f(x,u) = \mathbf{A}x + \mathbf{B}u \) into the collocation equation,

\[ x_{k+1} - x_k - \frac{h}{2}\Bigl( \mathbf{A}x_k + \mathbf{B}u_k + \mathbf{A}x_{k+1} + \mathbf{B}u_{k+1} \Bigr) = 0, \]

we collect terms to obtain

\[ \Bigl(\mathbf{I} - \frac{h}{2}\mathbf{A}\Bigr) x_{k+1} - \Bigl(\mathbf{I} + \frac{h}{2}\mathbf{A}\Bigr) x_k - \frac{h}{2}\mathbf{B}u_k - \frac{h}{2}\mathbf{B}u_{k+1} = 0. \]

This is linear in \( (x_k,x_{k+1},u_k,u_{k+1}) \). Thus, for linear dynamics, the collocation constraints contribute a sparse linear block structure to the NLP, which can be exploited by the solver.

Problem 3 (Scaling of Step Size and Grid Refinement): For a fixed horizon \( T \), consider two collocation grids with \( N \) and \( 2N \) segments. Give a qualitative argument for why the solution on the finer grid approximates the true continuous-time optimum more closely, and discuss the trade-off in terms of NLP size.

Solution: The step size scales as \( h = T/N \). Since trapezoidal collocation has local error \( \mathcal{O}(h^3) \) and global error \( \mathcal{O}(h^2) \), halving \( h \) (doubling \( N \)) reduces the global discretization error roughly by a factor of four, assuming a sufficiently smooth solution. Thus the refined grid trajectory lies closer to the continuous-time optimum in both state and control. The trade-off is that the number of decision variables grows from approximately \( (N+1)(2n + n) = 3n(N+1) \) to \( 3n(2N+1) \), and the number of constraints grows proportionally, making each NLP solve more expensive.

Problem 4 (Boundary Feasibility): Consider a single-DOF joint with dynamics \( \ddot{q} = u \), and bounded control \( |u(t)| \le u_{\max} \). Show that for a given horizon \( T \) there exist initial and final states \( (q_{\mathrm{init}},v_{\mathrm{init}}) \) and \( (q_{\mathrm{goal}},v_{\mathrm{goal}}) \) for which the collocation NLP is infeasible because the continuous-time problem itself is infeasible.

Solution: Under the dynamics \( \ddot{q} = u \) with bounded \( |u| \le u_{\max} \), the maximum achievable change in velocity magnitude over horizon \( T \) is \( |v_{\mathrm{goal}} - v_{\mathrm{init}}| \le u_{\max} T \). If the user specifies a boundary condition violating this inequality, no admissible control exists. Even when the velocity condition is satisfied, the position displacement \( q_{\mathrm{goal}} - q_{\mathrm{init}} \) must be compatible with the bounded-acceleration kinematics (e.g. double-integrator bang-bang profiles). If these reachability conditions fail, the continuous-time optimal control problem is infeasible, and so is any consistent collocation discretization, independent of grid resolution.

Problem 5 (Connection to Pontryagin's Minimum Principle): Explain qualitatively how the Karush–Kuhn–Tucker (KKT) conditions of the collocation NLP approximate the necessary conditions provided by Pontryagin's Minimum Principle (PMP) for the continuous-time problem.

Solution: In the continuous-time setting, PMP introduces a costate \( \lambda(t) \) and yields adjoint dynamics \( -\dot{\lambda} = \partial H / \partial x \) with Hamiltonian \( H(x,u,\lambda) = \ell(x,u) + \lambda^\top f(x,u) \). The optimal control minimizes \( H \) pointwise. In the collocation NLP, the Lagrange multipliers associated with the dynamic constraints at each node play the role of discrete costates. The KKT stationarity conditions with respect to the discrete states and controls then encode a discrete adjoint equation and a discrete optimality condition for the control, which converge to the PMP conditions as the grid is refined and the collocation scheme approximates the ODE more accurately.

In this lab you implemented direct collocation for a manipulator trajectory-planning problem. Starting from the continuous-time dynamics \( \dot{x} = f(x,u) \), you constructed trapezoidal collocation constraints on a finite grid, assembled a nonlinear program with boundary and box constraints, and solved it using numerical optimization tools in several programming environments. The resulting discrete trajectories can be validated via forward simulation, and grid refinement provides a systematic way to trade off solution accuracy against computational cost.