In robot control, constraints are intrinsic: joint limits, velocity bounds, torque saturation, collision avoidance, and sometimes contact forces. MPC is particularly attractive for robots because it can explicitly handle these constraints while optimizing a performance criterion.

Consider a discrete-time linear model (arising from linearization around a nominal trajectory, from Lesson 2):

\[ x_{k+1} = A x_k + B u_k, \]

where the state might stack joint positions and velocities, \( x_k = \begin{bmatrix} q_k \\ \dot{q}_k \end{bmatrix} \), and inputs are joint torques \( u_k = \boldsymbol{\tau}_k \). Typical hard constraints are polyhedral:

\[ \mathcal{X} = \{ x \in \mathbb{R}^n \mid G_x x \le g_x \}, \quad \mathcal{U} = \{ u \in \mathbb{R}^m \mid G_u u \le g_u \}, \]

for example, joint and torque limits:

\[ q^{\min} \le q_k \le q^{\max}, \quad -\boldsymbol{\tau}^{\max} \le \boldsymbol{\tau}_k \le \boldsymbol{\tau}^{\max}. \]

MPC incorporates these constraints into an optimization problem solved at each sampling instant. The high-level loop for constrained MPC is:

flowchart TD
  Xk["Measure current state xk"] --> PRED["Predict dynamics over horizon"]
  PRED --> CONST["Enforce state/input limits"]
  CONST --> QP["Solve finite-horizon constrained QP"]
  QP --> APPLY["Apply first control uk to robot"]
  APPLY --> ROBOT["Robot joints and actuators"]
  ROBOT --> NEXT["Next state x(k+1) at next sample"]
  NEXT --> Xk
        

This lesson focuses on how to encode and analyze the constraint part of this loop: feasibility, softening, and stability.

We consider a standard quadratic tracking cost for deviations from a reference \( x_k^{\text{ref}}, u_k^{\text{ref}} \) (for brevity we write regulation around zero; the tracking case is handled by shifting variables as in Lesson 2). For horizon length \( N \), define:

\[ J_N(x_t,\mathbf{u}) = \sum_{k=0}^{N-1} \left( x_{t+k}^\top Q x_{t+k} + u_{t+k}^\top R u_{t+k} \right) + x_{t+N}^\top P x_{t+N}, \]

At time \( t \), the constrained MPC problem is

\[ \begin{aligned} \min_{\mathbf{u}, \mathbf{x}} \;& \sum_{k=0}^{N-1} \left( x_{t+k}^\top Q x_{t+k} + u_{t+k}^\top R u_{t+k} \right) + x_{t+N}^\top P x_{t+N} \\ \text{s.t. } \;& x_{t} = x(t), \\ & x_{t+k+1} = A x_{t+k} + B u_{t+k}, \quad k = 0,\dots,N-1, \\ & x_{t+k} \in \mathcal{X}, \quad u_{t+k} \in \mathcal{U}, \quad k = 0,\dots,N-1, \\ & x_{t+N} \in \mathcal{X}_f. \end{aligned} \]

Here \( \mathcal{X}_f \subseteq \mathcal{X} \) is a terminal constraint set used to ensure stability (Section 5). Stacking all decision variables into a vector \( z \in \mathbb{R}^{N m + N n} \), the problem can be written in QP form

\[ \min_{z} \; \tfrac{1}{2} z^\top H z + f^\top z \quad \text{s.t.} \quad G z \le h, \quad E z = b(x(t)), \]

where equality constraints encode the dynamics and initial state, while inequality constraints encode state, input, and terminal limits. This QP structure is exploited by fast solvers (e.g. OSQP, qpOASES, CVXGEN) that are widely used in robotics MPC.

When constraints are treated as hard, they must hold for all predicted states and inputs. Define the feasible set of initial states for horizon \( N \):

\[ \mathcal{X}_N = \left\{ x \in \mathbb{R}^n \;\middle|\; \exists \{u_0,\dots,u_{N-1}\} \subset \mathcal{U} \text{ such that } x_k \in \mathcal{X},\; x_N \in \mathcal{X}_f \right\}. \]

A central property for MPC is recursive feasibility: if the QP is feasible at time \( t=0 \), then it remains feasible at all subsequent times under the MPC feedback law. We now state a standard result for linear MPC.

Definition (Control-invariant set). A set \( \mathcal{X}_f \subseteq \mathcal{X} \) is called control-invariant if

\[ \forall x \in \mathcal{X}_f \; \exists u \in \mathcal{U} \text{ such that } A x + B u \in \mathcal{X}_f. \]

Theorem 1 (Recursive feasibility). Assume:

• \( \mathcal{X}, \mathcal{U} \) are convex and compact.
• \( \mathcal{X}_f \subseteq \mathcal{X} \) is control-invariant.
• At time \( t \), the MPC QP is feasible and yields optimal sequence \( \{u_{t|t}^\star,\dots,u_{t+N-1|t}^\star\} \) with corresponding \( \{x_{t+k|t}^\star\} \).

Then the MPC QP is feasible at time \( t+1 \), i.e., \( x_{t+1} = x_{t+1|t}^\star \in \mathcal{X}_N \). Consequently, feasibility holds for all \( t \ge 0 \) as long as \( x_0 \in \mathcal{X}_N \).

Proof sketch. At time \( t+1 \), consider the candidate input sequence

\[ \tilde{u}_{t+1|t+1} = u_{t+1|t}^\star,\dots, \tilde{u}_{t+N-1|t+1} = u_{t+N-1|t}^\star,\; \tilde{u}_{t+N|t+1} = \kappa_f(x_{t+N|t}^\star), \]

where \( \kappa_f \) is a control law that keeps \( \mathcal{X}_f \) invariant (its existence follows from control-invariance). The shifted state sequence satisfies dynamics and all constraints:

• For \( k = 1,\dots,N-1 \), \( x_{t+k|t}^\star \in \mathcal{X} \) implies \( \tilde{x}_{t+k|t+1} = x_{t+k|t}^\star \in \mathcal{X} \).
• \( x_{t+N|t}^\star \in \mathcal{X}_f \) and invariance of \( \mathcal{X}_f \) imply \( \tilde{x}_{t+N+1|t+1} = A x_{t+N|t}^\star + B \kappa_f(x_{t+N|t}^\star) \in \mathcal{X}_f \).

Thus we have constructed a feasible sequence at time \( t+1 \), so \( x_{t+1} \in \mathcal{X}_N \). Repeating this argument proves recursive feasibility.

For robots, control-invariant terminal sets are often computed in joint space (e.g., by iteratively applying admissible torques and intersecting with joint limits).

In practice, demanding strict satisfaction of all constraints can render the QP infeasible (e.g. due to model mismatch or short horizon). A common solution is to soften selected constraints using slack variables. For example, suppose we want to enforce \( C x_k \le d \) (e.g. workspace or obstacle-avoidance constraints). We introduce slack variables \( \epsilon_k \in \mathbb{R}^{p} \):

\[ \begin{aligned} C x_{t+k} &\le d + \epsilon_{t+k}, \\ \epsilon_{t+k} &\ge 0. \end{aligned} \]

The cost is augmented with a penalty on violations:

\[ J_N^{\text{soft}}(x_t,\mathbf{u},\boldsymbol{\epsilon}) = J_N(x_t,\mathbf{u}) + \sum_{k=0}^{N} \rho \, \|\epsilon_{t+k}\|_1 \quad (\rho > 0 \text{ large}). \]

The optimization variables now include \( \epsilon_{t},\dots,\epsilon_{t+N} \), and the problem remains a QP (with additional variables and inequality constraints). The penalty parameter \( \rho \) should be chosen sufficiently large so that if there is a hard-feasible solution, the optimal soft solution will not use slacks.

Theorem 2 (Exact penalty behavior, simplified). Suppose

• the original hard-constrained MPC problem is feasible at state \( x_t \), and
• the cost \( J_N \) and constraints are convex, with bounded Lagrange multipliers \( \lambda_k^\star \) associated with \( C x_{t+k} \le d \) in the hard problem.

Then there exists \( \bar{\rho} > 0 \) such that for all \( \rho \ge \bar{\rho} \), the optimal solution of the soft-constrained MPC problem satisfies \( \epsilon_{t+k}^\star = 0 \) for all \( k \), i.e., the soft solution coincides with a hard-feasible solution.

Idea of proof. In convex optimization, exact \( \ell_1 \)-penalties can reproduce constrained optima when the penalty parameter exceeds the maximum absolute Lagrange multiplier of the original constraint. Here, choosing \( \rho > \max_k \|\lambda_k^\star\|_\infty \) ensures that any violation increases cost more than satisfying the constraint using the original optimal multipliers, hence optimal slacks are zero whenever a hard-feasible solution exists.

Soft constraints are particularly useful in robotics for safety-related constraints that should almost always hold, but for which strict infeasibility could cause control failure (for example, slightly penetrating a linearized obstacle constraint to avoid solver failure).

Constraints can destroy the simple LQR stability proof. For constrained MPC, stability is often enforced using a terminal cost and terminal set. We recall the finite-horizon value function

\[ V_N(x) = \min_{\mathbf{u}} J_N(x,\mathbf{u}) \quad \text{s.t. constraints}. \]

Under suitable conditions, \( V_N \) acts as a Lyapunov function. Consider the regulation case \( x^{\text{ref}} = 0 \), and assume:

• \( Q \) and \( R \) are positive definite (or at least \( Q \ge 0, R > 0 \)), and
• there exists a local linear feedback \( u = K x \) and a terminal cost \( V_f(x) = x^\top P x \) such that for all \( x \in \mathcal{X}_f \):

\[ (A+BK)x \in \mathcal{X}_f,\quad K x \in \mathcal{U}, \]

\[ (A+BK)^\top P (A+BK) - P + Q + K^\top R K \le 0. \]

The last inequality is a discrete-time Lyapunov inequality capturing that applying the local control \( u = Kx \) inside \( \mathcal{X}_f \) decreases the infinite-horizon LQR cost. With these assumptions, one can prove:

Theorem 3 (Asymptotic stability of constrained MPC, sketch). If \( x_0 \in \mathcal{X}_N \) and the above conditions hold, then the origin \( x = 0 \) is asymptotically stable under the constrained MPC law, and all constraints are satisfied for all time.

Proof idea (decrease of value function). Let \( x_t \) be the state at time \( t \) and let \( \{u_{t+k|t}^\star\} \) be the optimal sequence. Construct a shifted candidate sequence for time \( t+1 \) by dropping the first control and appending the local terminal control \( K x_{t+N|t}^\star \). Using the Lyapunov inequality, one shows

\[ V_N(x_{t+1}) - V_N(x_t) \le - \left( x_t^\top Q x_t + u_{t|t}^{\star\top} R u_{t|t}^\star \right) \le 0. \]

Thus \( V_N \) is nonincreasing along MPC trajectories and strictly decreases except at the origin. Together with positive definiteness, this yields asymptotic stability by standard Lyapunov arguments.

In robotics, \( \mathcal{X}_f \) is often chosen as an ellipsoid around the desired configuration (e.g. small joint position and velocity errors) where a local LQR/PD-type law is valid and respects joint/torque limits.

Designing constraint handling in MPC for a robot proceeds through a structured set of steps: modeling physical limits, classifying constraints as hard/soft, selecting terminal ingredients, and generating a solver-ready QP.

flowchart TD
  START["Start from robot model and task"] --> LIM["Identify physical limits: joints, velocities, torques"]
  LIM --> CLASS["Classify: hard vs soft constraints"]
  CLASS --> SETS["Define sets X, U, Xf (polyhedral or ellipsoidal)"]
  SETS --> COST["Choose Q, R, P and select horizon N"]
  COST --> FORM["Formulate QP with dynamics, constraints, and slacks"]
  FORM --> TEST["Offline test: feasibility and closed-loop behavior"]
  TEST --> IMPL["Implement online MPC with chosen solver"]
        

In later labs, this workflow is instantiated for joint-space and task-space tracking, including joint limits and torque saturation.

We sketch a Python implementation of constrained MPC for a single revolute joint modeled as a discrete-time double integrator. We use cvxpy for QP formulation and note how one would integrate this with a robot modeling library such as roboticstoolbox.

The discrete-time model (sampling time \( T_s \), inertia \( J > 0 \)):

\[ x_{k+1} = \begin{bmatrix} 1 & T_s \\ 0 & 1 \end{bmatrix} x_k + \begin{bmatrix} 0 \\ T_s / J \end{bmatrix} u_k, \quad x_k = \begin{bmatrix} q_k \\ \dot{q}_k \end{bmatrix}, \quad u_k = \tau_k. \]

Python code for one MPC step:

import numpy as np
import cvxpy as cp

# Robot joint parameters
Ts = 0.01  # sampling time
J = 0.05   # joint inertia

A = np.array([[1.0, Ts],
              [0.0, 1.0]])
B = np.array([[0.0],
              [Ts / J]])

# Horizon and cost
N = 20
Q = np.diag([50.0, 1.0])   # penalize position error heavily
R = np.array([[0.1]])

# Joint and torque limits
q_min = -1.5
q_max =  1.5
dq_max = 2.0
tau_max = 2.0

def mpc_step(x0, x_ref):
    """
    Solve constrained MPC step for a single joint.
    x0: current state (2,)
    x_ref: desired joint position (scalar), zero desired velocity.
    """
    nx = A.shape[0]
    nu = B.shape[1]

    # Decision variables
    x = cp.Variable((nx, N + 1))
    u = cp.Variable((nu, N))

    # Cost and constraints
    cost = 0
    constraints = []

    # Initial condition
    constraints.append(x[:, 0] == x0)

    for k in range(N):
        # Stage cost (track reference)
        x_ref_vec = np.array([x_ref, 0.0])
        cost += cp.quad_form(x[:, k] - x_ref_vec, Q) + cp.quad_form(u[:, k], R)

        # Dynamics
        constraints.append(x[:, k + 1] == A @ x[:, k] + B @ u[:, k])

        # State constraints
        constraints.append(x[0, k] >= q_min)
        constraints.append(x[0, k] <= q_max)
        constraints.append(cp.abs(x[1, k]) <= dq_max)

        # Input constraints
        constraints.append(cp.abs(u[:, k]) <= tau_max)

    # Terminal cost
    x_ref_vec = np.array([x_ref, 0.0])
    cost += cp.quad_form(x[:, N] - x_ref_vec, Q)

    # Solve QP
    prob = cp.Problem(cp.Minimize(cost), constraints)
    prob.solve(solver=cp.OSQP, warm_start=True)

    if prob.status not in ["optimal", "optimal_inaccurate"]:
        raise RuntimeError("MPC QP infeasible or solver failed")

    # Receding-horizon: apply the first control
    u0 = u[:, 0].value
    return u0

# Example usage in a control loop:
if __name__ == "__main__":
    x = np.array([0.0, 0.0])
    x_ref = 1.0
    for k in range(50):
        u0 = mpc_step(x, x_ref)
        # Simulate one step (here using the exact model)
        x = A @ x + B @ u0
        print(f"step {k}, q = {x[0]:.3f}, dq = {x[1]:.3f}, tau = {u0[0]:.3f}")
      

For a multi-joint manipulator, one would obtain a higher-dimensional discrete-time state-space model (e.g. from roboticstoolbox or custom dynamics) and build an MPC QP with stacked states and inputs, but the constraint structure (joint limits, torque bounds) remains similar.

In C++, constrained MPC is frequently implemented by building the QP matrices using Eigen and solving them with a QP solver such as OSQP or qpOASES. Below is a sketch that assumes we have already constructed the condensed QP:

#include <iostream>
#include <Eigen/Dense>
#include "osqp.h"  // OSQP C API

// H, g, A, l, u define the QP
//   minimize 0.5 z' H z + g' z
//   subject to l <= A z <= u

struct MPCQP {
    Eigen::MatrixXd H;
    Eigen::VectorXd g;
    Eigen::MatrixXd A;
    Eigen::VectorXd l;
    Eigen::VectorXd u;
};

MPCQP buildJointMPCQP(const Eigen::Vector2d& x0, double q_min, double q_max,
                      double dq_max, double tau_max,
                      double Ts, int N)
{
    MPCQP qp;
    // For brevity, we omit full construction.
    // In practice:
    //  - Build prediction matrices for x(k) from x0 and sequence of u
    //  - Encode constraints as linear inequalities on z (stacked u's or x,u)
    //  - Fill qp.H, qp.g, qp.A, qp.l, qp.u
    return qp;
}

Eigen::VectorXd solveOSQP(const MPCQP& qp)
{
    // Convert Eigen matrices to OSQP CSR structures and call OSQP API.
    // Here we only show the outline, not full boilerplate.
    // ...
    Eigen::VectorXd z_opt;
    return z_opt;
}

int main()
{
    double Ts = 0.01;
    int N = 20;
    double q_min = -1.5, q_max = 1.5, dq_max = 2.0, tau_max = 2.0;

    Eigen::Vector2d x;
    x << 0.0, 0.0;

    for (int k = 0; k < 50; ++k) {
        MPCQP qp = buildJointMPCQP(x, q_min, q_max, dq_max, tau_max, Ts, N);
        Eigen::VectorXd z_opt = solveOSQP(qp);

        // First element(s) of z_opt correspond to first control input u0
        double u0 = z_opt(0);

        // Simulate simple joint model x(k+1) = A x(k) + B u(k)
        Eigen::Matrix2d A;
        A << 1.0, Ts,
              0.0, 1.0;
        Eigen::Vector2d B;
        B << 0.0, Ts / 0.05;  // assume inertia J = 0.05

        x = A * x + B * u0;

        std::cout << "step " << k
                  << ", q = " << x(0)
                  << ", dq = " << x(1)
                  << ", tau = " << u0 << std::endl;
    }
    return 0;
}
      

In a full manipulator application, buildJointMPCQP uses the robot's joint-space linearization (e.g. from a dynamics library) and encodes joint/torque constraints as rows of the inequality matrix A.

Java is less common for low-level robot control, but it is used in some simulation and high-level control frameworks. A typical approach uses EJML for linear algebra and a QP solver such as ojAlgo. Below is a conceptual snippet:

import org.ejml.simple.SimpleMatrix;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.QuadraticSolver;
import org.ojalgo.optimisation.QuadraticSolver.Builder;

public class JointMPC {

    public static class MPCQP {
        public SimpleMatrix H;
        public SimpleMatrix g;
        public SimpleMatrix A;
        public SimpleMatrix l;
        public SimpleMatrix u;
    }

    public static MPCQP buildQP(SimpleMatrix x0, double qMin, double qMax,
                                double dqMax, double tauMax,
                                double Ts, int N) {
        MPCQP qp = new MPCQP();
        // Build prediction and constraint matrices here
        // ...
        return qp;
    }

    public static double solveAndGetFirstTorque(MPCQP qp) {
        Builder builder = QuadraticSolver.getBuilder(qp.H.numRows());

        // Set quadratic and linear terms
        builder.quadratic(qp.H.getMatrix());
        builder.linear(qp.g.getMatrix());

        // Inequality constraints l <= A z <= u
        // ojAlgo expects these in a specific canonical form;
        // convert qp.A, qp.l, qp.u accordingly.
        // ...

        QuadraticSolver solver = builder.build();
        Optimisation.Result result = solver.solve();

        if (result.getState().isFailure()) {
            throw new RuntimeException("MPC QP infeasible");
        }

        // Extract first control input
        double[] z = result.toRawCopy1D();
        return z[0];
    }

    public static void main(String[] args) {
        double Ts = 0.01;
        int N = 20;
        double qMin = -1.5, qMax = 1.5, dqMax = 2.0, tauMax = 2.0;

        SimpleMatrix x = new SimpleMatrix(2, 1);
        x.set(0, 0, 0.0);
        x.set(1, 0, 0.0);

        for (int k = 0; k < 50; ++k) {
            MPCQP qp = buildQP(x, qMin, qMax, dqMax, tauMax, Ts, N);
            double tau = solveAndGetFirstTorque(qp);

            // Simulate discrete double integrator
            SimpleMatrix A = new SimpleMatrix(new double[][]{
                    {1.0, Ts},
                    {0.0, 1.0}
            });
            SimpleMatrix B = new SimpleMatrix(new double[][]{
                    {0.0},
                    {Ts / 0.05}
            });

            x = A.mult(x).plus(B.scale(tau));

            System.out.println("step " + k
                    + ", q = " + x.get(0)
                    + ", dq = " + x.get(1)
                    + ", tau = " + tau);
        }
    }
}
      

In a multi-joint scenario, this pattern extends by stacking joint states and torques, with joint and torque limits generating additional rows in A, l, and u.

MATLAB's Model Predictive Control Toolbox provides built-in support for constrained MPC, which integrates naturally with robot models from the Robotics System Toolbox. For a single joint:

Ts = 0.01;
J  = 0.05;

A = [1 Ts;
     0 1];
B = [0;
     Ts/J];
C = eye(2);     % full state output
D = [0; 0];

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

% Horizon and weights
p = 20;   % prediction horizon
m = 3;    % control horizon

mpcobj = mpc(plant, Ts, p, m);
mpcobj.Weights.OutputVariables = [50 1];
mpcobj.Weights.ManipulatedVariables = 0.1;

% Joint and torque constraints
q_min = -1.5; q_max = 1.5;
dq_max = 2.0;
tau_max = 2.0;

mpcobj.MV.Min = -tau_max;
mpcobj.MV.Max =  tau_max;

% Output constraints (q, dq)
mpcobj.OV(1).Min = q_min;
mpcobj.OV(1).Max = q_max;
mpcobj.OV(2).Min = -dq_max;
mpcobj.OV(2).Max =  dq_max;

% Reference (desired joint angle)
yref = [1; 0];

x = [0; 0];
u = 0;

for k = 1:50
    % Compute constrained MPC move
    u = mpcmove(mpcobj, x, x, yref);
    % Simulate plant
    x = A*x + B*u;
    fprintf("step %d: q = %.3f, dq = %.3f, tau = %.3f\n", k, x(1), x(2), u);
end
      

In Simulink, the MPC Controller block can be connected to a robot plant model defined using the Robotics System Toolbox, with joint limits and torque saturation configured via block parameters. The toolbox transparently constructs and solves the underlying QP at each sample.

Mathematica can be used to symbolically derive the prediction model and then numerically solve the constrained QP. For a short horizon, one can directly construct the optimization problem:

(* System matrices for joint *)
Ts = 0.01;
J  = 0.05;
A = { {1, Ts},
     {0, 1} };
B = { {0},
     {Ts/J} };

(* Horizon and cost weights *)
N  = 10;
Q  = DiagonalMatrix[{50, 1}];
R  = {{0.1}};
xRef = {1, 0};

(* Decision variables: u[0..N-1], x[0..N] *)
u = Array[u, N];
x = Array[x, N + 1];

(* Initial condition *)
x0 = {0, 0};
constraints = {x[1] == x0};

(* Dynamics and constraints *)
qMin = -1.5; qMax = 1.5;
dqMax = 2.0; tauMax = 2.0;

For[k = 0, k < N, k++,
  (* Dynamics: x[k+2] because x[1] is x0 *)
  constraints = Join[constraints,
    {x[k + 2] == A . x[k + 1] + B . {u[k + 1]}}];

  (* State constraints *)
  constraints = Join[constraints, {
    qMin <= x[k + 1][[1]] <= qMax,
    -dqMax <= x[k + 1][[2]] <= dqMax}];

  (* Input constraints *)
  constraints = Join[constraints, {
    -tauMax <= u[k + 1] <= tauMax}];
];

(* Terminal state constraints *)
constraints = Join[constraints, {
  qMin <= x[N + 1][[1]] <= qMax,
  -dqMax <= x[N + 1][[2]] <= dqMax}];

(* Cost function *)
cost =
  Sum[(x[k + 1] - xRef).Q.(x[k + 1] - xRef) + {u[k + 1]}.R.{u[k + 1]},
      {k, 0, N - 1}] +
  (x[N + 1] - xRef).Q.(x[N + 1] - xRef);

(* Solve constrained optimization numerically *)
sol = NMinimize[{cost, constraints},
  Flatten[{Table[u[k + 1], {k, 0, N - 1}],
           Table[x[k + 1], {k, 0, N}]}]];

u0Opt = u[1] /. Last[sol]
      

For more complex robots, one can symbolically derive the discrete-time linearized dynamics around a trajectory and plug them into a similar finite-horizon optimization problem, then export the resulting controller as C code for deployment.

Problem 1 (QP KKT conditions for constrained MPC). Consider the linear MPC QP

\[ \min_z \; \tfrac{1}{2} z^\top H z + f^\top z \quad \text{s.t.} \quad G z \le h, \; E z = b, \]

where \( H \) is positive semidefinite. Derive the Karush–Kuhn–Tucker (KKT) optimality conditions and interpret them in the context of constrained MPC.

Solution. Introduce Lagrange multipliers \( \lambda \ge 0 \) for \( G z \le h \) and \( \nu \) for \( E z = b \). The Lagrangian is

\[ \mathcal{L}(z,\lambda,\nu) = \tfrac{1}{2} z^\top H z + f^\top z + \lambda^\top (G z - h) + \nu^\top (E z - b). \]

The KKT conditions are:

• Stationarity:

  \[ H z^\star + f + G^\top \lambda^\star + E^\top \nu^\star = 0. \]

• Primal feasibility:

  \[ G z^\star \le h, \quad E z^\star = b. \]

• Dual feasibility:

  \[ \lambda^\star \ge 0. \]

• Complementary slackness:

  \[ \lambda_i^\star (G z^\star - h)_i = 0 \quad \forall i. \]

In MPC, \( z^\star \) collects predicted states and inputs, while multipliers \( \lambda^\star \) measure the sensitivity of the optimal cost to tightening constraints. Large \( \lambda_i^\star \) indicate active, critical constraints (e.g. torque or joint limits significantly shaping the optimal trajectory).

Problem 2 (Feasible set for 1D joint with input saturation). Consider the scalar system

\[ x_{k+1} = x_k + u_k, \quad |u_k| \le u_{\max}, \]

with horizon \( N \) and state constraint \( |x_k| \le x_{\max} \) for all \( k = 0,\dots,N \). There is no terminal set constraint beyond \( |x_N| \le x_{\max} \). Characterize the set of initial conditions \( \mathcal{X}_N \) for which there exists at least one feasible input sequence.

Solution. The state after \( N \) steps is \( x_N = x_0 + \sum_{k=0}^{N-1} u_k \) with \( |u_k| \le u_{\max} \). The maximum reachable change in state magnitude after \( N \) steps is \( N u_{\max} \) in either direction. To satisfy \( |x_k| \le x_{\max} \) at each step, we must ensure in particular that it is possible to steer from \( x_0 \) into the interval \( [-x_{\max}, x_{\max}] \) at each step using bounded increments.

A sufficient (and for this integrator, also necessary) condition is that \( |x_0| \) is not farther than \( N u_{\max} \) from the admissible interval:

\[ |x_0| \le x_{\max} + N u_{\max}. \]

Indeed, if \( |x_0| > x_{\max} + N u_{\max} \), then even applying the maximum input in the direction of the origin at every step cannot bring the state into \( [-x_{\max}, x_{\max}] \) by time \( N \), so no feasible sequence exists. Conversely, if \( |x_0| \le x_{\max} + N u_{\max} \), one can choose inputs to drive \( x_k \) towards zero with steps of at most \( u_{\max} \), staying within the state bounds at each time because the state moves monotonically towards the admissible region.

Problem 3 (Soft constraint penalty effect). Consider the soft-constrained inequality \( C x \le d + \epsilon \) with \( \epsilon \ge 0 \) and penalty \( \rho \|\epsilon\|_1 \) in the cost. Suppose the unconstrained optimum of the original QP (without this inequality) satisfies \( C x^\star \le d \). Show that for any \( \rho > 0 \), the optimal slack is zero if we add this inequality as a soft constraint.

Solution. Let \( x^\star \) be the optimum of the original problem (without the inequality). Because \( C x^\star \le d \), the pair \( (x^\star, \epsilon = 0) \) is feasible for the soft problem. Any solution with \( \epsilon \neq 0 \) has cost

\[ J(x,\epsilon) = J(x,0) + \rho \|\epsilon\|_1 \ge J(x,0), \]

and in particular, for \( x = x^\star \) we have \( J(x^\star,\epsilon) > J(x^\star,0) \) whenever \( \epsilon \neq 0 \). Thus a nonzero slack can never be optimal; the optimizer must choose \( \epsilon^\star = 0 \). The role of a large penalty \( \rho \) only appears when the original hard inequality may become active or conflicting with other constraints.

Problem 4 (Lyapunov inequality for linear MPC terminal cost). Let \( (A,B) \) be stabilizable and \( K \) be an LQR gain with solution \( P \) to the discrete-time Algebraic Riccati Equation (DARE). Show that

\[ (A+BK)^\top P (A+BK) - P + Q + K^\top R K = 0, \]

and interpret this as a Lyapunov equality for the closed loop.

Solution. The DARE is

\[ P = A^\top P A - A^\top P B (R + B^\top P B)^{-1} B^\top P A + Q. \]

The optimal LQR feedback is \( K = - (R + B^\top P B)^{-1} B^\top P A \). Substituting \( K \) into the DARE and rearranging terms yields exactly

\[ (A+BK)^\top P (A+BK) - P + Q + K^\top R K = 0. \]

This equality states that along the closed-loop trajectories \( x_{k+1} = (A+BK)x_k \), the quantity \( V(x_k) = x_k^\top P x_k \) satisfies

\[ V(x_{k+1}) - V(x_k) = - \left( x_k^\top Q x_k + (K x_k)^\top R (K x_k) \right) \le 0, \]

which is exactly the Lyapunov decrease condition used in Theorem 3. Thus the LQR terminal cost and terminal feedback law provide a rigorous stability certificate for constrained MPC.

Problem 5 (Encoding robot joint limits as linear inequalities). Let \( q \in \mathbb{R}^n \) be joint positions with limits \( q^{\min} \le q \le q^{\max} \). Show how to write these limits as a single matrix inequality \( G_x x \le g_x \) when the state vector is \( x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \in \mathbb{R}^{2n} \).

Solution. The conditions \( q^{\min} \le q \le q^{\max} \) are equivalent to

\[ \begin{bmatrix} I_n \\ -I_n \end{bmatrix} q \le \begin{bmatrix} q^{\max} \\ -q^{\min} \end{bmatrix}. \]

Since \( x = \begin{bmatrix} q \\ \dot{q} \end{bmatrix} \), define the selector matrix

\[ S_q = \begin{bmatrix} I_n & 0_{n \times n} \end{bmatrix}, \]

so that \( q = S_q x \). Then the inequalities become

\[ \begin{bmatrix} I_n \\ -I_n \end{bmatrix} S_q x \le \begin{bmatrix} q^{\max} \\ -q^{\min} \end{bmatrix}. \]

Therefore we can set

\[ G_x = \begin{bmatrix} I_n \\ -I_n \end{bmatrix} S_q, \quad g_x = \begin{bmatrix} q^{\max} \\ -q^{\min} \end{bmatrix}, \]

which is the required linear-inequality form \( G_x x \le g_x \) used in the MPC QP.

In this lesson we formalized how MPC handles state and input constraints for robotic systems, casting the finite-horizon problem as a QP with polyhedral constraint sets. We defined feasible sets and proved recursive feasibility under control-invariant terminal sets, introduced soft constraints via slack variables and exact penalties, and established stability through terminal costs and terminal feedback laws derived from LQR. We also discussed practical implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica for joint-space MPC with joint/torque limits. These concepts form the core of constrained robot MPC and will support the real-time and lab-focused developments in subsequent lessons.