Model predictive control for robots: horizon, constraints and solve time

1. Engineering problem

Many robot motions have limits that a simple feedback law ignores: bounded torque, bounded acceleration, contact forces that must stay inside friction cones. Model predictive control handles these by optimising a short predicted trajectory at every control step.

2. When is this needed?

• Controlling a legged robot or a fast mobile robot with strict limits.
• Reading the Nav2 MPPI controller or humanoid locomotion papers.
• Choosing a solver and a control rate.

3. Selection and design decisions

From the Wikipedia article (read 2026-11-06): MPC controls a process while satisfying a set of constraints; it relies on a dynamic model of the process, uses the current measurements and state, and calculates future changes in the controlled variables to hold the outputs near target while honouring constraints; linear models over a small operating range are common, and nonlinear MPC is used when linear models are not accurate enough. Tools (README descriptions, read 2026-11-06): OCS2 is a C++ toolbox for nonlinear optimal control with an emphasis on real-time MPC for robotics (with a ROS 2 branch); acados provides fast, embedded solvers for nonlinear optimal control used in MPC and moving horizon estimation, written in C with Python, MATLAB and Octave interfaces; Crocoddyl is an optimal-control library for robot motion (see its README). The control rate must leave enough time for the solver: the problem size grows with the horizon length.

4. Advantages and disadvantages

• Horizon: longer is better behaved and slower to solve.
• Model fidelity: a better model improves prediction and costs compute.
• Exact constraints against solver time on embedded hardware.

5. Requirements and constraints

• Worst-case solve time is below the control period with margin (measure the worst case, not the mean).
• The model is validated against the robot (see the system identification and calibration guides).
• A fallback controller exists if the solver fails or is late.

6. Architecture

measure state ─▶ solve over the horizon (model, cost, constraints) ─▶ apply the first input ─▶ repeat next step; warm start from the previous plan

7. Implementation

1. Write the model, cost and constraints.
2. Choose the horizon from the time the robot needs to stop or react.
3. Solve in a loop and record iterations or time (below).
4. Add warm starting and a fallback.
5. Test with the real control rate and worst-case conditions.

import numpy as np
dt, umax = 0.1, 1.0
A = np.array([[1, dt], [0, 1]]); B = np.array([[0.5 * dt * dt], [dt]])         # double integrator: position and velocity, acceleration input
def mpc(x0, N, ref=1.0, iters=400):
    # stacked prediction X = F x0 + G U ; minimise sum (pos - ref)^2 + 0.01 vel^2 + 0.05 u^2 over U with |u| <= umax (projected gradient)
    F = np.zeros((2 * N, 2)); G = np.zeros((2 * N, N)); Ak = np.eye(2)
    for i in range(N):
        Ak = A @ Ak; F[2 * i:2 * i + 2] = Ak
        for j in range(i + 1):
            G[2 * i:2 * i + 2, j] = (np.linalg.matrix_power(A, i - j) @ B)[:, 0]
    Q = np.kron(np.eye(N), np.diag([1.0, 0.01])); R = 0.05 * np.eye(N); xr = np.tile([ref, 0.0], N)
    H = G.T @ Q @ G + R; f = G.T @ Q @ (F @ x0 - xr); U = np.zeros(N); step = 1.0 / np.linalg.eigvalsh(H).max()
    for _ in range(iters):
        U = np.clip(U - step * (H @ U + f), -umax, umax)
    return U[0]
for N in (3, 10, 30):
    x = np.array([0.0, 0.0]); peak = 0.0; us = []
    for _ in range(80):
        u = mpc(x, N); us.append(u); x = A @ x + B[:, 0] * u; peak = max(peak, x[0])
    print(f"horizon {N:2d} steps ({N * dt:.1f} s): final position {x[0]:.3f}, overshoot {max(0.0, peak - 1.0):.3f}, max |u| {max(abs(v) for v in us):.2f} (limit {umax})")
print("decision variables per solve = horizon length; the cost of each solve grows with it, and it must finish inside the control period")

Printed (checked by the site’s tests):

horizon  3 steps (0.3 s): final position 0.805, overshoot 0.678, max |u| 0.87 (limit 1.0)
horizon 10 steps (1.0 s): final position 1.000, overshoot 0.063, max |u| 1.00 (limit 1.0)
horizon 30 steps (3.0 s): final position 1.000, overshoot 0.035, max |u| 1.00 (limit 1.0)
decision variables per solve = horizon length; the cost of each solve grows with it, and it must finish inside the control period

Reading it: a double integrator must reach position 1 with the acceleration limited to 1. With a 3-step (0.3 s) horizon the controller cannot see far enough to brake and overshoots by 0.678 (final position 0.805 after 8 s); with 10 steps (1.0 s) the overshoot is 0.063 and the final position is 1.000, and with 30 steps (3.0 s) it is 0.035. The acceleration limit is respected in every case. The problem and the weights are illustrative; the solver is a plain projected-gradient routine, not a production solver.

8. Real examples

• Quadruped locomotion: a short-horizon MPC over contact forces, solved at a fixed control rate that the solver’s worst-case time must beat.

9. Failure scenarios

• A horizon shorter than the braking time.
• A solver that occasionally misses the control deadline.
• An inaccurate model with tight constraints.

10. Troubleshooting

1. Plot the predicted and the executed trajectories.
2. Measure worst-case solve time under load.
3. Check the model against logged data.

11. Validation

• The example prints the lines shown (executed by the site tests).
• The worst-case solve time on the target computer is below the control period with margin.

12. Engineering checklist

• Model, cost, constraints written
• Horizon from the braking time
• Worst-case solve time measured
• Warm start
• Fallback controller
• Model validated

13. Further references

• Wikipedia, Model predictive control, read 2026-11-06 (secondary source).
• OCS2 README: leggedrobotics/ocs2 (a C++ toolbox for nonlinear optimal control with an emphasis on real-time model predictive control for robotics; a ROS 2 branch exists), read 2026-11-06.
• acados README: acados/acados (fast embedded solvers for nonlinear optimal control, for model predictive control and moving horizon estimation), read 2026-11-06.

Related on FixTheRobot