We assume a robot manipulator with \( n \) revolute joints and known dynamics (from previous chapters on kinematics and dynamics). Using an inner-loop model-based torque controller (e.g., computed torque), we approximate each joint's closed-loop error dynamics as a double integrator. The MPC we design in this lab acts as an outer loop for joint or task-space error regulation/tracking.

The continuous-time joint dynamics of an \( n \)-DOF manipulator are

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

where \( \mathbf{q} \in \mathbb{R}^{n} \) are joint angles, \( \boldsymbol{\tau} \in \mathbb{R}^{n} \) are actuator torques, and \( \mathbf{d} \) captures disturbances and unmodeled dynamics. With inner-loop compensation (using Chapters 3 and 9), we obtain approximately

\[ \ddot{\mathbf{e}}_q(t) \approx \mathbf{u}(t), \]

for the joint position error \( \mathbf{e}_q(t) = \mathbf{q}(t) - \mathbf{q}^{\mathrm{ref}}(t) \), where \( \mathbf{u} \) is an equivalent virtual input that we will generate with MPC and then map back to joint torques via the inner-loop controller.

With sampling period \( T_s \), define the discrete-time state \( \mathbf{x}_k = \begin{bmatrix} \mathbf{e}_q(k) \\ \dot{\mathbf{e}}_q(k) \end{bmatrix} \). A forward-Euler discretization of the double integrator yields

\[ \mathbf{x}_{k+1} = \underbrace{ \begin{bmatrix} \mathbf{I}_n & T_s \mathbf{I}_n \\ \mathbf{0} & \mathbf{I}_n \end{bmatrix}}_{\mathbf{A}} \mathbf{x}_k + \underbrace{ \begin{bmatrix} \mathbf{0} \\ T_s \mathbf{I}_n \end{bmatrix}}_{\mathbf{B}} \mathbf{u}_k. \]

This state-space model is the basis for our joint-space MPC. Task-space MPC will be obtained by composing this with the manipulator Jacobian and forward kinematics.

Consider a reference trajectory for joint positions and velocities, \( \mathbf{x}_k^{\mathrm{ref}} = \begin{bmatrix} \mathbf{e}_q^{\mathrm{ref}}(k) \\ \dot{\mathbf{e}}_q^{\mathrm{ref}}(k) \end{bmatrix} \), where typically \( \mathbf{e}_q^{\mathrm{ref}}(k) = \mathbf{0} \) and \( \dot{\mathbf{e}}_q^{\mathrm{ref}}(k) = \mathbf{0} \) for pure tracking of an outer trajectory \( \mathbf{q}^{\mathrm{ref}}(k) \). For a prediction horizon \( N \), define the cost

\[ J(\mathbf{x}_0,\mathbf{U}) = \sum_{k=0}^{N-1} \Big( (\mathbf{x}_k - \mathbf{x}_k^{\mathrm{ref} })^{\top} \mathbf{Q} (\mathbf{x}_k - \mathbf{x}_k^{\mathrm{ref} }) + (\mathbf{u}_k - \mathbf{u}_k^{\mathrm{ref} })^{\top} \mathbf{R} (\mathbf{u}_k - \mathbf{u}_k^{\mathrm{ref} }) \Big) + \\ (\mathbf{x}_N - \mathbf{x}_N^{\mathrm{ref} })^{\top} \mathbf{P} (\mathbf{x}_N - \mathbf{x}_N^{\mathrm{ref} }), \]

where \( \mathbf{Q} \succeq 0 \), \( \mathbf{P} \succeq 0 \) and \( \mathbf{R} \succ 0 \) weight state and input errors. Typically \( \mathbf{P} \) is chosen as the solution of the discrete-time algebraic Riccati equation corresponding to the unconstrained LQR problem.

Stack the predicted states and inputs as \( \mathbf{X} = \begin{bmatrix} \mathbf{x}_1^{\top} & \dots & \mathbf{x}_N^{\top} \end{bmatrix}^{\top} \) and \( \mathbf{U} = \begin{bmatrix} \mathbf{u}_0^{\top} & \dots & \mathbf{u}_{N-1}^{\top} \end{bmatrix}^{\top} \). From the linear dynamics we obtain the condensed prediction model

\[ \mathbf{X} = \boldsymbol{\Phi} \mathbf{x}_0 + \boldsymbol{\Gamma} \mathbf{U}, \]

where

\[ \boldsymbol{\Phi} = \begin{bmatrix} \mathbf{A} \\ \mathbf{A}^2 \\ \vdots \\ \mathbf{A}^{N} \end{bmatrix}, \quad \boldsymbol{\Gamma} = \begin{bmatrix} \mathbf{B} & \mathbf{0} & \dots & \mathbf{0} \\ \mathbf{A}\mathbf{B} & \mathbf{B} & \dots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{A}^{N-1}\mathbf{B} & \mathbf{A}^{N-2}\mathbf{B} & \dots & \mathbf{B} \end{bmatrix}. \]

Define block-diagonal weight matrices \( \bar{\mathbf{Q}} = \mathrm{diag}(\mathbf{Q},\dots,\mathbf{Q},\mathbf{P}) \) and \( \bar{\mathbf{R}} = \mathrm{diag}(\mathbf{R},\dots,\mathbf{R}) \), and stacked reference vector \( \bar{\mathbf{X}}^{\mathrm{ref}} \). Then the cost can be written as a quadratic function of \( \mathbf{U} \):

\[ J(\mathbf{x}_0,\mathbf{U}) = \tfrac{1}{2}\mathbf{U}^{\top}\mathbf{H}\mathbf{U} + \mathbf{f}(\mathbf{x}_0)^{\top}\mathbf{U} + \text{const}, \]

with

\[ \mathbf{H} = 2\left( \boldsymbol{\Gamma}^{\top}\bar{\mathbf{Q}}\boldsymbol{\Gamma} + \bar{\mathbf{R}} \right), \quad \mathbf{f}(\mathbf{x}_0) = 2\boldsymbol{\Gamma}^{\top}\bar{\mathbf{Q}} \left(\boldsymbol{\Phi}\mathbf{x}_0 - \bar{\mathbf{X}}^{\mathrm{ref}}\right). \]

Joint constraints, such as velocity and torque limits, can be written as

\[ \mathbf{G}\mathbf{U} \leq \mathbf{h} + \mathbf{S}\mathbf{x}_0, \]

where \( \mathbf{G} \), \( \mathbf{h} \) encode box constraints on each \( \mathbf{u}_k \) and possibly state constraints on \( \mathbf{x}_k \) via the prediction model. The MPC optimization at each sampling instant is thus a strictly convex QP:

\[ \min_{\mathbf{U}} \; \tfrac{1}{2}\mathbf{U}^{\top}\mathbf{H}\mathbf{U} + \mathbf{f}(\mathbf{x}_0)^{\top}\mathbf{U} \quad \text{s.t.} \quad \mathbf{G}\mathbf{U} \leq \mathbf{h} + \mathbf{S}\mathbf{x}_0. \]

Because \( \mathbf{R} \succ 0 \), one can show \( \mathbf{H} \succ 0 \), so there is a unique optimal control sequence \( \mathbf{U}^{\star} \). The MPC law applies only the first control action \( \mathbf{u}_0^{\star} \) and repeats the optimization at the next sampling instant.

Task-space tracking is defined with respect to an end-effector pose \( \mathbf{y} = \mathbf{x}_{\mathrm{ee}} \in \mathbb{R}^{m} \) (position and possibly orientation parameters), given by the forward kinematics

\[ \mathbf{y}(k) = \mathbf{f}(\mathbf{q}(k)). \]

Around a nominal trajectory \( \mathbf{q}^{\mathrm{ref}}(k) \), first-order linearization yields the relation between task and joint errors:

\[ \delta \mathbf{y}(k) \approx \mathbf{J}(\mathbf{q}^{\mathrm{ref}}(k)) \, \delta \mathbf{q}(k), \]

where \( \mathbf{J} \) is the manipulator Jacobian. Using the state \( \mathbf{x}_k = \begin{bmatrix} \mathbf{e}_q(k) \\ \dot{\mathbf{e}}_q(k) \end{bmatrix} \), a linear output map is

\[ \mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}} = \mathbf{C}_k \mathbf{x}_k, \quad \mathbf{C}_k = \begin{bmatrix} \mathbf{J}(\mathbf{q}^{\mathrm{ref}}(k)) & \mathbf{0} \end{bmatrix}. \]

A task-space MPC cost for tracking a desired end-effector trajectory \( \mathbf{y}_k^{\mathrm{ref}} \) is then

\[ J_{\mathrm{task}}(\mathbf{x}_0,\mathbf{U}) = \sum_{k=0}^{N-1} \Big( (\mathbf{C}_k \mathbf{x}_k)^{\top}\mathbf{Q}_y (\mathbf{C}_k \mathbf{x}_k) + \mathbf{u}_k^{\top}\mathbf{R}\mathbf{u}_k \Big) + (\mathbf{C}_N \mathbf{x}_N)^{\top}\mathbf{P}_y (\mathbf{C}_N \mathbf{x}_N), \]

with \( \mathbf{Q}_y \succeq 0 \), \( \mathbf{P}_y \succeq 0 \). This can be re-expressed as a state-weighting MPC by defining

\[ \mathbf{Q}_k^{\mathrm{eq}} = \mathbf{C}_k^{\top} \mathbf{Q}_y \mathbf{C}_k, \quad \mathbf{P}_N^{\mathrm{eq}} = \mathbf{C}_N^{\top} \mathbf{P}_y \mathbf{C}_N, \]

and using these in place of \( \mathbf{Q} \) and \( \mathbf{P} \) in the joint-space MPC derivation. In practice, task-space MPC and joint-space MPC differ only by replacing the state weights using the Jacobian. Constraints on joint variables remain linear, but workspace constraints may be approximated by linear constraints in task space, then mapped to joint space via linearization.

The following diagram summarizes the practical steps you will implement in code for either joint-space or task-space MPC tracking on a model of a manipulator.

flowchart TD
  A["Choose model: joint double-integrator or task-space linearization"] --> B["Select horizon N, weights Q, R, P"]
  B --> C["Build prediction matrices Phi, Gamma"]
  C --> D["Form QP matrices H, f(x0), constraints G, h, S"]
  D --> E["At each sampling instant: measure state x0"]
  E --> F["Update linear term f(x0), constraint offsets"]
  F --> G["Solve QP for optimal sequence U*"]
  G --> H["Apply first control input u0* to inner torque loop"]
  H --> I["Advance simulation or hardware, repeat"]
        

In joint space, the inner torque loop interprets \( \mathbf{u}_k \) as a desired virtual acceleration and maps it to torques using the dynamics model. In task space, the MPC cost is expressed in end-effector coordinates via the Jacobian, while the control variables remain joint-level.

We implement a 2-DOF joint-space MPC using numpy, scipy, and the QP solver osqp. The same structure generalizes to \( n \)-DOF. We consider joint error states and simple box constraints on virtual accelerations.

import numpy as np
from scipy import sparse
import osqp

# Joint-space double integrator model for n = 2
n = 2
Ts = 0.02

I = np.eye(n)
A = np.block([[I, Ts * I],
              [np.zeros((n, n)), I]])
B = np.block([[np.zeros((n, n))],
              [Ts * I]])

nx = A.shape[0]
nu = B.shape[1]

# MPC parameters
N = 20  # prediction horizon
Q = np.diag([100.0, 100.0, 10.0, 10.0])  # position, velocity weights
R = 0.1 * np.eye(n)
P = Q  # terminal weight (could be DARE solution)

# Build block-diagonal Qbar, Rbar
Q_blocks = [Q] * (N - 1) + [P]
Qbar = sparse.block_diag(Q_blocks, format="csc")
Rbar = sparse.block_diag([R] * N, format="csc")

# Build prediction matrices Phi, Gamma
def build_prediction_matrices(A, B, N):
    nx, nu = A.shape[0], B.shape[1]
    Phi = np.zeros((N * nx, nx))
    Gamma = np.zeros((N * nx, N * nu))
    A_power = np.eye(nx)
    for i in range(N):
        A_power = A_power @ A
        Phi[i * nx:(i + 1) * nx, :] = A_power
        for j in range(i + 1):
            A_power_j = np.linalg.matrix_power(A, i - j)
            Gamma[i * nx:(i + 1) * nx,
                  j * nu:(j + 1) * nu] = A_power_j @ B
    return sparse.csc_matrix(Phi), sparse.csc_matrix(Gamma)

Phi, Gamma = build_prediction_matrices(A, B, N)

# Cost matrices: H and parametric linear term f(x0)
H = 2.0 * (Gamma.T @ Qbar @ Gamma + Rbar)
H = (H + H.T) * 0.5  # make numerically symmetric
H = sparse.csc_matrix(H)

# Constraints on virtual accelerations u: u_min <= u_k <= u_max
u_min = -5.0 * np.ones(n)
u_max = 5.0 * np.ones(n)
u_min_vec = np.tile(u_min, N)
u_max_vec = np.tile(u_max, N)

G_u = sparse.vstack([
    sparse.eye(N * nu),
    -sparse.eye(N * nu)
])
h_u = np.hstack([u_max_vec, -u_min_vec])

# No explicit state constraints in this first implementation
G = G_u
h = h_u

# OSQP problem prepared once; only q vector changes with x0
prob = osqp.OSQP()
# Dummy initial q vector, updated later
q_init = np.zeros(N * nu)
prob.setup(P=H, q=q_init, A=G, l=-np.inf * np.ones(G.shape[0]),
           u=h, verbose=False, warm_start=True)

def compute_q(x0, xref_seq=None):
    """
    Compute linear term q for OSQP given current state x0 and reference sequence.
    For simplicity, we consider state reference x_ref = 0 (pure regulation).
    """
    if xref_seq is None:
        Xref = np.zeros(Phi.shape[0])
    else:
        Xref = xref_seq.reshape(-1)
    # f(x0) = 2 * Gamma^T Qbar (Phi x0 - Xref)
    fx = 2.0 * Gamma.T @ Qbar @ (Phi @ x0 - Xref)
    return np.array(fx).reshape(-1)

# Simulation
T_sim = 4.0  # seconds
steps = int(T_sim / Ts)

# Initial error: joints start at zero, want to reach some target (q_ref)
q_ref_final = np.array([0.5, -0.3])
x = np.zeros(nx)

# Reference is ramped for the first second, then constant
def joint_reference(k):
    t = k * Ts
    if t < 1.0:
        alpha = t / 1.0
    else:
        alpha = 1.0
    return alpha * q_ref_final

traj_q = []
traj_u0 = []

for k in range(steps):
    # Build reference sequence in state space (here: e_q = q - q_ref)
    xref_seq = []
    q_ref_k = joint_reference(k)
    dq_ref_k = np.zeros(n)
    for i in range(1, N + 1):
        q_ref = q_ref_k
        dq_ref = dq_ref_k
        e_q_ref = np.zeros_like(q_ref)
        de_q_ref = np.zeros_like(dq_ref)
        xref_seq.append(np.hstack([e_q_ref, de_q_ref]))
    xref_seq = np.array(xref_seq)

    # Current error state x = [e_q; e_qdot]
    # (Here we maintain x directly; in a real system we would measure q, dq.)

    q_vec = compute_q(x, xref_seq)
    prob.update(q=q_vec)

    res = prob.solve()
    if res.info.status_val not in (1, 2):
        raise RuntimeError("OSQP did not converge")

    U_opt = res.x
    u0 = U_opt[:nu]

    # Apply u0 to the double-integrator model
    x = A @ x + B @ u0

    traj_q.append(x[:n].copy())
    traj_u0.append(u0.copy())

traj_q = np.array(traj_q)
traj_u0 = np.array(traj_u0)

print("Final joint error:", traj_q[-1])
      

This script simulates the error dynamics. In a real robot control stack, you would: measure \( \mathbf{q}(k) \) and \( \dot{\mathbf{q}}(k) \), compute the error state \( \mathbf{x}_k \), solve the QP, obtain \( \mathbf{u}_k \), and then map this to a torque command \( \boldsymbol{\tau}_k \) via the inverse dynamics model.

In C++, a convenient combination is Eigen for linear algebra and OSQP-Eigen for QP solving. The following sketch shows a single-step MPC solve for joint-space tracking with the same double-integrator model.

#include <iostream>
#include <Eigen/Dense>
#include <OsqpEigen/OsqpEigen.h>

int main() {
    const int n = 2;
    const double Ts = 0.02;
    const int N = 20;

    const int nx = 2 * n;
    const int nu = n;

    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n);
    Eigen::MatrixXd A(nx, nx);
    Eigen::MatrixXd B(nx, nu);

    A.setZero();
    A.block(0, 0, n, n) = I;
    A.block(0, n, n, n) = Ts * I;
    A.block(n, n, n, n) = I;

    B.setZero();
    B.block(n, 0, n, n) = Ts * I;

    // Weights
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(nx, nx);
    Q.diagonal() << 100.0, 100.0, 10.0, 10.0;
    Eigen::MatrixXd R = 0.1 * Eigen::MatrixXd::Identity(nu, nu);

    // Build Phi, Gamma and H, similar to the Python code (omitted for brevity).
    // Suppose H is (N * nu) x (N * nu) and q is length N * nu.
    Eigen::SparseMatrix<double> H;
    Eigen::VectorXd q;

    // Box constraints on u
    Eigen::VectorXd u_min = -5.0 * Eigen::VectorXd::Ones(nu);
    Eigen::VectorXd u_max =  5.0 * Eigen::VectorXd::Ones(nu);

    Eigen::SparseMatrix<double> Aineq(2 * N * nu, N * nu);
    Eigen::VectorXd l(2 * N * nu), u(2 * N * nu);

    // Aineq = [I; -I]
    Aineq.setIdentity();
    for (int i = 0; i < N * nu; ++i) {
        Aineq.insert(N * nu + i, i) = -1.0;
    }

    for (int k = 0; k < N; ++k) {
        for (int j = 0; j < nu; ++j) {
            int idx = k * nu + j;
            l(idx) = -OsqpEigen::INFTY;
            u(idx) = u_max(j);
            l(N * nu + idx) = -OsqpEigen::INFTY;
            u(N * nu + idx) = -u_min(j);
        }
    }

    OsqpEigen::Solver solver;
    solver.settings().setWarmStart(true);
    solver.data().setNumberOfVariables(N * nu);
    solver.data().setNumberOfConstraints(2 * N * nu);
    if (!solver.data().setHessianMatrix(H)) return 1;
    if (!solver.data().setGradient(q)) return 1;
    if (!solver.data().setLinearConstraintsMatrix(Aineq)) return 1;
    if (!solver.data().setLowerBound(l)) return 1;
    if (!solver.data().setUpperBound(u)) return 1;

    if (!solver.initSolver()) return 1;

    // At runtime, update q when x0 changes, then solve:
    // solver.updateGradient(q_new);
    // solver.solveProblem();
    // Eigen::VectorXd U_opt = solver.getSolution();
    // Eigen::VectorXd u0 = U_opt.segment(0, nu);

    std::cout << "C++ MPC setup complete." << std::endl;
    return 0;
}
      

The missing steps (construction of prediction matrices and \( \mathbf{H} \), \( \mathbf{q} \)) follow the same algebraic expressions as in the Python implementation. In a full lab, you would wrap this MPC in a class that takes \( \mathbf{x}_0 \) and returns the optimal \( \mathbf{u}_0 \) at each control cycle.

In Java, EJML can be used for linear algebra and ojAlgo for QP solving. The example below shows the structure of a single-step MPC computation using an unconstrained formulation (for clarity). In a full implementation, you would use ojAlgo's QP solver to enforce input bounds similar to the Python and C++ versions.

import org.ejml.simple.SimpleMatrix;

public class JointMPC {
    private int n = 2;
    private double Ts = 0.02;
    private int N = 20;

    private SimpleMatrix A;
    private SimpleMatrix B;
    private SimpleMatrix Q;
    private SimpleMatrix R;

    public JointMPC() {
        int nx = 2 * n;
        int nu = n;

        SimpleMatrix I = SimpleMatrix.identity(n);
        A = new SimpleMatrix(nx, nx);
        B = new SimpleMatrix(nx, nu);

        A.zero();
        A.insertIntoThis(0, 0, I);
        A.insertIntoThis(0, n, I.scale(Ts));
        A.insertIntoThis(n, n, I);

        B.zero();
        B.insertIntoThis(n, 0, I.scale(Ts));

        Q = new SimpleMatrix(nx, nx);
        Q.zero();
        Q.set(0, 0, 100.0);
        Q.set(1, 1, 100.0);
        Q.set(2, 2, 10.0);
        Q.set(3, 3, 10.0);

        R = SimpleMatrix.identity(nu).scale(0.1);
    }

    public double[] computeControl(double[] x0Array) {
        // For brevity, we use a one-step LQR-like feedback as approximation:
        // u = -K x, where K solves a discrete Riccati equation (can be computed offline).
        // In ojAlgo you would form H and q and solve the QP instead.
        SimpleMatrix x0 = new SimpleMatrix(2 * n, 1, true, x0Array);

        // Placeholder gain (to be computed offline)
        double[] Kdata = { 50.0, 0.0, 10.0, 0.0,
                           0.0, 50.0, 0.0, 10.0 };
        SimpleMatrix K = new SimpleMatrix(n, 2 * n, true, Kdata);

        SimpleMatrix u = K.mult(x0).negative();
        return u.getDDRM().getData();
    }

    public static void main(String[] args) {
        JointMPC mpc = new JointMPC();
        double[] x0 = { 0.2, -0.1, 0.0, 0.0 };
        double[] u0 = mpc.computeControl(x0);
        System.out.println("u0[0] = " + u0[0] + ", u0[1] = " + u0[1]);
    }
}
      

To implement a full MPC in Java, you would: construct block matrices \( \boldsymbol{\Phi} \), \( \boldsymbol{\Gamma} \), \( \mathbf{H} \), \( \mathbf{q} \), and use an ojAlgo QP solver to minimize the quadratic cost under box constraints on \( \mathbf{u}_k \).

MATLAB offers both low-level MPC implementation using basic matrix operations and high-level tools in the Model Predictive Control Toolbox. Below is a script that builds an MPC controller for the double-integrator joint model using the toolbox. This controller can then be dropped into a Simulink model.

% Joint-space double-integrator MPC in MATLAB
n  = 2;
Ts = 0.02;

I = eye(n);
A = [I, Ts * I;
     zeros(n), I];
B = [zeros(n);
     Ts * I];
C = [eye(n), zeros(n)];   % measure joint error (here we treat x directly)
D = zeros(n, n);

plant = ss(A, B, C, D, Ts);

% Weights and horizon
Q = diag([100, 100, 10, 10]);
R = 0.1 * eye(n);
P = Q;  % for simplicity

mpc_horizon = 20;
control_horizon = 3;

mpcobj = mpc(plant, Ts, mpc_horizon, control_horizon);

% Set weights on states and inputs (Toolbox uses output/reference weights)
mpcobj.Weights.OutputVariables = [1 1];     % position error
mpcobj.Weights.ManipulatedVariables = [0.1 0.1];
mpcobj.Weights.ManipulatedVariablesRate = [0.0 0.0];

% Input (virtual acceleration) constraints
mpcobj.MV(1).Min = -5;
mpcobj.MV(1).Max =  5;
mpcobj.MV(2).Min = -5;
mpcobj.MV(2).Max =  5;

% In Simulink:
% 1. Create a Simulink model with a "State-Space" block representing the plant.
% 2. Add an "MPC Controller" block and link it to mpcobj (or use "mpcDesigner").
% 3. Connect measured states (or outputs) and reference commands.
% 4. Run the simulation and visualize joint tracking.

% Direct simulation example with sim():
x0 = [0.2; -0.1; 0; 0];
Tf = 4.0;
sim(mpcobj, Tf / Ts, x0);
      

For task-space MPC, you may linearize the robot model about a trajectory using linmod or linearize (Simulink Control Design), obtain state and output matrices including the Jacobian-based output, and then construct an MPC object for that linearized model.

In Wolfram Mathematica, an MPC step can be implemented via symbolic or numeric construction of the condensed QP followed by QuadraticOptimization or FindMinimum.

(* Joint-space double-integrator MPC in Mathematica *)

n  = 2;
Ts = 0.02;

I  = IdentityMatrix[n];
A  = ArrayFlatten[{ {I, Ts I}, {ConstantArray[0, {n, n}], I} }];
B  = ArrayFlatten[{ {ConstantArray[0, {n, n}]}, {Ts I} }];

nx = Length[A];
nu = Dimensions[B][[2]];
N  = 10;

Q  = DiagonalMatrix[{100., 100., 10., 10.}];
R  = 0.1 IdentityMatrix[nu];
P  = Q;

x0  = {0.2, -0.1, 0., 0.};

(* Build prediction matrices Phi and Gamma *)
PhiRows   = Table[MatrixPower[A, i], {i, 1, N}];
Phi       = ArrayFlatten[Table[If[i == j, PhiRows[[i]], ConstantArray[0, {nx, nx}]],
                               {i, 1, N}, {j, 1, 1}]];
GammaList = Table[
   Table[If[i >= j, MatrixPower[A, i - j].B, ConstantArray[0, {nx, nu}]],
         {j, 0, N - 1}],
   {i, 1, N}
   ];
Gamma     = ArrayFlatten[GammaList];

Qbar = DiagonalMatrix[Flatten@Table[
     If[k < N, Diagonal[Q], Diagonal[P]], {k, 1, N}
   ]];
Rbar = DiagonalMatrix[Flatten@Table[Diagonal[R], {k, 1, N}]];

H = 2 Transpose[Gamma].Qbar.Gamma + 2 Rbar;
f = 2 Transpose[Gamma].Qbar.(Phi.x0);

(* Input bounds *)
uMin = -5.;
uMax =  5.;
uVars = Array[u, {N, nu}];
uVec  = Flatten[uVars];

lb = ConstantArray[uMin, N nu];
ub = ConstantArray[uMax, N nu];

sol = QuadraticOptimization[H, f, {uVec}, lb <= uVec <= ub];
uOpt = uVec /. sol["PrimalMinimizer"];
u0   = uOpt[[1 ;; nu]];
      

This code constructs \( \boldsymbol{\Phi} \) and \( \boldsymbol{\Gamma} \) numerically, forms the condensed Hessian \( \mathbf{H} \) and linear term \( \mathbf{f} \), and uses QuadraticOptimization with box constraints on inputs. The first optimal input \( \mathbf{u}_0 \) is extracted and applied to the plant.

In practice, the MPC controller is not directly commanding raw torques but rather virtual accelerations or reference trajectories for an inner loop. The following diagram summarizes the architecture for both joint-space and task-space MPC.

flowchart TD
  RQ["Joint or task reference trajectory"] --> CMP["MPC (outer loop)"]
  CMP --> U["Virtual acceleration command u_k"]
  U --> INV["Inverse dynamics / \nlow-level torque controller"]
  INV --> ROB["Robot manipulator"]
  ROB --> SENS["Sensors (joint positions, velocities)"]
  SENS --> EST["State estimator \n(if needed)"]
  EST --> CMP
  ROB --> FK["Forward kinematics \nfor task-space error"]
  FK --> CMP
        

For joint-space MPC, the error is computed directly from joint states. For task-space MPC, the forward kinematics and Jacobian are used to compute end-effector errors and equivalent state weights in the cost function.

Problem 1 (Condensed Prediction Matrices): For the discrete-time system \( \mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k \), derive the explicit expressions for \( \boldsymbol{\Phi} \) and \( \boldsymbol{\Gamma} \) in the condensed prediction model \( \mathbf{X} = \boldsymbol{\Phi}\mathbf{x}_0 + \boldsymbol{\Gamma}\mathbf{U} \) for horizon \( N \).

Solution: Using forward substitution:

\[ \begin{aligned} \mathbf{x}_1 &= \mathbf{A}\mathbf{x}_0 + \mathbf{B}\mathbf{u}_0, \\ \mathbf{x}_2 &= \mathbf{A}\mathbf{x}_1 + \mathbf{B}\mathbf{u}_1 = \mathbf{A}^2 \mathbf{x}_0 + \mathbf{A}\mathbf{B}\mathbf{u}_0 + \mathbf{B}\mathbf{u}_1, \\ \mathbf{x}_3 &= \mathbf{A}\mathbf{x}_2 + \mathbf{B}\mathbf{u}_2 = \mathbf{A}^3 \mathbf{x}_0 + \mathbf{A}^2\mathbf{B}\mathbf{u}_0 + \mathbf{A}\mathbf{B}\mathbf{u}_1 + \mathbf{B}\mathbf{u}_2, \\ &\vdots \end{aligned} \]

Gathering terms, the stacked state vector \( \mathbf{X} = \begin{bmatrix} \mathbf{x}_1^{\top} & \dots & \mathbf{x}_N^{\top} \end{bmatrix}^{\top} \) can be written as

\[ \boldsymbol{\Phi} = \begin{bmatrix} \mathbf{A} \\ \mathbf{A}^2 \\ \vdots \\ \mathbf{A}^{N} \end{bmatrix}, \quad \boldsymbol{\Gamma} = \begin{bmatrix} \mathbf{B} & \mathbf{0} & \dots & \mathbf{0} \\ \mathbf{A}\mathbf{B} & \mathbf{B} & \dots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{A}^{N-1}\mathbf{B} & \mathbf{A}^{N-2}\mathbf{B} & \dots & \mathbf{B} \end{bmatrix}. \]

This matches the expressions used in the lab derivation.

Problem 2 (Positive Definiteness of the Hessian): Show that if \( \mathbf{Q} \succeq 0 \) and \( \mathbf{R} \succ 0 \), then the condensed Hessian \( \mathbf{H} = 2(\boldsymbol{\Gamma}^{\top}\bar{\mathbf{Q}}\boldsymbol{\Gamma} + \bar{\mathbf{R}}) \) is positive definite for any horizon \( N \geq 1 \), assuming \( \bar{\mathbf{R}} \) is block-diagonal with \( \mathbf{R} \) on the diagonal.

Solution: For any nonzero stacked input vector \( \mathbf{U} \),

\[ \mathbf{U}^{\top}\mathbf{H}\mathbf{U} = 2\mathbf{U}^{\top}\boldsymbol{\Gamma}^{\top}\bar{\mathbf{Q}}\boldsymbol{\Gamma}\mathbf{U} + 2\mathbf{U}^{\top}\bar{\mathbf{R}}\mathbf{U}. \]

Since \( \bar{\mathbf{Q}} \succeq 0 \), the first term is nonnegative. For the second term, block-diagonality implies

\[ \mathbf{U}^{\top}\bar{\mathbf{R}}\mathbf{U} = \sum_{k=0}^{N-1} \mathbf{u}_k^{\top}\mathbf{R}\mathbf{u}_k. \]

Because \( \mathbf{R} \succ 0 \), each term \( \mathbf{u}_k^{\top}\mathbf{R}\mathbf{u}_k \geq 0 \), and if \( \mathbf{U} \neq \mathbf{0} \), there exists some \( k \) with \( \mathbf{u}_k \neq \mathbf{0} \), making the sum strictly positive. Thus \( \mathbf{U}^{\top}\mathbf{H}\mathbf{U} > 0 \) for all \( \mathbf{U} \neq \mathbf{0} \), so \( \mathbf{H} \succ 0 \).

Problem 3 (Task-Space Weighting via Jacobian): Consider task-space MPC with output error \( \mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}} = \mathbf{C}_k \mathbf{x}_k \), where \( \mathbf{C}_k = \begin{bmatrix} \mathbf{J}_k & \mathbf{0} \end{bmatrix} \) and \( \mathbf{J}_k \) is the manipulator Jacobian at time \( k \). Suppose you use the quadratic cost

\[ J_y = \sum_{k=0}^{N-1} (\mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}})^{\top} \mathbf{Q}_y (\mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}}). \]

Show that this is equivalent to a state-space cost with \( \mathbf{Q}_k^{\mathrm{eq}} = \mathbf{C}_k^{\top}\mathbf{Q}_y\mathbf{C}_k \).

Solution: Substituting \( \mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}} = \mathbf{C}_k \mathbf{x}_k \),

\[ (\mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}})^{\top} \mathbf{Q}_y (\mathbf{y}_k - \mathbf{y}_k^{\mathrm{ref}}) = (\mathbf{C}_k \mathbf{x}_k)^{\top} \mathbf{Q}_y (\mathbf{C}_k \mathbf{x}_k) = \mathbf{x}_k^{\top}\mathbf{C}_k^{\top}\mathbf{Q}_y\mathbf{C}_k\mathbf{x}_k = \mathbf{x}_k^{\top}\mathbf{Q}_k^{\mathrm{eq}}\mathbf{x}_k. \]

Thus the task-space cost can be implemented within the standard MPC framework by using the time-varying state weights \( \mathbf{Q}_k^{\mathrm{eq}} \).

Problem 4 (Effect of Sampling Time on MPC Performance): Consider the double-integrator joint-space model used in this lab. Qualitatively describe how increasing the sampling time \( T_s \) affects: (a) numerical conditioning of \( \boldsymbol{\Gamma} \) and \( \mathbf{H} \); and (b) closed-loop tracking performance.

Solution: (a) As \( T_s \) increases, the discretized dynamics become more "coarse". Powers of \( \mathbf{A} \) in \( \boldsymbol{\Phi} \) and \( \boldsymbol{\Gamma} \) involve terms like \( T_s \) and \( T_s^2 \), so large \( T_s \) can lead to strong differences in magnitude between rows of \( \boldsymbol{\Gamma} \) and entries of \( \mathbf{H} \), degrading numerical conditioning and QP solver robustness. (b) A large \( T_s \) means slower update rates and less ability to react to disturbances and tracking errors, typically increasing overshoot and steady-state error. Too small \( T_s \) improves performance but increases computational load since QPs must be solved more frequently and prediction horizons in steps must be larger to cover the same physical time window.

In this lab you implemented joint-space and task-space MPC for a manipulator using a double-integrator joint error model. You derived the condensed prediction model \( \mathbf{X} = \boldsymbol{\Phi}\mathbf{x}_0 + \boldsymbol{\Gamma}\mathbf{U} \), built the QP matrices \( \mathbf{H} \) and \( \mathbf{f}(\mathbf{x}_0) \), and encoded simple box constraints on virtual accelerations. You then implemented this controller in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, and you understood how to integrate it as an outer loop around an inner torque controller. The same structure generalizes to more complex models and constraint sets, and forms the basis for advanced constrained motion control in robotics.