In previous lessons you formulated MPC for robotic systems as a finite-horizon optimization problem over a sequence of predicted states \( x_{k+i\mid k} \) and inputs \( u_{k+i\mid k} \), subject to the discrete-time dynamics and constraints. At the conceptual level, MPC uses the receding-horizon principle:

• At time step \( k \), measure/estimate \( x_k \).
• Solve a finite-horizon optimal control problem (OCP) to obtain the sequence \( \{u_{k+i\mid k}^\star\}_{i=0}^{N-1} \).
• Apply only the first input \( u_k = u_{k\mid k}^\star \).
• Discard the rest, increase \( k \leftarrow k+1 \), and repeat.

For a real robot with sampling period \( T_s \), the optimization at time \( k \) must be completed before the next sample arrives. Let \( t_{\text{solve}}(x_k) \) be the time that the solver needs to return a primal-dual solution for the current MPC problem. The real-time feasibility requirement is

\[ t_{\text{solve}}(x_k) \;<\; T_s \quad \text{for all relevant } x_k. \]

This lesson focuses on how to design the MPC formulation and its numerical solver so that:

• The optimization problem is numerically well conditioned.
• The solver exploits the parametric structure with respect to the current state \( x_k \).
• Warm-starting is used to provide good initial guesses from the previous time step, dramatically reducing the number of iterations.

For a linearized robot model around an operating point (obtained earlier in the course), consider the discrete-time system

\[ x_{k+1} = A x_k + B u_k, \quad x_k \in \mathbb{R}^{n_x},\; u_k \in \mathbb{R}^{n_u}. \]

With a quadratic stage cost and terminal cost, the finite-horizon cost at time \( k \) is

\[ J_k = \sum_{i=0}^{N-1} \bigl( (x_{k+i\mid k}-x_{k+i}^{\text{ref}})^\top Q (x_{k+i\mid k}-x_{k+i}^{\text{ref}}) + (u_{k+i\mid k}-u_{k+i}^{\text{ref}})^\top R (u_{k+i\mid k}-u_{k+i}^{\text{ref}}) \bigr) + \\ (x_{k+N\mid k}-x_{k+N}^{\text{ref}})^\top P (x_{k+N\mid k}-x_{k+N}^{\text{ref}}), \]

subject to dynamics and (for example) polyhedral constraints \( x_{k+i\mid k} \in \mathcal{X} \), \( u_{k+i\mid k} \in \mathcal{U} \). Eliminating the state variables by forward substitution (a standard condensing step) yields

\[ X = S x_k + T U, \]

where \( U \in \mathbb{R}^{N n_u} \) is the stacked input vector and \( X \in \mathbb{R}^{(N+1)n_x} \) is the stacked state trajectory. The condensed MPC problem is a parametric quadratic program (QP)

\[ \begin{aligned} \min_{U} \quad & \tfrac{1}{2} U^\top H U + f(x_k)^\top U + c(x_k) \\ \text{s.t.} \quad & G U \leq g(x_k), \end{aligned} \]

where \( H \) is positive definite (if \( R > 0 \) and \( P \) is chosen appropriately). The constraints typically encode joint limits, torque limits, obstacle approximations, and velocity bounds.

For a dense interior-point method, the worst-case complexity of solving this QP scales roughly as

\[ \mathcal{O}\bigl((N n_u)^3\bigr) \]

per MPC step, because the main cost is factorizing a matrix of size \( N n_u \times N n_u \). In practice, structure-exploiting solvers (e.g., sparse Riccati recursions, tailored interior-point or active-set methods) reduce the effective complexity to approximately

\[ \mathcal{O}\bigl(N (n_x^3 + n_u^3)\bigr) \]

using block-banded linear algebra. Nevertheless, on embedded processors typical in robotic controllers, reducing the number of solver iterations is often more important than asymptotic complexity, which is where warm-starting becomes crucial.

flowchart TD
  M["Measure / estimate x_k"] --> F["Form QP matrices H, f(x_k), G, g(x_k)"]
  F --> W["Provide initial guess for U_k (warm-start)"]
  W --> S["Run QP solver iterations"]
  S --> U["Extract optimal sequence U_k*"]
  U --> A["Apply first control u_k = u_{k|k}*"]
  A --> NSTEP["k = k + 1 and repeat"]
        

The condensed MPC problem can be viewed as a parametric QP in the parameter \( \theta = x_k \):

\[ \begin{aligned} \min_{U} \quad & \tfrac{1}{2} U^\top H U + (F \theta)^\top U \\ \text{s.t.} \quad & G U \leq g_0 + E \theta. \end{aligned} \]

Here \( F \) and \( E \) arise from condensing the dynamics and constraints. Assume:

• \( H \) is positive definite.
• The constraints are qualified (e.g., Slater condition holds for all relevant \( \theta \)).
• The active set at the solution has full row rank.

Under these conditions, the KKT system for the optimal primal-dual pair \( (U^\star(\theta), \lambda^\star(\theta)) \) is

\[ \begin{bmatrix} H & G_{\mathcal{A}}^\top(\theta) \\ G_{\mathcal{A}}(\theta) & 0 \end{bmatrix} \begin{bmatrix} U^\star(\theta) \\ \lambda^\star_{\mathcal{A}}(\theta) \end{bmatrix} = - \begin{bmatrix} F \theta \\ g_{\mathcal{A},0} + E_{\mathcal{A}} \theta \end{bmatrix}, \]

where \( \mathcal{A} \) is the active constraint index set at the optimum. By the implicit function theorem, the mapping \( \theta \mapsto (U^\star(\theta), \lambda^\star(\theta)) \) is locally differentiable and Lipschitz continuous as long as the active set does not change.

Since successive MPC problems differ only by the new state \( x_{k+1} \), which is close to \( x_k \) for a sufficiently small sampling period, the optimal solution for time \( k+1 \) is expected to be close to the optimal solution at time \( k \). This is the mathematical justification for warm-starting.

Warm-starting provides initial guesses for the primal and dual variables of the QP at time \( k+1 \) based on the solution at time \( k \). Denote:

• \( U_k^\star = [u_{k\mid k}^\star;\dots;u_{k+N-1\mid k}^\star] \),
• \( \lambda_k^\star \) the optimal Lagrange multipliers.

A shift-based primal warm-start defines

\[ \hat{U}_{k+1}^{(0)} = \begin{bmatrix} u_{k+1\mid k}^\star \\ u_{k+2\mid k}^\star \\ \vdots \\ u_{k+N-1\mid k}^\star \\ u_{k+N-1\mid k}^\star \end{bmatrix}, \]

i.e. we drop the first element (already applied as \( u_k \)) and shift the rest forward in time, repeating the last element or appending a reference input. This is consistent with the receding-horizon principle: if the system evolves as predicted, the previously optimal \( u_{k+1\mid k}^\star \) is a good candidate for \( u_{k+1\mid k+1} \).

Similarly, a dual warm-start uses the previous multipliers:

\[ \hat{\lambda}_{k+1}^{(0)} = \Pi_{\mathbb{R}_+^{m}} \bigl( S_\mathcal{A} \lambda_k^\star \bigr), \]

where \( S_\mathcal{A} \) shifts the multipliers associated with stage-wise constraints and \( \Pi_{\mathbb{R}_+^{m}} \) projects onto the nonnegative orthant (enforcing dual feasibility). Many modern QP solvers (OSQP, qpOASES, etc.) accept both primal and dual warm-starts.

On embedded hardware, the practical benefits include:

• Reduced number of Newton or gradient iterations.
• In active-set methods, faster identification of the correct active set.
• More predictable worst-case computation time.

flowchart TD
  SOLV["QP at time k solved: U_k*, lambda_k*"] --> SHIFT["Shift U_k* and lambda_k* forward in time"]
  SHIFT --> INIT["Use shifted values as initial guess for QP at time k+1"]
  INIT --> FEAS["Optionally project onto feasible sets"]
  FEAS --> SOLV2["Run reduced number of iterations to refine solution"]
        

For nonlinear robot dynamics (e.g. full rigid-body dynamics with Coriolis and gravity), MPC leads to a nonlinear program (NLP). A popular approach is sequential quadratic programming (SQP): linearize the dynamics and constraints, quadratize the cost around the current trajectory, and solve a QP to obtain a step.

The real-time iteration (RTI) scheme applies only one SQP step per sampling instant, using the previous solution as a warm-start. Denoting the Newton step at time \( k \) by \( \Delta U_k \), the update is

\[ U_k^{\text{new}} = U_k^{\text{old}} + \alpha_k \Delta U_k, \]

with \( \alpha_k \in (0,1] \) a step size (often chosen as 1 in RTI). Under suitable assumptions (smoothness, stabilizing terminal ingredients, sufficiently small sampling time), one can show that:

• Tracking error remains bounded and small.
• The RTI scheme behaves similarly to a fully converged SQP method.

From the coding perspective, the RTI loop has the same structure as the linear MPC loop, except that the QP matrices are updated by linearizing the dynamics at the current nominal trajectory, and the nominal trajectory itself is generated by rolling out the last control sequence.

For a robot joint-space controller with sampling time \( T_s \), typical design steps are:

1. Choose horizon length \( N \) and weights \( Q, R, P \).
2. Derive or import a discrete-time linear model \( (A,B) \) for MPC.
3. Condense or keep the QP in sparse form depending on solver preference.
4. Pre-factorize constant parts of the KKT system offline.
5. Implement warm-start shifting of \( U_k^\star \) and optionally \( \lambda_k^\star \).
6. Measure actual solver time and verify \( t_{\text{solve}}(x_k) \;<\; T_s \) with a margin.

For manipulators with several joints (e.g., 6 or 7 DOF), the MPC dimension \( N n_u \) can quickly become large. Exploiting structure (block-diagonal constraints, decoupled joints in some modes, etc.) and aggressive warm-start strategies is key to fitting MPC into high-rate inner loops (1–2 kHz) or moderately fast loops (100–200 Hz).

We now illustrate a simple linear MPC with warm-start in Python using numpy, scipy, and the QP solver osqp, which is widely used in robotics.

import numpy as np
import scipy.sparse as sp
import osqp

# Discrete-time linear model x_{k+1} = A x_k + B u_k
A = np.array([[1.0, 0.01],
              [0.0, 1.0]])      # simple joint position-velocity model
B = np.array([[0.0],
              [0.01]])          # joint torque or velocity input
nx, nu = A.shape[0], B.shape[1]

N = 20                         # prediction horizon
Q = np.diag([100.0, 1.0])
R = np.diag([0.1])
P = Q.copy()

# Build condensed matrices for a regulation problem x_ref = 0, u_ref = 0
# X = S x0 + T U
S = np.zeros(( (N+1)*nx, nx ))
T = np.zeros(( (N+1)*nx, N*nu ))

Ak = np.eye(nx)
for i in range(N+1):
    S[i*nx:(i+1)*nx, :] = Ak
    if i > 0:
        # contribution of previous controls
        Aj = np.eye(nx)
        for j in range(i):
            row = i*nx
            col = j*nu
            T[row:row+nx, col:col+nu] = Aj @ B
            Aj = Aj @ A
    Ak = Ak @ A

# Cost: 0.5 * U.T H U + x0.T F.T U + const
Qbar = sp.block_diag([sp.kron(sp.eye(N), Q), P]).toarray()
TQ = T.T @ Qbar
H = TQ @ T + sp.kron(sp.eye(N), R).toarray()
F = TQ @ S

# Simple box constraints on u: u_min <= u_k <= u_max
u_min = -0.5
u_max = 0.5
G_u = np.vstack([np.eye(N*nu), -np.eye(N*nu)])
g_u = np.hstack([u_max * np.ones(N*nu), -u_min * np.ones(N*nu)])

# OSQP setup (P must be sparse and symmetric)
P_qp = sp.csc_matrix((H + H.T) * 0.5)   # numerical symmetrization
A_qp = sp.csc_matrix(G_u)

# Initial state
x = np.array([0.5, 0.0])   # initial joint position error and velocity

# Warm-start buffers
U_prev = np.zeros(N*nu)
lam_prev = np.zeros(G_u.shape[0])

prob = osqp.OSQP()
prob.setup(P=P_qp,
           q=F.T @ x,
           A=A_qp,
           l=-np.inf * np.ones(G_u.shape[0]),
           u=g_u,
           warm_start=True,
           verbose=False)

for k in range(100):
    # Update linear term with new state
    q = F.T @ x
    prob.update(q=q)

    # Warm-start with previous primal and dual solutions
    prob.warm_start(x=U_prev, y=lam_prev)

    # Solve QP
    res = prob.solve()
    U_opt = res.x
    lam_opt = res.y

    # Extract first control and update state
    u = U_opt[0:nu]
    x = A @ x + B @ u

    # Shift warm-start for next step
    U_prev = np.roll(U_opt, -nu)
    U_prev[-nu:] = U_prev[-2*nu:-nu]    # repeat last element
    lam_prev = lam_opt.copy()

    # Here you would send u to the robot's actuator interface
      

The key points are:

• The QP structure (P and A) is constant and set up once.
• Only the vector q (and possibly constraint bounds) depends on x.
• prob.warm_start is called with shifted U_prev and lam_prev.

In C++, MPC is often implemented using qpOASES (an online active-set QP solver) or OSQP (operator-splitting QP solver), together with a linear algebra library such as Eigen. The essential idea is again to build constant QP data offline and then update the affine parts and warm-start.

#include <Eigen/Dense>
#include <osqp.h>

// Assuming H, G, and other matrices have been constructed similarly to Python code

int main() {
    const int nx = 2;
    const int nu = 1;
    const int N  = 20;
    const int nU = N * nu;
    const int nIneq = 2 * nU;  // simple bounds

    Eigen::MatrixXd H(nU, nU);
    Eigen::MatrixXd F(nU, nx);
    Eigen::MatrixXd G(nIneq, nU);
    Eigen::VectorXd g(nIneq);

    // ... fill H, F, G, g ...

    // Convert to CSC format for OSQP
    c_int n = nU;
    c_int m = nIneq;

    // Build OSQP data structures (P, A, q, l, u)
    // For brevity, details of CSC conversion are omitted

    OSQPSettings *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
    OSQPData *data = (OSQPData *)c_malloc(sizeof(OSQPData));
    osqp_set_default_settings(settings);
    settings->verbose = 0;
    settings->warm_start = 1;

    // fill data->n, data->m, data->P, data->q, data->A, data->l, data->u
    // ...

    OSQPWorkspace *work = osqp_setup(data, settings);

    Eigen::VectorXd x(nx);
    x << 0.5, 0.0;

    Eigen::VectorXd U_prev = Eigen::VectorXd::Zero(nU);
    Eigen::VectorXd lam_prev = Eigen::VectorXd::Zero(m);

    for (int k = 0; k != 100; ++k) {
        // Update q = F^T x
        Eigen::VectorXd q = F.transpose() * x;
        for (int i = 0; i != nU; ++i) {
            work->data->q[i] = q(i);
        }

        // Warm-start
        osqp_warm_start_x(work, U_prev.data());
        osqp_warm_start_y(work, lam_prev.data());

        // Solve
        osqp_solve(work);

        // Extract solution
        Eigen::VectorXd U_opt(nU);
        for (int i = 0; i != nU; ++i) {
            U_opt(i) = work->solution->x[i];
        }
        Eigen::VectorXd y_opt(m);
        for (int i = 0; i != m; ++i) {
            y_opt(i) = work->solution->y[i];
        }

        // Apply first control and update state with your discrete dynamics
        double u = U_opt(0);
        // x = A * x + B * u;  (using your system matrices)

        // Shift warm-start
        for (int i = 0; i != nU - nu; ++i) {
            U_prev(i) = U_opt(i + nu);
        }
        for (int i = nU - nu; i != nU; ++i) {
            U_prev(i) = U_prev(i - nu);  // repeat last value
        }
        lam_prev = y_opt;
    }

    osqp_cleanup(work);
    c_free(settings);
    c_free(data);
    return 0;
}
      

The pattern is the same as in Python. A production-quality implementation would encapsulate the MPC and solver interfaces into classes and ensure deterministic execution times, careful memory allocation, and integration with the real-time operating system.

Java is less common for hard real-time robot control but often used for higher-level planning. Nevertheless, the warm-start logic is the same. Here is a simplified sketch using a generic QP solver interface (for example, from ojAlgo), focusing on the warm-start and receding-horizon loop.

public class MpcController {

    private final int nx;
    private final int nu;
    private final int N;
    private final double[][] H;
    private final double[][] F;  // F^T x
    private final double[][] G;
    private final double[] g;
    private final QpSolver solver;

    private double[] uWarm;
    private double[] lambdaWarm;

    public MpcController(int nx, int nu, int N,
                         double[][] H, double[][] F,
                         double[][] G, double[] g,
                         QpSolver solver) {
        this.nx = nx;
        this.nu = nu;
        this.N  = N;
        this.H  = H;
        this.F  = F;
        this.G  = G;
        this.g  = g;
        this.solver = solver;

        int nU = N * nu;
        int nIneq = G.length;
        this.uWarm = new double[nU];
        this.lambdaWarm = new double[nIneq];
    }

    public double[] computeControl(double[] x) {
        int nU = N * nu;
        int nIneq = G.length;

        // q = F^T x
        double[] q = new double[nU];
        for (int i = 0; i != nU; ++i) {
            double s = 0.0;
            for (int j = 0; j != nx; ++j) {
                s += F[j][i] * x[j];
            }
            q[i] = s;
        }

        // Set up QP with warm-start
        QpProblem prob = new QpProblem(H, q, G, g);
        prob.setPrimalWarmStart(uWarm);
        prob.setDualWarmStart(lambdaWarm);

        QpSolution sol = solver.solve(prob);

        double[] Uopt = sol.getPrimal();
        double[] lam  = sol.getDual();

        // Shift warm-start for next step
        for (int i = 0; i != nU - nu; ++i) {
            uWarm[i] = Uopt[i + nu];
        }
        for (int i = nU - nu; i != nU; ++i) {
            uWarm[i] = uWarm[i - nu];
        }
        System.arraycopy(lam, 0, lambdaWarm, 0, nIneq);

        // Return first control move
        double[] u0 = new double[nu];
        System.arraycopy(Uopt, 0, u0, 0, nu);
        return u0;
    }
}
      

The class QpSolver abstracts away the details of the underlying QP algorithm. The user only needs to ensure that the solver exposes primal and dual warm-start interfaces and is configured for deterministic and efficient execution.

MATLAB's Model Predictive Control Toolbox provides an MPC object that internally maintains the predicted state and input trajectories and automatically uses them as warm-starts. The programmer interacts via the mpc, mpcstate, and mpcmove interfaces.

% Discrete-time state-space model for joint-space dynamics
Ts = 0.01;
A = [1 Ts; 0 1];
B = [0; Ts];
C = [1 0];
D = 0;

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

% MPC horizon and weights
p = 20;   % prediction horizon
m = 3;    % control horizon
mpcobj = mpc(plant, Ts, p, m);
mpcobj.Weights.OutputVariables = 1;
mpcobj.Weights.ManipulatedVariables = 0.1;
mpcobj.MV.Min = -0.5;
mpcobj.MV.Max =  0.5;

% Initial conditions
x = [0.5; 0.0];
y = C*x;
xref = 0;  % reference position

% Internal state object holds warm-start info
xmpc = mpcstate(mpcobj);

for k = 1:200
    % Compute control move; mpcmove uses internal warm-start information
    u = mpcmove(mpcobj, xmpc, y, xref);

    % Apply to plant
    x = A*x + B*u;
    y = C*x;

    % In Simulink, the MPC Controller block plays the same role
end
      

In Simulink, the MPC Controller block encapsulates the same behavior. When using generated C code (e.g., via Simulink Coder), the warm-start logic is compiled into the embedded MPC function, which is crucial for high-frequency real-time control.

In Wolfram Mathematica, convex QPs can be solved using ConvexOptimization. Warm-starting is realized by supplying an initialization point for the decision variables, typically via method options.

(* Dimensions *)
nx = 2; nu = 1; N = 20;

(* Matrices A, B, H, F, G, g constructed similarly as in Python example *)
(* For brevity, we assume they are already defined as symbolic matrices. *)

(* MPC step as a function with warm-started initial guess Uinit *)
Clear[MpcStep];
MpcStep[xk_, Uinit_] :=
 Module[{U, x, cost, cons, sol},
  U = Array[u, N];  (* scalar input sequence for nu = 1 *)
  x = Array[xx, {N + 1, nx}];

  (* Dynamics constraints *)
  cons = {x[[1]] == xk};
  Do[
   cons = Join[cons,
     {x[[i + 1]] ==
       A . x[[i]] + B . {U[[i]]}}];
   ,
   {i, 1, N}
  ];

  (* Input constraints u_min <= u_i <= u_max *)
  umin = -0.5; umax = 0.5;
  cons = Join[cons,
    Table[umin <= U[[i]] <= umax, {i, 1, N}]
  ];

  (* Quadratic cost *)
  cost =
   Sum[(x[[i]] . Q . x[[i]]) + (U[[i]]^2 R[[1, 1]]), {i, 1, N}] +
   x[[N + 1]] . P . x[[N + 1]];

  sol = ConvexOptimization[
    cost,
    cons,
    {Flatten[x], U},
    Method -> {"InteriorPoint", "InitialPoint" -> Flatten[{xk, Uinit}]}
  ];

  {U /. sol["PrimalMinimizer"], x /. sol["PrimalMinimizer"]}
 ];

(* Closed-loop simulation *)
x = {0.5, 0.0};
Uwarm = ConstantArray[0.0, N];

Do[
  {Uopt, xTraj} = MpcStep[x, Uwarm];
  u0 = Uopt[[1]];
  x = A . x + B . {u0};

  (* Shift warm-start *)
  Uwarm = Join[Rest[Uopt], {Last[Uopt]}];
  ,
  {k, 1, 50}
]
      

Here, the option "InitialPoint" -> Flatten[{xk, Uinit}] provides a warm-start for the interior-point algorithm in the decision space. For high-rate control this approach may still be too heavy, but it is useful for off-line analysis and prototyping of warm-start strategies.

Problem 1 (Feasibility of Shifted Warm-Start): Consider linear MPC with box constraints on inputs \( u_{\min} \leq u_{k+i\mid k} \leq u_{\max} \) and no state constraints. Suppose that at time \( k \) the optimal sequence \( U_k^\star \) is feasible. Show that the shifted sequence \( \hat{U}_{k+1}^{(0)} \) defined in Section 4 is also feasible at time \( k+1 \) when the constraints and bounds do not change between steps.

Solution: Feasibility with only box constraints is checked component-wise. At time \( k \), each element satisfies

\[ u_{\min} \leq u_{k+i\mid k}^\star \leq u_{\max}, \quad i = 0,\dots,N-1. \]

The shifted sequence at time \( k+1 \) is \( \hat{U}_{k+1}^{(0)} = [u_{k+1\mid k}^\star;\dots;u_{k+N-1\mid k}^\star;u_{k+N-1\mid k}^\star] \), i.e. every element is either \( u_{k+i\mid k}^\star \) for some \( i \geq 1 \) or a repetition of \( u_{k+N-1\mid k}^\star \). Since each \( u_{k+i\mid k}^\star \) satisfies the box constraints, and the bounds do not change between time steps, all components of \( \hat{U}_{k+1}^{(0)} \) lie in the same interval \( [u_{\min}, u_{\max}] \). Thus \( \hat{U}_{k+1}^{(0)} \) is feasible.

Problem 2 (Real-Time Feasibility Margin): A robotic manipulator is controlled by an MPC loop with sampling period \( T_s = 5\,\text{ms} \). The QP solver uses an average of 8 iterations per step, and each iteration takes \( 0.4\,\text{ms} \) on the target CPU. (a) Compute the average solve time. (b) What fraction of the sampling period remains for sensing, communication, and safety checks?

Solution: (a) The average solve time is

\[ t_{\text{solve}} = 8 \times 0.4\,\text{ms} = 3.2\,\text{ms}. \]

(b) The total sampling period is \( T_s = 5\,\text{ms} \), so the remaining time is

\[ T_s - t_{\text{solve}} = 5\,\text{ms} - 3.2\,\text{ms} = 1.8\,\text{ms}. \]

The fraction (or utilization) is

\[ \frac{t_{\text{solve}}}{T_s} = \frac{3.2}{5} = 0.64, \]

meaning 64% of the sampling period is used by the solver and 36% remains for sensing and other tasks. To improve robustness, one may reduce the horizon or exploit more aggressive warm-starting to introduce a safety margin.

Problem 3 (Effect of Warm-Start on Convergence): Consider a QP solved by a gradient projection method whose iteration error satisfies \( \| U^{(j+1)} - U^\star \| \leq \gamma \| U^{(j)} - U^\star \| \) with \( 0 < \gamma < 1 \). Let \( U^{(0)} \) be the initial guess. (a) Express the error after \( j \) iterations. (b) If warm-starting reduces \( \| U^{(0)} - U^\star \| \) by a factor \( \beta \in (0,1) \), how does this affect the error after \( j \) iterations?

Solution: (a) By induction,

\[ \| U^{(j)} - U^\star \| \leq \gamma^j \| U^{(0)} - U^\star \|. \]

(b) With warm-start, the initial error becomes \( \| \tilde{U}^{(0)} - U^\star \| = \beta \| U^{(0)} - U^\star \| \). After \( j \) iterations, the bound is

\[ \| \tilde{U}^{(j)} - U^\star \| \leq \gamma^j \beta \| U^{(0)} - U^\star \|. \]

Hence, warm-starting multiplies the error after \( j \) iterations by \( \beta \), which can be interpreted as either (i) reducing the final error for a fixed number of iterations, or (ii) allowing fewer iterations to achieve the same accuracy (freeing computation time).

Problem 4 (Closed-Loop Stability Insight): Assume that without warm-starting, the MPC controller always solves the QP to full optimality and is known to render the closed loop asymptotically stable for the origin. When warm-starting is used, the solver is terminated early, yielding a suboptimal control sequence with cost \( J_k^{\text{sub}} \) satisfying \( J_k^{\text{sub}} - J_k^\star \leq \delta \) for some known bound \( \delta > 0 \) at each time step. Explain qualitatively how this affects stability.

Solution: The exact optimal MPC law uses the value function \( V(x_k) = J_k^\star \) as a Lyapunov function candidate, and one has \( V(x_{k+1}) - V(x_k) \leq -\ell(x_k, u_k^\star) \), with stage cost \( \ell \), implying asymptotic stability. With suboptimal inputs, one obtains a perturbed inequality

\[ V(x_{k+1}) - V(x_k) \leq -\ell(x_k, u_k^{\text{sub}}) + \delta, \]

so the decrease in \( V \) is degraded by \( \delta \). If \( \delta \) is sufficiently small relative to the stage cost and the value function, one can show that the origin remains practically stable: trajectories converge to a small neighborhood whose size is proportional to \( \delta \). In practice, this motivates monitoring the suboptimality gap or using warm-starting to keep it small.

Problem 5 (Horizon vs Real-Time Constraint): A joint-space MPC controller uses horizon \( N \) and runs on hardware where the QP iteration complexity is approximately proportional to \( N^2 \). The robot sampling period is fixed. Explain qualitatively why there exists an upper bound on \( N \) beyond which real-time feasibility cannot be maintained, even if warm-starting is used.

Solution: Since each QP iteration cost scales roughly as \( N^2 \) and the number of iterations does not decrease to zero (even with good warm-starting), the solve time grows at least proportionally to \( N^2 \). For a fixed sampling period \( T_s \), there will be an upper bound on \( N \) such that \( t_{\text{solve}}(N) \leq T_s \). Beyond this, the solver cannot finish within one sample, leading to missed deadlines. Warm-starting can reduce the constant factor (fewer iterations) but cannot remove the fundamental dependence on the problem dimension.

In this lesson we examined how MPC can be implemented under strict real-time constraints typical in robot control. We used the condensed QP structure of linear MPC to motivate parametric QP and solution sensitivity, and we showed how warm-starting of both primal and dual variables exploits the slowly varying nature of the robot state. We briefly introduced the real-time iteration idea for nonlinear MPC, highlighted practical design rules for manipulators, and presented implementation sketches in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. The next lesson will apply these ideas in a concrete laboratory setting for joint and task-space tracking.