The lab focuses on a single-link rotary manipulator with torque input. You will:

• Write the nonlinear dynamics in state-space form.
• Linearize and discretize the dynamics around an equilibrium.
• Implement finite-horizon LQR to stabilize the equilibrium.
• Implement iLQR to track a time-varying reference trajectory using the full nonlinear dynamics.
• Compare closed-loop performance and control effort.

Conceptually, the lab realizes the optimal control ideas from Lessons 2 and 3 in a reproducible simulation workflow.

flowchart TD
  A["Define single-link model xdot = f(x, u)"] --> B["Choose equilibrium (q*, dq* = 0, tau*)"]
  B --> C["Linearize to get (A, B), then discretize (Ad, Bd)"]
  C --> D["Design finite-horizon LQR via Riccati recursion"]
  D --> E["Simulate stabilization from perturbed initial states"]
  E --> F["Specify reference trajectory x_ref(k), u_ref(k)"]
  F --> G["Run iLQR: rollout, linearize, backward pass, forward pass"]
  G --> H["Compare trajectories, costs, and control effort"]
        

Consider a single-link rotary manipulator of mass \( m \), center-of-mass distance \( \ell \), inertia \( I = m\ell^2 \), viscous damping \( b \), and torque input \( \tau \). The configuration is the joint angle \( q \), measured from the downward vertical; the dynamics are a special case of the standard manipulator equation:

\[ I \ddot{q} + b \dot{q} + m g \ell \sin(q) = \tau. \]

Define the state \( \mathbf{x} = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \) and input \( u = \tau \). Then the continuous-time state-space model is:

\[ \dot{\mathbf{x}} = \begin{bmatrix} \dot{q} \\ \dfrac{1}{I}\big(u - b\dot{q} - m g \ell \sin(q)\big) \end{bmatrix} \;=\; \mathbf{f}(\mathbf{x},u). \]

For numerical simulation and digital control, we discretize with sampling time \( \Delta t \). Using forward Euler for simplicity:

\[ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \,\mathbf{f}(\mathbf{x}_k, u_k). \]

Higher-order schemes (e.g., Runge–Kutta) can be used in practice, but the discrete-time optimal control recursions depend only on the discrete map \( \mathbf{x}_{k+1} = \mathbf{f}_d(\mathbf{x}_k,u_k) \).

3.1 Equilibrium and Linearization

Choose an equilibrium \( (q^\star,\dot{q}^\star,u^\star) \) such that \( \dot{\mathbf{x}} = \mathbf{0} \). Imposing \( \dot{q}^\star = 0 \) and \( \ddot{q}^\star = 0 \) gives:

\[ u^\star = m g \ell \sin(q^\star). \]

For small deviations around this equilibrium, define \( \delta \mathbf{x} = \mathbf{x} - \mathbf{x}^\star \) and \( \delta u = u - u^\star \). The linearization of \( \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x},u) \) around \( (\mathbf{x}^\star,u^\star) \) is:

\[ \delta\dot{\mathbf{x}} = A_c \,\delta\mathbf{x} + B_c \,\delta u, \]

where

\[ A_c = \left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right|_{\mathbf{x}^\star,u^\star} = \begin{bmatrix} 0 & 1 \\ -\dfrac{m g \ell}{I}\cos(q^\star) & -\dfrac{b}{I} \end{bmatrix}, \quad B_c = \left.\frac{\partial \mathbf{f}}{\partial u}\right|_{\mathbf{x}^\star,u^\star} = \begin{bmatrix} 0 \\ \dfrac{1}{I} \end{bmatrix}. \]

A standard zero-order-hold discretization (with constant input on each sampling interval) can be approximated by:

\[ A_d \approx I_2 + \Delta t\, A_c, \qquad B_d \approx \Delta t\, B_c, \]

where \( I_2 \) is the identity matrix of size 2. In practice, \( A_d,B_d \) can be computed more accurately via matrix exponentials, but this approximation is sufficient for a small \( \Delta t \) in the lab.

3.2 Finite-Horizon LQR Riccati Recursion

For the discrete-time linear system \( \delta\mathbf{x}_{k+1} = A_d \delta\mathbf{x}_k + B_d \delta u_k \) consider the quadratic cost

\[ J = \sum_{k=0}^{N-1} \Big( \delta\mathbf{x}_k^\top Q \,\delta\mathbf{x}_k + \delta u_k^\top R \,\delta u_k \Big) + \delta\mathbf{x}_N^\top Q_f \,\delta\mathbf{x}_N, \]

where \( Q,Q_f \succeq 0 \) and \( R \succ 0 \). In Lesson 2 we saw that the optimal feedback has the form \( \delta u_k = K_k \delta\mathbf{x}_k \), with \( K_k \) obtained via dynamic programming. Assume a quadratic value function:

\[ V_k(\delta\mathbf{x}_k) = \delta\mathbf{x}_k^\top P_k \delta\mathbf{x}_k, \quad P_N = Q_f. \]

For a given \( P_{k+1} \), the cost-to-go at step \( k \) is

\[ \begin{aligned} J_k(\delta\mathbf{x}_k,\delta u_k) &= \delta\mathbf{x}_k^\top Q \,\delta\mathbf{x}_k + \delta u_k^\top R \,\delta u_k + (A_d\delta\mathbf{x}_k + B_d\delta u_k)^\top P_{k+1} (A_d\delta\mathbf{x}_k + B_d\delta u_k). \end{aligned} \]

Completing the square in \( \delta u_k \) and minimizing yields the optimal control law

\[ \delta u_k^\star = K_k \delta\mathbf{x}_k, \quad K_k = -\big(R + B_d^\top P_{k+1} B_d\big)^{-1} B_d^\top P_{k+1} A_d. \]

Substituting back gives the Riccati recursion:

\[ P_k = Q + A_d^\top P_{k+1} A_d - A_d^\top P_{k+1} B_d \big(R + B_d^\top P_{k+1} B_d\big)^{-1} B_d^\top P_{k+1} A_d. \]

Backward iteration from \( P_N = Q_f \) yields \( P_k,K_k \) for all \( k=0,\dots,N-1 \). In the lab we use this recursion directly rather than calling a black-box Riccati solver, to make the connection to dynamic programming explicit.

4.1 Tracking Cost and Local Models

For iLQR we work with the full nonlinear discrete dynamics \( \mathbf{x}_{k+1} = \mathbf{f}_d(\mathbf{x}_k,u_k) \), where \( \mathbf{f}_d \) is obtained from numerical integration of the continuous dynamics. Given a reference trajectory \( \mathbf{x}_k^{\text{ref}}, u_k^{\text{ref}} \), we use the quadratic tracking cost

\[ J = \sum_{k=0}^{N-1} \Big( (\mathbf{x}_k - \mathbf{x}_k^{\text{ref}})^\top Q (\mathbf{x}_k - \mathbf{x}_k^{\text{ref}}) + (u_k - u_k^{\text{ref}})^\top R (u_k - u_k^{\text{ref}}) \Big) + (\mathbf{x}_N - \mathbf{x}_N^{\text{ref}})^\top Q_f (\mathbf{x}_N - \mathbf{x}_N^{\text{ref}}). \]

iLQR iteratively improves a control sequence \( \{u_k\}_{k=0}^{N-1} \). At iteration \( i \), we have a nominal trajectory \( \{\mathbf{x}_k^{(i)},u_k^{(i)}\} \). Linearizing dynamics and quadratically expanding the cost around this trajectory gives local models:

\[ \delta\mathbf{x}_{k+1} \approx A_k \delta\mathbf{x}_k + B_k \delta u_k, \quad A_k = \left.\frac{\partial \mathbf{f}_d}{\partial \mathbf{x}}\right|_{\mathbf{x}_k^{(i)},u_k^{(i)}}, \quad B_k = \left.\frac{\partial \mathbf{f}_d}{\partial u}\right|_{\mathbf{x}_k^{(i)},u_k^{(i)}}, \]

and local \( Q \)-function derivatives \( Q_x,Q_u,Q_{xx},Q_{ux},Q_{uu} \) that mirror the LQR expressions (see Lesson 3).

4.2 Backward and Forward Pass

The backward pass computes feedforward and feedback terms \( \mathbf{k}_k,K_k \) such that the control update is \( \delta u_k = \mathbf{k}_k + K_k \,\delta\mathbf{x}_k \). Denote by \( V_x^{k+1},V_{xx}^{k+1} \) the gradient and Hessian of the value function at time \( k+1 \). Then:

\[ \begin{aligned} Q_x &= \ell_x + A_k^\top V_x^{k+1}, \\ Q_u &= \ell_u + B_k^\top V_x^{k+1}, \\ Q_{xx} &= \ell_{xx} + A_k^\top V_{xx}^{k+1} A_k, \\ Q_{ux} &= \ell_{ux} + B_k^\top V_{xx}^{k+1} A_k, \\ Q_{uu} &= \ell_{uu} + B_k^\top V_{xx}^{k+1} B_k. \end{aligned} \]

Assuming \( Q_{uu} \) is positive definite, we set

\[ \mathbf{k}_k = -Q_{uu}^{-1} Q_u, \qquad K_k = -Q_{uu}^{-1} Q_{ux}. \]

and update the value function derivatives:

\[ \begin{aligned} V_x^k &= Q_x + K_k^\top Q_{uu} \mathbf{k}_k + K_k^\top Q_u + Q_{ux}^\top \mathbf{k}_k, \\ V_{xx}^k &= Q_{xx} + K_k^\top Q_{uu} K_k + K_k^\top Q_{ux} + Q_{ux}^\top K_k. \end{aligned} \]

The forward pass generates a new control sequence using a line-search parameter \( \alpha \in (0,1] \):

\[ u_k^{(i+1)} = u_k^{(i)} + \alpha \mathbf{k}_k + K_k \big(\mathbf{x}_k^{(i+1)} - \mathbf{x}_k^{(i)}\big), \]

with \( \mathbf{x}_k^{(i+1)} \) obtained by simulating the nonlinear dynamics forward. The process is repeated until the cost reduction is below a tolerance.

flowchart TD
  S["Initialize u(k) (e.g., zeros or PD)"] --> R["Rollout nonlinear dynamics x(k+1) = f_d(x(k), u(k))"]
  R --> L["Linearize dynamics and cost along trajectory"]
  L --> BWD["Backward pass: compute k(k), \nK(k) via local quadratic Q"]
  BWD --> FWD["Forward pass: apply control update, \nsimulate new trajectory"]
  FWD --> C["Check cost decrease and convergence"]
  C -->|not converged| R
  C -->|converged| DONE["Return optimized u(k), x(k)"]
        

Below is a compact Python implementation that demonstrates (i) finite horizon LQR for stabilization of the single-link equilibrium and (ii) a minimal iLQR loop for trajectory tracking. For a real lab, you would add plotting and logging utilities and possibly use robotics libraries for multibody dynamics; here we keep dynamics explicit.

import numpy as np

# Physical parameters
m = 1.0
ell = 1.0
I = m * ell**2
b = 0.1
g = 9.81
dt = 0.01

def dynamics(x, u):
    """Continuous-time dynamics xdot = f(x, u), x = [q, dq]."""
    q, dq = x
    ddq = (u - b * dq - m * g * ell * np.sin(q)) / I
    return np.array([dq, ddq])

def step(x, u):
    """Discrete-time step with forward Euler."""
    return x + dt * dynamics(x, u)

# Linearization around equilibrium (q*, dq* = 0, u* = m g ell sin(q*))
q_star = 0.0  # downward equilibrium
dq_star = 0.0
u_star = m * g * ell * np.sin(q_star)
x_star = np.array([q_star, dq_star])

A_c = np.array([[0.0, 1.0],
                [-(m * g * ell / I) * np.cos(q_star), -b / I]])
B_c = np.array([[0.0],
                [1.0 / I]])

A_d = np.eye(2) + dt * A_c
B_d = dt * B_c

# LQR gains via finite-horizon Riccati recursion
N = 500  # horizon
Q = np.diag([10.0, 1.0])
R = np.array([[0.1]])
Qf = np.diag([50.0, 5.0])

P = [None] * (N + 1)
K = [None] * N
P[N] = Qf

for k in range(N - 1, -1, -1):
    S = R + B_d.T @ P[k + 1] @ B_d
    K[k] = -np.linalg.solve(S, B_d.T @ P[k + 1] @ A_d)
    Acl = A_d + B_d @ K[k]
    P[k] = Q + Acl.T @ P[k + 1] @ Acl

def simulate_lqr(x0, steps=500):
    xs = np.zeros((steps + 1, 2))
    us = np.zeros(steps)
    xs[0] = x0
    for k in range(steps):
        dx = xs[k] - x_star
        u = float(u_star + K[min(k, N - 1)] @ dx)
        xs[k + 1] = step(xs[k], u)
        us[k] = u
    return xs, us

# Minimal iLQR implementation for tracking a reference
def rollout(x0, us):
    xs = [x0]
    for u in us:
        xs.append(step(xs[-1], u))
    return np.array(xs)

def ilqr(x0, x_ref, u_init, Q, R, Qf, max_iter=50, alpha_list=None):
    if alpha_list is None:
        alpha_list = [1.0, 0.5, 0.25, 0.1]
    N = len(u_init)
    n = x0.shape[0]
    us = u_init.copy()
    xs = rollout(x0, us)
    for it in range(max_iter):
        Vx = 2.0 * Qf @ (xs[-1] - x_ref[-1])
        Vxx = 2.0 * Qf.copy()
        ks = [None] * N
        Ks = [None] * N

        # Backward pass (finite-difference Jacobians for simplicity)
        for k in range(N - 1, -1, -1):
            xk = xs[k]
            uk = us[k]
            xr = x_ref[k]
            ur = 0.0  # here reference torque is zero

            # Cost derivatives (quadratic tracking)
            lx = 2.0 * Q @ (xk - xr)
            lu = 2.0 * R @ np.array([[uk - ur]])
            lxx = 2.0 * Q
            luu = 2.0 * R.copy()
            lux = np.zeros((1, n))

            # Numerical linearization of dynamics
            eps = 1e-5
            A = np.zeros((n, n))
            B = np.zeros((n, 1))
            fx = step(xk, uk)
            for i in range(n):
                dx = np.zeros(n)
                dx[i] = eps
                A[:, i] = (step(xk + dx, uk) - fx) / eps
            du = 1e-5
            B[:, 0] = (step(xk, uk + du) - fx) / du

            Qx = lx + A.T @ Vx
            Qu = lu + B.T @ Vx
            Qxx = lxx + A.T @ Vxx @ A
            Quu = luu + B.T @ Vxx @ B
            Qux = lux + B.T @ Vxx @ A

            # Regularization for numerical stability
            Quu_reg = Quu + 1e-6 * np.eye(Quu.shape[0])
            k_ff = -np.linalg.solve(Quu_reg, Qu)
            K_fb = -np.linalg.solve(Quu_reg, Qux)

            ks[k] = k_ff
            Ks[k] = K_fb

            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
            Vxx = 0.5 * (Vxx + Vxx.T)  # symmetrize

        # Forward pass with line search
        cost_old = compute_cost(xs, us, x_ref, Q, R, Qf)
        improved = False
        for alpha in alpha_list:
            xs_new = [x0]
            us_new = []
            for k in range(N):
                dx = xs_new[-1] - xs[k]
                du = alpha * ks[k] + Ks[k] @ dx
                u_new = us[k] + float(du)
                us_new.append(u_new)
                xs_new.append(step(xs_new[-1], u_new))
            xs_new = np.array(xs_new)
            us_new = np.array(us_new)
            cost_new = compute_cost(xs_new, us_new, x_ref, Q, R, Qf)
            if cost_new < cost_old:
                xs, us = xs_new, us_new
                improved = True
                break
        if not improved:
            break
    return xs, us

def compute_cost(xs, us, x_ref, Q, R, Qf):
    J = 0.0
    for k in range(len(us)):
        dx = xs[k] - x_ref[k]
        du = us[k]
        J += dx.T @ Q @ dx + R * du * du
    dxN = xs[-1] - x_ref[-1]
    J += dxN.T @ Qf @ dxN
    return float(J)
      

In a teaching environment you can simplify iLQR by fixing Jacobians analytically for the single-link dynamics or by using automatic differentiation libraries instead of finite differences.

In C++, Eigen is a natural choice for small-matrix manipulations. The following sketch shows how to implement the Riccati recursion and simulate LQR stabilization of the single-link model. The nonlinear iLQR loop reuses the same ideas with linearization and quadratic expansion implemented numerically or via analytic Jacobians.

#include <iostream>
#include <Eigen/Dense>

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

// Physical parameters
double m = 1.0;
double ell = 1.0;
double Iinertia = m * ell * ell;
double b = 0.1;
double g = 9.81;
double dt = 0.01;

// Continuous-time dynamics xdot = f(x,u) for single-link
Vector2d dynamics(const Vector2d& x, double u) {
    double q = x(0);
    double dq = x(1);
    double ddq = (u - b * dq - m * g * ell * std::sin(q)) / Iinertia;
    Vector2d xdot;
    xdot << dq, ddq;
    return xdot;
}

Vector2d step(const Vector2d& x, double u) {
    return x + dt * dynamics(x, u);
}

int main() {
    // Equilibrium at q* = 0, dq* = 0
    double q_star = 0.0;
    double dq_star = 0.0;
    double u_star = m * g * ell * std::sin(q_star);
    Vector2d x_star(q_star, dq_star);

    // Linearization matrices
    Matrix2d A_c;
    A_c << 0.0, 1.0,
             -(m * g * ell / Iinertia) * std::cos(q_star), -b / Iinertia;
    Eigen::Vector2d B_c_vec(0.0, 1.0 / Iinertia);
    MatrixXd B_c(2, 1);
    B_c.col(0) = B_c_vec;

    Matrix2d A_d = Matrix2d::Identity() + dt * A_c;
    MatrixXd B_d = dt * B_c;

    // Cost matrices
    Matrix2d Q;
    Q << 10.0, 0.0,
           0.0, 1.0;
    Matrix2d Qf;
    Qf << 50.0, 0.0,
            0.0, 5.0;
    Eigen::MatrixXd R(1, 1);
    R(0, 0) = 0.1;

    int N = 500;
    std::vector<Matrix2d> P(N + 1);
    std::vector<Eigen::RowVector2d> K(N);

    P[N] = Qf;
    for (int k = N - 1; k >= 0; --k) {
        Eigen::MatrixXd S = R + B_d.transpose() * P[k + 1] * B_d;
        Eigen::RowVector2d Kk =
            -S.ldlt().solve(B_d.transpose() * P[k + 1] * A_d);
        K[k] = Kk;
        Matrix2d Acl = A_d + B_d * Kk;
        P[k] = Q + Acl.transpose() * P[k + 1] * Acl;
    }

    // Simulate LQR stabilization
    Vector2d x;
    x << 0.5, 0.0;  // perturbed initial angle
    for (int k = 0; k < 500; ++k) {
        Vector2d dx = x - x_star;
        double u = u_star + (K[std::min(k, N - 1)] * dx)(0);
        x = step(x, u);
        std::cout << k * dt << " " << x(0) << " " << x(1) << " " << u << "\n";
    }
    return 0;
}
      

For iLQR, one can implement a class that stores a control sequence std::vector<double> and trajectories, computes Jacobians via finite differences, and performs the backward/forward passes analogous to the Python implementation.

In Java, the EJML library provides convenient dense linear algebra. Below is an outline for the finite-horizon LQR recursion using SimpleMatrix. The manipulator dynamics can be coded in a similar style as in Python and C++.

import org.ejml.simple.SimpleMatrix;

public class SingleLinkLQR {
    static final double m = 1.0;
    static final double ell = 1.0;
    static final double I = m * ell * ell;
    static final double b = 0.1;
    static final double g = 9.81;
    static final double dt = 0.01;

    static SimpleMatrix dynamics(SimpleMatrix x, double u) {
        double q = x.get(0);
        double dq = x.get(1);
        double ddq = (u - b * dq - m * g * ell * Math.sin(q)) / I;
        return new SimpleMatrix(2, 1, true, new double[]{dq, ddq});
    }

    static SimpleMatrix step(SimpleMatrix x, double u) {
        return x.plus(dynamics(x, u).scale(dt));
    }

    public static void main(String[] args) {
        double qStar = 0.0;
        double dqStar = 0.0;
        double uStar = m * g * ell * Math.sin(qStar);
        SimpleMatrix xStar = new SimpleMatrix(2, 1, true, new double[]{qStar, dqStar});

        // Linearization
        SimpleMatrix A_c = new SimpleMatrix(2, 2, true, new double[]{
            0.0, 1.0,
            -(m * g * ell / I) * Math.cos(qStar), -b / I
        });
        SimpleMatrix B_c = new SimpleMatrix(2, 1, true, new double[]{0.0, 1.0 / I});
        SimpleMatrix A_d = SimpleMatrix.identity(2).plus(A_c.scale(dt));
        SimpleMatrix B_d = B_c.scale(dt);

        SimpleMatrix Q = new SimpleMatrix(2, 2, true, new double[]{
            10.0, 0.0,
            0.0, 1.0
        });
        SimpleMatrix Qf = new SimpleMatrix(2, 2, true, new double[]{
            50.0, 0.0,
            0.0, 5.0
        });
        SimpleMatrix R = new SimpleMatrix(1, 1, true, new double[]{0.1});

        int N = 500;
        SimpleMatrix[] P = new SimpleMatrix[N + 1];
        SimpleMatrix[] K = new SimpleMatrix[N];

        P[N] = Qf.copy();
        for (int k = N - 1; k >= 0; --k) {
            SimpleMatrix S = R.plus(B_d.transpose().mult(P[k + 1]).mult(B_d));
            SimpleMatrix Kk = S.solve(B_d.transpose().mult(P[k + 1]).mult(A_d)).negative();
            K[k] = Kk;
            SimpleMatrix Acl = A_d.plus(B_d.mult(Kk));
            P[k] = Q.plus(Acl.transpose().mult(P[k + 1]).mult(Acl));
        }

        // Simulate LQR stabilization
        SimpleMatrix x = new SimpleMatrix(2, 1, true, new double[]{0.5, 0.0});
        for (int k = 0; k < 500; ++k) {
            SimpleMatrix dx = x.minus(xStar);
            SimpleMatrix Kk = K[Math.min(k, N - 1)];
            double u = uStar + Kk.mult(dx).get(0);
            x = step(x, u);
            System.out.println(k * dt + " " + x.get(0) + " " + x.get(1) + " " + u);
        }
    }
}
      

Extending to iLQR follows the same algorithmic structure: preallocate arrays of SimpleMatrix for A[k], B[k], Vx[k], Vxx[k], K[k], k[k] and embed the backward/forward passes in loops.

MATLAB has built-in LQR tools; however, to align with the theoretical derivations, we also show explicit Riccati recursion. The Simulink model can use a custom MATLAB Function block to implement the dynamics and a discrete-time state-feedback block for the LQR law.

% Parameters
m = 1.0;
ell = 1.0;
I = m * ell^2;
b = 0.1;
g = 9.81;
dt = 0.01;

q_star = 0.0;
dq_star = 0.0;
u_star = m * g * ell * sin(q_star);
x_star = [q_star; dq_star];

% Linearization
A_c = [0, 1;
       -m * g * ell / I * cos(q_star), -b / I];
B_c = [0;
       1 / I];

A_d = eye(2) + dt * A_c;
B_d = dt * B_c;

Q = diag([10, 1]);
Qf = diag([50, 5]);
R = 0.1;

N = 500;
P = cell(N + 1, 1);
K = cell(N, 1);
P{N + 1} = Qf;

for k = N:-1:1
    S = R + B_d' * P{k + 1} * B_d;
    K{k} = -S \ (B_d' * P{k + 1} * A_d);
    Acl = A_d + B_d * K{k};
    P{k} = Q + Acl' * P{k + 1} * Acl;
end

% Simulation
steps = 500;
x = [0.5; 0.0];
xs = zeros(2, steps + 1);
us = zeros(1, steps);
xs(:, 1) = x;

for k = 1:steps
    dx = x - x_star;
    Kk = K{min(k, N)};
    u = u_star + Kk * dx;
    % Forward Euler integration of nonlinear dynamics
    q = x(1);
    dq = x(2);
    ddq = (u - b * dq - m * g * ell * sin(q)) / I;
    x = x + dt * [dq; ddq];
    xs(:, k + 1) = x;
    us(k) = u;
end

% Plot results
t = dt * (0:steps);
figure; subplot(2,1,1); plot(t, xs(1,:), t, xs(2,:));
legend('q', 'dq'); grid on;
subplot(2,1,2); plot(t(1:end-1), us); grid on; ylabel('tau');
      

In Simulink, you can implement the nonlinear dynamics as a MATLAB Function block that outputs xddot given q,dq,tau, integrate it with a Discrete-Time Integrator (Euler or more accurate scheme), and apply the state-feedback law tau = u_star + K * (x - x_star) via a Gain block.

The following Mathematica code shows a simple finite-horizon LQR design and simulation for the single-link manipulator. The Riccati recursion is implemented explicitly for didactic clarity.

(* Parameters *)
m = 1.0;
ell = 1.0;
I = m*ell^2;
b = 0.1;
g = 9.81;
dt = 0.01;

qStar = 0.0;
dqStar = 0.0;
uStar = m*g*ell*Sin[qStar];
xStar = {qStar, dqStar};

(* Dynamics and step *)
f[{q_, dq_}, u_] := Module[{ddq},
  ddq = (u - b*dq - m*g*ell*Sin[q])/I;
  {dq, ddq}
];

step[x_, u_] := x + dt*f[x, u];

(* Linearization matrices *)
Ac = { {0.0, 1.0},
      {-(m*g*ell/I)*Cos[qStar], -b/I} };
Bc = { {0.0}, {1.0/I} };

Ad = IdentityMatrix[2] + dt*Ac;
Bd = dt*Bc;

Q = DiagonalMatrix[{10.0, 1.0}];
Qf = DiagonalMatrix[{50.0, 5.0}];
R = {{0.1}};

nH = 500;
P = ConstantArray[0, {nH + 1, 2, 2}];
K = ConstantArray[0, {nH, 1, 2}];

P[[nH + 1]] = Qf;

Do[
  Module[{Pk1, S, Kk, Acl},
    Pk1 = P[[k + 1]];
    S = R + Transpose[Bd].Pk1.Bd;
    Kk = -Inverse[S].Transpose[Bd].Pk1.Ad;
    K[[k]] = Kk;
    Acl = Ad + Bd.Kk;
    P[[k]] = Q + Transpose[Acl].Pk1.Acl;
  ],
  {k, nH, 1, -1}
];

(* Simulation *)
steps = 500;
x = {0.5, 0.0};
xs = ConstantArray[0.0, {steps + 1, 2}];
us = ConstantArray[0.0, steps];
xs[[1]] = x;

Do[
  Module[{dx, kIdx, u},
    kIdx = Min[k, nH];
    dx = x - xStar;
    u = uStar + (K[[kIdx]].dx)[[1]];
    x = step[x, u];
    xs[[k + 1]] = x;
    us[[k]] = u;
  ],
  {k, 1, steps}
];

t = dt*Range[0, steps];
ListLinePlot[
  {
    Transpose[{t, xs[[All, 1]]}],
    Transpose[{t, xs[[All, 2]]}]
  },
  PlotLegends -> {"q", "dq"},
  GridLines -> Automatic
]
      

An iLQR implementation in Mathematica closely parallels the Python code: use Table and Do loops to construct arrays of Jacobians and value function derivatives, and iterate the backward/forward passes.

Problem 1 (Linearization Check): For the single-link manipulator dynamics \( I \ddot{q} + b\dot{q} + m g \ell \sin(q) = \tau \) , derive the continuous-time linearized state-space matrices \( A_c,B_c \) around \( (q^\star,\dot{q}^\star,u^\star) \) with \( \dot{q}^\star = 0 \) and \( u^\star = m g \ell \sin(q^\star) \).

Solution:

With state \( \mathbf{x} = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \) and input \( u \), we have

\[ \dot{\mathbf{x}} = \begin{bmatrix} \dot{q} \\ \dfrac{1}{I}\big(u - b\dot{q} - m g \ell \sin(q)\big) \end{bmatrix} = \mathbf{f}(\mathbf{x},u). \]

The Jacobians are \( A_c = \frac{\partial \mathbf{f}}{\partial \mathbf{x}} \), \( B_c = \frac{\partial \mathbf{f}}{\partial u} \). We compute

\[ \frac{\partial \dot{q}}{\partial q} = 0,\quad \frac{\partial \dot{q}}{\partial \dot{q}} = 1, \]

\[ \frac{\partial \ddot{q}}{\partial q} = -\frac{m g \ell}{I}\cos(q), \quad \frac{\partial \ddot{q}}{\partial \dot{q}} = -\frac{b}{I}, \quad \frac{\partial \ddot{q}}{\partial u} = \frac{1}{I}. \]

Evaluating at \( (q^\star,\dot{q}^\star,u^\star) \) gives

\[ A_c = \begin{bmatrix} 0 & 1 \\ -\dfrac{m g \ell}{I}\cos(q^\star) & -\dfrac{b}{I} \end{bmatrix}, \quad B_c = \begin{bmatrix} 0 \\ \dfrac{1}{I} \end{bmatrix}, \]

which matches the matrices used in the lab implementations.

Problem 2 (Symmetry of Riccati Solution): Show that if \( Q,Q_f \) and \( R \) are symmetric and \( P_{k+1} \) is symmetric, then \( P_k \) defined by the discrete Riccati recursion

\[ P_k = Q + A_d^\top P_{k+1} A_d - A_d^\top P_{k+1} B_d \big(R + B_d^\top P_{k+1} B_d\big)^{-1} B_d^\top P_{k+1} A_d \]

is also symmetric.

Solution:

The first term \( Q \) is symmetric by assumption. For the second term, \( A_d^\top P_{k+1} A_d \) is symmetric if \( P_{k+1} \) is symmetric because \( (A_d^\top P_{k+1} A_d)^\top = A_d^\top P_{k+1}^\top A_d = A_d^\top P_{k+1} A_d \). For the third term, let \( S = R + B_d^\top P_{k+1} B_d \). Then \( S \) is symmetric and invertible, so \( S^{-1} \) is symmetric. The product

\[ A_d^\top P_{k+1} B_d S^{-1} B_d^\top P_{k+1} A_d \]

is of the form \( X S^{-1} X^\top \) with \( X = A_d^\top P_{k+1} B_d \), hence symmetric. The negative sign does not affect symmetry. Therefore all terms in \( P_k \) are symmetric, and so \( P_k = P_k^\top \). By backward induction from \( P_N = Q_f \), all \( P_k \) are symmetric.

Problem 3 (Scalar LQR Gain): Consider the scalar discrete-time system \( x_{k+1} = a x_k + b u_k \) with cost

\[ J = \sum_{k=0}^{\infty} (q x_k^2 + r u_k^2), \quad q > 0,\; r > 0, \]

and a stabilizable pair \( (a,b) \). Show that the infinite-horizon LQR feedback \( u_k = K x_k \) has gain

\[ K = -\frac{b p a}{r + b^2 p}, \]

where \( p \) solves the scalar algebraic Riccati equation

\[ p = q + a^2 p - \frac{(a b p)^2}{r + b^2 p}. \]

Solution:

For the scalar case, the Riccati and gain formulas reduce to \( P_k \in \mathbb{R} \) and \( K_k \in \mathbb{R} \). The finite-horizon Riccati recursion is

\[ P_k = q + a^2 P_{k+1} - \frac{(a b P_{k+1})^2}{r + b^2 P_{k+1}}, \quad K_k = -\frac{b P_{k+1} a}{r + b^2 P_{k+1}}. \]

In the infinite-horizon limit, set \( P_k = p \) and \( K_k = K \), yielding the algebraic Riccati equation and the gain expression \( K = -\dfrac{b p a}{r + b^2 p} \) as claimed.

Problem 4 (Positive-Definite \( Q_{uu} \) in iLQR): In the iLQR backward pass, why is the condition \( Q_{uu} \succ 0 \) important? What are the numerical consequences if \( Q_{uu} \) becomes indefinite, and how is this handled in practice?

Solution:

The matrix \( Q_{uu} \) is the Hessian of the local \( Q \)-function with respect to control. If \( Q_{uu} \succ 0 \), the quadratic model is locally strictly convex in \( \delta u_k \), so the stationary point given by \( \mathbf{k}_k = -Q_{uu}^{-1} Q_u \) is a unique local minimizer, and \( Q_{uu}^{-1} \) is well defined and numerically stable. If \( Q_{uu} \) is indefinite or nearly singular, the step may not be descent, and the linear system for \( \mathbf{k}_k,K_k \) is ill conditioned. In practice, iLQR implementations add a regularization term \( \lambda I \) to \( Q_{uu} \) (Levenberg–Marquardt style), or project to the nearest positive-definite matrix via eigenvalue clipping to enforce \( Q_{uu} \succ 0 \).

Problem 5 (Effect of Sampling Time): For the single-link LQR design, qualitatively describe how choosing \( \Delta t \) too large affects (i) the accuracy of the discrete-time model \( (A_d,B_d) \) and (ii) the closed-loop behavior under the LQR law.

Solution:

(i) A large \( \Delta t \) makes the approximation \( A_d \approx I + \Delta t A_c \) and \( B_d \approx \Delta t B_c \) inaccurate, especially for faster modes. The discrete-time eigenvalues may move far from the true discretized eigenvalues \( e^{\lambda \Delta t} \). (ii) The LQR gains are computed with respect to the approximate discrete-time model; if \( \Delta t \) is too large, the resulting controller can be overly aggressive or even destabilizing for the true sampled dynamics, and the discrete-time implementation may exhibit overshoot, oscillations, or aliasing of fast modes. Decreasing \( \Delta t \) improves the match between continuous- and discrete-time behavior and leads to more predictable LQR performance.

This lab translated the theoretical LQR and iLQR frameworks into concrete multi-language implementations for a single-link manipulator. We derived the linearized state-space model, implemented the finite- horizon Riccati recursion, and used it for stabilization. We then extended the dynamic-programming ideas to nonlinear iLQR, including local linearization, quadratic cost approximation, and backward/forward passes. These techniques provide a computational foundation for more advanced optimal controllers, including MPC, in subsequent chapters.