Consider a robot manipulator with state \( x_k \in \mathbb{R}^n \) (typically joint angles and velocities) and control input (joint torques) \( u_k \in \mathbb{R}^m \) in discrete time:

\[ x_{k+1} = f_k(x_k, u_k), \quad k = 0,\dots,N-1. \]

A finite-horizon nonlinear optimal control problem for a manipulator chooses the control sequence \( \mathbf{u} = (u_0,\dots,u_{N-1}) \) to minimize a cost

\[ J(\mathbf{u}) = \phi(x_N) + \sum_{k=0}^{N-1} \ell_k(x_k, u_k), \]

where \( \ell_k \) penalizes tracking error, torque effort and smoothness, and \( \phi \) penalizes terminal error (from Lesson 1 of this chapter). Dynamic programming gives a global solution but is intractable for high-dimensional manipulators. DDP and iLQR instead compute a local optimum by:

• Choosing a nominal state-control trajectory \((\bar{x}_k,\bar{u}_k)\).
• Locally approximating dynamics and cost by linear-quadratic models around that trajectory.
• Solving the resulting LQR-like problem by backward recursion (as in Lesson 2).
• Rolling out a new nominal trajectory and repeating until convergence.

DDP uses second-order derivatives of both cost and dynamics, while iLQR uses only first-order derivatives of dynamics and second-order derivatives of the cost (a Gauss-Newton approximation).

flowchart TD
  A["Initial guess of x_k, u_k"] --> B["Backward pass: \ncompute k_k, K_k"]
  B --> C["Forward rollout: \nsimulate robot dynamics"]
  C --> D{"Cost improved?"}
  D -->|yes| E["Update nominal trajectory"]
  D -->|no| F["Reduce step size alpha"]
  F --> B
  E --> G{"Converged?"}
  G -->|no| B
  G -->|yes| H["Return locally optimal control sequence"]
        

For an \(n\)-DOF manipulator with joint coordinates \( q \in \mathbb{R}^n \) and velocities \( \dot{q} \in \mathbb{R}^n \), the continuous-time dynamics (from the robotics course) are

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau, \]

where \( M(q) \) is the inertia matrix, \( C(q,\dot{q}) \) the Coriolis/centrifugal matrix, and \( g(q) \) the gravity torque. We define the state

\[ x = \begin{bmatrix} q \\[2pt] \dot{q} \end{bmatrix}, \quad u = \tau. \]

Using a simple forward Euler discretization with sampling time \( \Delta t \),

\[ x_{k+1} = \begin{bmatrix} q_{k+1} \\[2pt] \dot{q}_{k+1} \end{bmatrix} \approx \begin{bmatrix} q_k + \Delta t\,\dot{q}_k \\ \dot{q}_k + \Delta t\,\ddot{q}_k \end{bmatrix}, \quad \ddot{q}_k = M(q_k)^{-1}\bigl(u_k - C(q_k,\dot{q}_k)\dot{q}_k - g(q_k)\bigr). \]

The cost is typically quadratic in tracking error and control effort:

\[ \ell_k(x_k,u_k) = \tfrac{1}{2}(x_k - x_k^{\mathrm{ref}})^\top Q_k (x_k - x_k^{\mathrm{ref}}) + \tfrac{1}{2} u_k^\top R_k u_k, \]

\[ \phi(x_N) = \tfrac{1}{2}(x_N - x_N^{\mathrm{ref}})^\top P_N (x_N - x_N^{\mathrm{ref}}), \]

where \( Q_k \succeq 0 \), \( R_k \succ 0 \) and \( P_N \succeq 0 \) are design matrices (Lesson 2).

Define the value function at stage \(k\):

\[ V_k(x) = \min_{u_k,\dots,u_{N-1}} \left\{ \phi(x_N) + \sum_{t=k}^{N-1} \ell_t(x_t,u_t) \right\} \quad \text{s.t.} \quad x_{t+1} = f_t(x_t,u_t). \]

The dynamic programming principle gives the discrete-time Bellman recursion:

\[ V_N(x) = \phi(x), \qquad V_k(x) = \min_{u} \bigl\{ \ell_k(x,u) + V_{k+1}(f_k(x,u)) \bigr\}. \]

If we could represent \( V_k(x) \) for all states, we would have a global optimal feedback law \( \pi_k^\star(x) \). For manipulators, however, the state dimension is large, and storing/solving the Bellman equation on a grid is impossible in practice (curse of dimensionality). DDP and iLQR avoid explicit gridding by approximating \( V_k \) locally around a nominal trajectory and updating this trajectory iteratively.

Fix a nominal trajectory \( (\bar{x}_0,\bar{u}_0,\dots,\bar{x}_N) \) satisfying \( \bar{x}_{k+1} = f_k(\bar{x}_k,\bar{u}_k) \). Define perturbations

\[ \delta x_k = x_k - \bar{x}_k, \qquad \delta u_k = u_k - \bar{u}_k. \]

First-order linearization of the dynamics gives

\[ \delta x_{k+1} \approx f_{x,k}\,\delta x_k + f_{u,k}\,\delta u_k, \quad f_{x,k} = \left.\frac{\partial f_k}{\partial x}\right|_{(\bar{x}_k,\bar{u}_k)}, \quad f_{u,k} = \left.\frac{\partial f_k}{\partial u}\right|_{(\bar{x}_k,\bar{u}_k)}. \]

For DDP we also consider second-order derivatives of the dynamics (e.g. \( f_{xx,k} \), \( f_{xu,k} \), \( f_{uu,k} \)), but for iLQR we keep only the first-order dynamics and put second-order information entirely into the cost terms.

The stage cost is approximated to second order:

\[ \ell_k(x_k,u_k) \approx \ell_k^0 + \ell_{x,k}^\top \delta x_k + \ell_{u,k}^\top \delta u_k + \tfrac{1}{2} \begin{bmatrix} \delta x_k \\[2pt] \delta u_k \end{bmatrix}^\top \begin{bmatrix} \ell_{xx,k} & \ell_{xu,k} \\ \ell_{ux,k} & \ell_{uu,k} \end{bmatrix} \begin{bmatrix} \delta x_k \\[2pt] \delta u_k \end{bmatrix}, \]

with derivatives evaluated at \((\bar{x}_k,\bar{u}_k)\). Assume inductively that \( V_{k+1}(x) \) is locally quadratic:

\[ V_{k+1}(x_{k+1}) \approx V_{k+1}^0 + V_{x,k+1}^\top \delta x_{k+1} + \tfrac{1}{2}\,\delta x_{k+1}^\top V_{xx,k+1}\,\delta x_{k+1}. \]

Define the local Q-function at stage \(k\) for perturbations \((\delta x_k,\delta u_k)\):

\[ Q_k(\delta x_k,\delta u_k) = \ell_k(x_k,u_k) + V_{k+1}(x_{k+1}). \]

Substituting the linearized dynamics \( \delta x_{k+1} \approx f_{x,k}\delta x_k + f_{u,k}\delta u_k \) into the quadratic model of \( V_{k+1} \) and keeping terms up to second order yields a quadratic form

\[ Q_k(\delta x_k,\delta u_k) \approx Q_k^0 + Q_{x,k}^\top \delta x_k + Q_{u,k}^\top \delta u_k + \tfrac{1}{2} \begin{bmatrix} \delta x_k \\[2pt] \delta u_k \end{bmatrix}^\top \begin{bmatrix} Q_{xx,k} & Q_{xu,k} \\ Q_{ux,k} & Q_{uu,k} \end{bmatrix} \begin{bmatrix} \delta x_k \\[2pt] \delta u_k \end{bmatrix}. \]

For the iLQR (first-order dynamics) approximation, the derivatives are

\[ \begin{aligned} Q_{x,k} &= \ell_{x,k} + f_{x,k}^\top V_{x,k+1}, \\ Q_{u,k} &= \ell_{u,k} + f_{u,k}^\top V_{x,k+1}, \\ Q_{xx,k} &= \ell_{xx,k} + f_{x,k}^\top V_{xx,k+1} f_{x,k}, \\ Q_{uu,k} &= \ell_{uu,k} + f_{u,k}^\top V_{xx,k+1} f_{u,k}, \\ Q_{ux,k} &= \ell_{ux,k} + f_{u,k}^\top V_{xx,k+1} f_{x,k}, \quad Q_{xu,k} = Q_{ux,k}^\top. \end{aligned} \]

We now minimize this quadratic approximation with respect to \( \delta u_k \). Assuming \( Q_{uu,k} \) is positive definite, the stationary condition \( \partial Q_k/\partial \delta u_k = 0 \) gives the affine feedback law

\[ \delta u_k^\star = k_k + K_k \delta x_k, \]

\[ k_k = - Q_{uu,k}^{-1} Q_{u,k}, \qquad K_k = - Q_{uu,k}^{-1} Q_{ux,k}. \]

Substituting this optimal \(\delta u_k^\star\) back into the quadratic approximation of \( Q_k \) yields a quadratic form for \( V_k(x_k) \):

\[ V_{x,k} = Q_{x,k} + K_k^\top Q_{uu,k} k_k + K_k^\top Q_{u,k} + Q_{xu,k}^\top k_k, \]

\[ V_{xx,k} = Q_{xx,k} + K_k^\top Q_{uu,k} K_k + K_k^\top Q_{ux,k} + Q_{xu,k} K_k. \]

These backward recursions (from \(k=N-1\) down to \(0\)) produce the local gains \( k_k, K_k \) and updated quadratic models of \( V_k \). At the final time step

\[ V_{x,N} = \phi_x(\bar{x}_N), \qquad V_{xx,N} = \phi_{xx}(\bar{x}_N). \]

The backward pass gives the local feedback law \( \delta u_k^\star = k_k + K_k \delta x_k \). We then perform a forward rollout from the initial state \( x_0 \) using a step size \( \alpha \in (0,1] \):

\[ x_0^{\mathrm{new}} = x_0, \qquad u_k^{\mathrm{new}} = \bar{u}_k + \alpha\,k_k + K_k(x_k^{\mathrm{new}} - \bar{x}_k), \]

\[ x_{k+1}^{\mathrm{new}} = f_k(x_k^{\mathrm{new}}, u_k^{\mathrm{new}}), \quad k=0,\dots,N-1. \]

The resulting trajectory \( (x_k^{\mathrm{new}},u_k^{\mathrm{new}}) \) defines a new nominal trajectory \((\bar{x}_k,\bar{u}_k)\) if it decreases the total cost \(J\). A simple line search \( \alpha \in \{1, 0.5, 0.25,\dots\} \) is often used.

To maintain positive definiteness of \( Q_{uu,k} \), we can add a Levenberg–Marquardt style regularization:

\[ \tilde{Q}_{uu,k} = Q_{uu,k} + \lambda I, \]

with \( \lambda \ge 0 \) increased when the forward pass fails to reduce the cost and decreased otherwise.

flowchart TD
  S["Start with initial (x_k, u_k)"] --> B1["Backward pass: \ncompute Q-derivatives"]
  B1 --> B2["Compute k_k, K_k (regularized)"]
  B2 --> F["Forward pass with step size alpha"]
  F --> J["Evaluate new cost J_new"]
  J --> C{"J_new < J_old ?"}
  C -->|yes| U["Accept trajectory, \nreduce lambda"]
  C -->|no| L["Reject, increase lambda \nor shrink alpha"]
  U --> T{"Termination criterion met?"}
  L --> B1
  T -->|no| B1
  T -->|yes| D["Done: locally optimal trajectory"]
        

The difference between DDP and iLQR lies in the treatment of dynamics second derivatives:

• DDP: Includes second-order terms of the dynamics in the Q-function expansion, such as \( f_{xx,k}, f_{xu,k}, f_{uu,k} \). This yields more accurate curvature information and often faster convergence, but requires computation or approximation of second derivatives of the manipulator dynamics.
• iLQR: Ignores dynamics second derivatives and uses only the linearization \( f_{x,k}, f_{u,k} \), while still using second-order cost derivatives. This is equivalent to a Gauss–Newton approximation of the Hessian and is substantially simpler to implement, especially when manipulator dynamics are provided by a library.

For robot manipulators, where high-quality analytical dynamics (and Jacobians) are available, DDP can exploit structure but is more complex. iLQR strikes a practical balance between performance and implementation complexity and is widely used for trajectory optimization.

We implement iLQR for a single-link pendulum (rotary joint) with state \( x = [\theta, \dot{\theta}]^\top \) and torque input \( u \). For simplicity we use Euler discretization and a quadratic tracking cost to drive the joint to zero.

import numpy as np

# Pendulum parameters
m = 1.0
l = 1.0
g = 9.81
dt = 0.01

def dynamics(x, u):
    """Discrete-time pendulum dynamics x_{k+1} = f(x_k, u_k)."""
    theta, dtheta = x
    # continuous-time acceleration
    ddtheta = (-g / l) * np.sin(theta) + u / (m * l**2)
    # Euler integration
    theta_next = theta + dt * dtheta
    dtheta_next = dtheta + dt * ddtheta
    return np.array([theta_next, dtheta_next])

def linearize_dynamics(x, u):
    """Return f_x (2x2) and f_u (2x1) at (x, u)."""
    theta, dtheta = x
    # partial derivatives
    ddtheta_dtheta = (-g / l) * np.cos(theta)
    ddtheta_du = 1.0 / (m * l**2)

    fx = np.eye(2)
    fx[0, 1] = dt  # d theta_next / d dtheta
    fx[1, 0] = dt * ddtheta_dtheta
    fx[1, 1] = 1.0  # d dtheta_next / d dtheta

    fu = np.zeros((2, 1))
    fu[1, 0] = dt * ddtheta_du
    return fx, fu

# Cost matrices
Q = np.diag([10.0, 1.0])
R = np.array([[0.1]])
P_N = np.diag([50.0, 5.0])

def stage_cost(x, u, x_ref):
    dx = x - x_ref
    return 0.5 * dx.T @ Q @ dx + 0.5 * u.T @ R @ u

def terminal_cost(x, x_ref):
    dx = x - x_ref
    return 0.5 * dx.T @ P_N @ dx

def ilqr(x0, x_ref_traj, N, max_iter=50):
    # Initial (zero) control sequence
    n = x0.size
    m = 1
    U = np.zeros((N, m))
    X = np.zeros((N + 1, n))
    X[0] = x0

    # Rollout to initialize X
    for k in range(N):
        X[k + 1] = dynamics(X[k], U[k])

    for it in range(max_iter):
        # Backward pass
        Vx = P_N @ (X[N] - x_ref_traj[N])
        Vxx = P_N.copy()

        K = np.zeros((N, m, n))
        k_feedforward = np.zeros((N, m))

        regularization = 1e-6

        for k in range(N - 1, -1, -1):
            xk = X[k]
            uk = U[k]
            x_ref = x_ref_traj[k]

            fx, fu = linearize_dynamics(xk, uk)

            # Cost derivatives (quadratic tracking cost)
            dx = xk - x_ref
            lx = Q @ dx
            lu = R @ uk
            lxx = Q
            luu = R
            lux = np.zeros((m, n))

            # Q derivatives for iLQR
            Qx = lx + fx.T @ Vx
            Qu = lu + fu.T @ Vx
            Qxx = lxx + fx.T @ Vxx @ fx
            Quu = luu + fu.T @ Vxx @ fu + regularization * np.eye(m)
            Qux = lux + fu.T @ Vxx @ fx

            # Compute gains
            Quu_inv = np.linalg.inv(Quu)
            k_ff = -Quu_inv @ Qu
            K_fb = -Quu_inv @ Qux

            k_feedforward[k] = k_ff.ravel()
            K[k] = K_fb

            # Update value function
            Vx = Qx + K_fb.T @ Quu @ k_ff + K_fb.T @ Qu + Qux.T @ k_ff
            Vxx = Qxx + K_fb.T @ Quu @ K_fb + K_fb.T @ Qux + Qux.T @ K_fb
            # Symmetrize for numerical stability
            Vxx = 0.5 * (Vxx + Vxx.T)

        # Forward pass with line search
        alpha_list = [1.0, 0.5, 0.25, 0.1]
        cost_old = (
            sum(stage_cost(X[k], U[k], x_ref_traj[k]) for k in range(N))
            + terminal_cost(X[N], x_ref_traj[N])
        )
        improved = False
        for alpha in alpha_list:
            X_new = np.zeros_like(X)
            U_new = np.zeros_like(U)
            X_new[0] = x0
            for k in range(N):
                du = alpha * k_feedforward[k] + K[k] @ (X_new[k] - X[k])
                U_new[k] = U[k] + du
                X_new[k + 1] = dynamics(X_new[k], U_new[k])
            cost_new = (
                sum(stage_cost(X_new[k], U_new[k], x_ref_traj[k]) for k in range(N))
                + terminal_cost(X_new[N], x_ref_traj[N])
            )
            if cost_new < cost_old:
                X, U = X_new, U_new
                improved = True
                break

        if not improved:
            # Could increase regularization here; for simplicity we just stop
            break

    return X, U

# Example usage: stabilize around upright theta = 0
N = 300
x0 = np.array([0.5, 0.0])  # initial angle 0.5 rad
x_ref_traj = np.zeros((N + 1, 2))
X_opt, U_opt = ilqr(x0, x_ref_traj, N)
      

For multi-DOF manipulators, the same algorithm applies with higher-dimensional states and torques and dynamics obtained from a robotics library (for example, computing M(q), C(q, dq), g(q), and the discrete-time map f(x, u) via a library such as pinocchio or Python-based robotics toolboxes).

The following C++ code sketch shows the core backward and forward passes using Eigen. For a real manipulator, one would call a dynamics library (e.g. a C++-based rigid-body dynamics package) inside dynamics() and linearizeDynamics().

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

using Eigen::Vector2d;
using Eigen::Matrix2d;
using Eigen::VectorXd;
using Eigen::MatrixXd;

struct ILQROptions {
  int N;
  int maxIter;
  double dt;
};

Vector2d dynamics(const Vector2d& x, double u, double dt) {
  double m = 1.0, l = 1.0, g = 9.81;
  double theta = x(0);
  double dtheta = x(1);

  double ddtheta = (-g / l) * std::sin(theta) + u / (m * l * l);
  Vector2d xnext;
  xnext(0) = theta + dt * dtheta;
  xnext(1) = dtheta + dt * ddtheta;
  return xnext;
}

void linearizeDynamics(const Vector2d& x, double /*u*/, double dt,
                       Matrix2d& fx, Eigen::Vector2d& fu) {
  double m = 1.0, l = 1.0, g = 9.81;
  double theta = x(0);

  double ddtheta_dtheta = (-g / l) * std::cos(theta);
  double ddtheta_du = 1.0 / (m * l * l);

  fx.setIdentity();
  fx(0, 1) = dt;
  fx(1, 0) = dt * ddtheta_dtheta;
  fx(1, 1) = 1.0;

  fu.setZero();
  fu(1) = dt * ddtheta_du;
}

void ilqr(const Vector2d& x0,
          const std::vector<Vector2d>& xRef,
          const ILQROptions& opt,
          std::vector<Vector2d>& X,
          std::vector<double>& U) {
  int N = opt.N;
  double dt = opt.dt;

  X.assign(N + 1, Vector2d::Zero());
  U.assign(N, 0.0);
  X[0] = x0;

  // Initial rollout
  for (int k = 0; k < N; ++k) {
    X[k + 1] = dynamics(X[k], U[k], dt);
  }

  Matrix2d Q, P_N;
  Q.setZero();
  Q(0, 0) = 10.0;
  Q(1, 1) = 1.0;
  P_N.setZero();
  P_N(0, 0) = 50.0;
  P_N(1, 1) = 5.0;
  double R = 0.1;

  for (int it = 0; it < opt.maxIter; ++it) {
    // Backward pass
    Eigen::Vector2d Vx = P_N * (X[N] - xRef[N]);
    Matrix2d Vxx = P_N;

    std::vector<Eigen::Matrix<double, 1, 2> > K(N);
    std::vector<double> k_ff(N);

    double reg = 1e-6;

    for (int k = N - 1; k >= 0; --k) {
      const Vector2d& xk = X[k];
      double uk = U[k];
      const Vector2d& xref = xRef[k];

      Matrix2d fx;
      Eigen::Vector2d fu;
      linearizeDynamics(xk, uk, dt, fx, fu);

      Vector2d dx = xk - xref;
      Vector2d lx = Q * dx;
      double lu = R * uk;
      Matrix2d lxx = Q;
      double luu = R;

      Vector2d Qx = lx + fx.transpose() * Vx;
      double Qu = lu + fu.transpose() * Vx;
      Matrix2d Qxx = lxx + fx.transpose() * Vxx * fx;
      double Quu = luu + (fu.transpose() * Vxx * fu)(0) + reg;
      Eigen::RowVector2d Qux = (fu.transpose() * Vxx * fx);

      double Quu_inv = 1.0 / Quu;
      double k_local = -Quu_inv * Qu;
      Eigen::RowVector2d K_local = -Quu_inv * Qux;

      k_ff[k] = k_local;
      K[k] = K_local;

      Vx = Qx + K_local.transpose() * Quu * k_local
          + K_local.transpose() * Qu + Qux.transpose() * k_local;
      Vxx = Qxx + K_local.transpose() * Quu * K_local
           + K_local.transpose() * Qux + Qux.transpose() * K_local;
      Vxx = 0.5 * (Vxx + Vxx.transpose());
    }

    // Forward pass not fully shown (similar to Python version)
    // ...
  }
}
      

In a full implementation, the forward pass mirrors the Python version: apply u_k_new = u_k + alpha * k_ff[k] + K[k] * (x_k_new - X[k]) and propagate the dynamics. For a multi-DOF robot, replace Vector2d with higher-dimensional vectors, and compute dynamics via a rigid-body library.

Java is less common for low-level robot control, but we can still implement iLQR using a numerical linear algebra library (e.g. EJML). Below is a skeleton that focuses on the algorithmic structure.

public class ILQRPendulum {

    static final double m = 1.0, l = 1.0, g = 9.81, dt = 0.01;

    static double[] dynamics(double[] x, double u) {
        double theta = x[0];
        double dtheta = x[1];
        double ddtheta = (-g / l) * Math.sin(theta) + u / (m * l * l);
        double[] xnext = new double[2];
        xnext[0] = theta + dt * dtheta;
        xnext[1] = dtheta + dt * ddtheta;
        return xnext;
    }

    static void ilqr(double[] x0, double[][] xRef, int N, int maxIter) {
        double[][] X = new double[N + 1][2];
        double[] U = new double[N];
        X[0] = x0.clone();

        // Initial rollout
        for (int k = 0; k < N; ++k) {
            X[k + 1] = dynamics(X[k], U[k]);
        }

        for (int it = 0; it < maxIter; ++it) {
            // Backward and forward passes would go here,
            // using EJML matrices for Q-derivatives and gains K, k.
            // This skeleton omits details to focus on the structure.
        }
    }

    public static void main(String[] args) {
        int N = 300;
        double[] x0 = {0.5, 0.0};
        double[][] xRef = new double[N + 1][2]; // all zeros
        ilqr(x0, xRef, N, 50);
    }
}
      

In a full implementation, one would: (i) use a matrix library for Qxx, Quu etc., (ii) follow the Python/C++ formulas for the backward pass, and (iii) integrate with a robot dynamics engine if available.

MATLAB is widely used in control engineering. Below is a script implementing iLQR for the pendulum; the dynamics can be replaced by calls to a rigidBodyTree-based manipulator model. A Simulink model can use this iLQR controller as a discrete-time subsystem.

function [X, U] = ilqr_pendulum_matlab(x0, N, maxIter)
  m = 1.0; l = 1.0; g = 9.81; dt = 0.01;
  Q = diag([10, 1]);
  R = 0.1;
  P_N = diag([50, 5]);

  X = zeros(2, N+1);
  U = zeros(1, N);
  X(:,1) = x0(:);
  xref = zeros(2, N+1);

  % initial rollout
  for k = 1:N
    X(:,k+1) = dynamics(X(:,k), U(k), m, l, g, dt);
  end

  for it = 1:maxIter
    Vx = P_N * (X(:,N+1) - xref(:,N+1));
    Vxx = P_N;

    K = zeros(1, 2, N);
    k_ff = zeros(1, N);
    lambda = 1e-6;

    for k = N:-1:1
      xk = X(:,k);
      uk = U(k);
      xr = xref(:,k);

      [fx, fu] = linearize_dynamics(xk, uk, m, l, g, dt);

      dx = xk - xr;
      lx = Q * dx;
      lu = R * uk;
      lxx = Q;
      luu = R;

      Qx = lx + fx' * Vx;
      Qu = lu + fu' * Vx;
      Qxx = lxx + fx' * Vxx * fx;
      Quu = luu + fu' * Vxx * fu + lambda;
      Qux = fu' * Vxx * fx;

      k_local = -Quu \ Qu;
      K_local = -Quu \ Qux;

      k_ff(k) = k_local;
      K(1,:,k) = K_local;

      Vx = Qx + K_local' * Quu * k_local + K_local' * Qu + Qux' * k_local;
      Vxx = Qxx + K_local' * Quu * K_local + K_local' * Qux + Qux' * K_local;
      Vxx = 0.5 * (Vxx + Vxx');
    end

    % forward pass
    Xnew = zeros(size(X));
    Unew = zeros(size(U));
    Xnew(:,1) = x0(:);

    alpha_list = [1.0, 0.5, 0.25, 0.1];
    Jold = cost_total(X, U, xref, Q, R, P_N, N);

    improved = false;
    for a = alpha_list
      for k = 1:N
        du = a * k_ff(k) + squeeze(K(1,:,k)) * (Xnew(:,k) - X(:,k));
        Unew(k) = U(k) + du;
        Xnew(:,k+1) = dynamics(Xnew(:,k), Unew(k), m, l, g, dt);
      end
      Jnew = cost_total(Xnew, Unew, xref, Q, R, P_N, N);
      if Jnew < Jold
        X = Xnew; U = Unew;
        improved = true;
        break;
      end
    end

    if ~improved
      break;
    end
  end
end

function xnext = dynamics(x, u, m, l, g, dt)
  theta = x(1); dtheta = x(2);
  ddtheta = (-g / l) * sin(theta) + u / (m * l^2);
  xnext = [theta + dt * dtheta;
           dtheta + dt * ddtheta];
end

function [fx, fu] = linearize_dynamics(x, u, m, l, g, dt)
  theta = x(1);
  ddtheta_dtheta = (-g / l) * cos(theta);
  ddtheta_du = 1.0 / (m * l^2);

  fx = eye(2);
  fx(1,2) = dt;
  fx(2,1) = dt * ddtheta_dtheta;
  fx(2,2) = 1.0;

  fu = [0; dt * ddtheta_du];
end

function J = cost_total(X, U, xref, Q, R, P_N, N)
  J = 0;
  for k = 1:N
    dx = X(:,k) - xref(:,k);
    J = J + 0.5 * dx' * Q * dx + 0.5 * R * U(k)^2;
  end
  dxN = X(:,N+1) - xref(:,N+1);
  J = J + 0.5 * dxN' * P_N * dxN;
end
      

In Simulink, a discrete-time subsystem can call ilqr_pendulum_matlab at a slower planning rate to compute an open-loop optimized torque sequence, while a low-level PD loop runs at the base sample time.

Mathematica is convenient for symbolically computing derivatives required by DDP (including second derivatives of dynamics). Below is a minimal example for a scalar nonlinear system with cost, showing how to obtain expressions for the Q-function derivatives.

(* Define symbols *)
Clear[x, u];
f[x_, u_] := x + dt * (Sin[x] + u); (* sample nonlinear system *)
ell[x_, u_] := 1/2 q x^2 + 1/2 r u^2;

(* Value function at stage k+1 (quadratic approximation) *)
Vnext[x_] := v0 + vx x + 1/2 vxx x^2;

(* Q-function *)
Q[x_, u_] := ell[x, u] + Vnext[f[x, u]];

(* First and second derivatives of Q *)
Qx = D[Q[x, u], x];
Qu = D[Q[x, u], u];
Qxx = D[Q[x, u], {x, 2}];
Quu = D[Q[x, u], {u, 2}];
Qux = D[Q[x, u], x, u];

(* Optimal control law in terms of symbolic derivatives *)
kSym = -Simplify[Inverse[Quu] . Qu /. x -> 0 /. u -> 0];
KSym = -Simplify[Inverse[Quu] . Qux /. x -> 0 /. u -> 0];

kSym
KSym
      

The resulting expressions for kSym and KSym can be inspected, simplified, and then exported as C or MATLAB code to be used in a real-time controller for a robot manipulator.

Problem 1 (Bellman Recursion for a Manipulator): Consider a discrete-time manipulator model \( x_{k+1} = f(x_k,u_k) \) with quadratic cost as in Section 2. Derive the Bellman recursion for \( V_k(x) \) and show that for a linear system with quadratic cost (LQR case) the optimal control is linear in \( x_k \).

Solution: The Bellman recursion is

\[ V_N(x) = \phi(x), \qquad V_k(x) = \min_{u} \bigl\{ \ell(x,u) + V_{k+1}(f(x,u)) \bigr\}. \]

For a linear system \( x_{k+1} = A x_k + B u_k \) and quadratic cost, we can assume \( V_k(x) = \tfrac{1}{2} x^\top P_k x \). Substituting into the recursion and completing the square in \( u \) yields

\[ V_k(x) = \tfrac{1}{2} x^\top P_k x, \quad u_k^\star = -K_k x_k, \]

with \( K_k = (R_k + B^\top P_{k+1} B)^{-1} B^\top P_{k+1} A \) and \( P_k \) solving the discrete-time Riccati recursion, as studied in Lesson 2.

Problem 2 (Scalar iLQR Step): Consider a scalar nonlinear system \( x_{k+1} = f(x_k,u_k) \) and quadratic stage cost \( \ell(x_k,u_k) = \tfrac{1}{2} q x_k^2 + \tfrac{1}{2} r u_k^2 \). Assuming a quadratic approximation of \( V_{k+1}(x) \) with derivative parameters \( V_{x,k+1} \) and \( V_{xx,k+1} \), write the expressions for \( Q_x, Q_u, Q_{xx}, Q_{uu} \) and the optimal local control law \( \delta u_k^\star \).

Solution: Using \( f_x = \partial f/\partial x \) and \( f_u = \partial f/\partial u \) at the nominal point,

\[ \begin{aligned} Q_x &= q\,\bar{x}_k + f_x V_{x,k+1}, \\ Q_u &= r\,\bar{u}_k + f_u V_{x,k+1}, \\ Q_{xx} &= q + f_x^2 V_{xx,k+1}, \\ Q_{uu} &= r + f_u^2 V_{xx,k+1}. \end{aligned} \]

The local optimal control perturbation is

\[ \delta u_k^\star = k_k + K_k \delta x_k, \quad k_k = - Q_{uu}^{-1} Q_u, \quad K_k = - Q_{uu}^{-1} Q_{ux}, \]

where \( Q_{ux} = f_u f_x V_{xx,k+1} \) in the scalar case.

Problem 3 (Connection to Newton's Method): Explain qualitatively why iLQR can be seen as a Newton or Gauss–Newton method applied to the optimal control problem in the space of control sequences \( \mathbf{u} = (u_0,\dots,u_{N-1}) \).

Solution: The cost \( J(\mathbf{u}) \) is a nonlinear function of the control sequence via the dynamics. The iLQR backward pass computes a local quadratic approximation of \( J \) around the nominal control sequence; the resulting affine update \( \delta u_k = k_k + K_k \delta x_k \) corresponds to a Newton step in control space, using an approximate Hessian \( Q_{uu,k} \). Ignoring dynamics second derivatives yields a Gauss–Newton approximation. The forward pass with line search is the standard globalization strategy used with Newton methods.

Problem 4 (Positive Definiteness of \( Q_{uu} \)): Why is it important that \( Q_{uu,k} \) be positive definite at each time step in iLQR or DDP, and how does regularization help when this condition is violated?

Solution: Positive definiteness of \( Q_{uu,k} \) guarantees that the quadratic model of \( Q_k \) is strictly convex in \( \delta u_k \), so the stationary point is a local minimizer and \( Q_{uu,k}^{-1} \) exists. If \( Q_{uu,k} \) is indefinite or singular, the quadratic model may predict unbounded descent directions, leading to numerical instability. Adding a regularization term \( \lambda I \) to \( Q_{uu,k} \) shifts its eigenvalues and can enforce positive definiteness, at the cost of a more conservative update.

Problem 5 (Manipulator-Specific Interpretation): For a manipulator with state \( x = (q,\dot{q}) \) and cost tracking a desired joint trajectory, interpret the feedback term \( K_k \delta x_k \) and the feedforward term \( k_k \) in terms of joint servoing and nominal torque profiles.

Solution: The feedforward term \( k_k \) adjusts the nominal torque profile \( \bar{u}_k \) to reduce the cost even if the state is exactly at the nominal trajectory; it corresponds to improving the open-loop torque plan. The feedback term \( K_k \delta x_k \) introduces a linear state-feedback component around the nominal trajectory, analogous to a time-varying PD controller whose gains are optimized by the backward pass. Together they yield a time-varying local stabilizing controller around the optimized motion.

In this lesson we formulated nonlinear finite-horizon optimal control for robot manipulators and introduced DDP and iLQR as practical trajectory-optimization algorithms based on local quadratic approximations of the Bellman recursion. Starting from a nominal trajectory, we derived the backward pass (Q-function expansion, local Riccati-like equations), the forward rollout, and the role of regularization. We implemented iLQR for a pendulum in Python, C++, Java, MATLAB and Mathematica, setting the stage for the next lesson on real-time feasibility and trade-offs between optimality and computation.