Model Predictive Control
In this example we shall demonstrate an instance of using the box cone, as well as reusing a cached workspace and using warm-starting.
In model predictive control (MPC) the control action at each time-step is obtained by solving an optimization problem that simulates the dynamical system over some time horizon. Here, we consider the problem of controlling a linear, time-invariant dynamical systems with quadratic stage costs and box constraints on the state and action:
\[\begin{split}\begin{array}{ll}
\mbox{minimize} & x_T^T Q_T x_T + q_T^\top x_T + \sum_{t=0}^{T-1}\left( x_t^T Q x_t + u_t^T R u_t + q^\top x_t \right) \\
\mbox{subject to} & x_{t+1} = A x_t + B u_t, \quad t = 0, \ldots T-1\\
& x_{\rm min} \le x_t \le x_{\rm max}, \quad t = 0, \ldots T\\
& u_{\rm min} \le u_t \le u_{\rm max}, \quad t = 0, \ldots T-1\\
& x_0 = \bar{x}
\end{array}\end{split}\]
over variables corresponding to the states \(x_t \in \mathbf{R}^{n}\), \(t=0,\ldots,T\), and the inputs \(u_t \in \mathbf{R}^{m}\), \(t=0,\ldots,T-1\), where \(A \in \mathbf{R}^{n \times n}\), \(B \in \mathbf{R}^{n \times m}\) correspond to the linear dynamics and \(Q \in \mathbf{R}^{n \times n}\), \(R \in \mathbf{R}^{m \times m}\), \(q \in \mathbf{R}^{n}\) correspond to the positive semidefinite quadratic cost functions with terminal cost defined by \(Q_T \in \mathbf{R}^{n \times n}\) and \(q_T \in \mathbf{R}^{n}\).
The problem is solved repeatedly for varying initial state \(\bar{x} \in \mathbf{R}^{n}\). The upper and lower bound constraints can be expressed in SCS using the box cone.
Python code to solve this is below.
import scs
import numpy as np
import scipy as sp
from scipy import sparse
np.random.seed(1)
class MPC(object):
"""Model Predictive Contoller using SCS."""
def __init__(self, Ad, Bd, Q, R, q, QT, qT, xmin, xmax, umin, umax, T):
# State and action dimension
self.nx, self.nu = Bd.shape
# Stack variables as follows:
# [x_0, x_1, ..., x_{T-1}, x_T, u_0, u_1, ..., u_{T-1}]
# Quadratic objective
P = sparse.block_diag(
[sparse.kron(sparse.eye(T), Q), QT, sparse.kron(sparse.eye(T), R)],
format="csc",
)
# Linear objective
c = np.hstack([np.kron(np.ones(T), q), qT, np.zeros(T * self.nu)])
# Linear dynamics
Ax = sparse.kron(sparse.eye(T + 1), -sparse.eye(self.nx)) + sparse.kron(
sparse.eye(T + 1, k=-1), Ad
)
Bu = sparse.kron(
sparse.vstack([sparse.csc_matrix((1, T)), sparse.eye(T)]), Bd
)
Aeq = sparse.hstack([Ax, Bu])
# Will update this later with initial state
beq = np.zeros((T + 1) * self.nx)
# Box constraints on state and action
Aineq = sparse.eye((T + 1) * self.nx + T * self.nu)
box_lower = np.hstack(
[np.kron(np.ones(T + 1), xmin), np.kron(np.ones(T), umin)]
)
box_upper = np.hstack(
[np.kron(np.ones(T + 1), xmax), np.kron(np.ones(T), umax)]
)
A = sparse.vstack(
[
# zero cone
Aeq,
# Box cone {(t, s) | -t l <= s <= t u }
sparse.csc_matrix((1, (T + 1) * self.nx + T * self.nu)),
-Aineq,
],
format="csc",
)
# Box cone add constraint t=1
self.b = np.hstack([beq, 1, np.zeros((T + 1) * self.nx + T * self.nu)])
data = dict(P=P, A=A, b=self.b, c=c)
cone = dict(z=(T + 1) * self.nx, bu=box_upper, bl=box_lower)
# Create an SCS object
self.solver = scs.SCS(data, cone, eps_abs=1e-5, eps_rel=1e-5)
def control(self, x0):
# Overwrite b with new initial state
self.b[: self.nx] = -x0
# Update b
self.solver.update(b=self.b)
sol = self.solver.solve() # will warm-start automatically
if sol["info"]["status"] != "solved":
raise ValueError("SCS failed to solve the problem.")
# Return first action
return sol["x"][-T * self.nu : -(T - 1) * self.nu]
# States dimension
nx = 20
# Control dimension
nu = 5
# State dynamics matrices
Ad = 0.1 * np.random.randn(nx, nx) # State -> State
Bd = np.random.randn(nx, nu) # Control -> State
# Cost matrices
Q = sparse.eye(nx) # State
QT = 10 * Q # Terminal State
R = 0.1 * sparse.eye(nu) # Control
# Linear cost vector
q = 0.1 * np.random.randn(nx)
qT = q
# Initial state
x0 = 10 * np.random.randn(nx)
# Prediction horizon
T = 30
# Bounds on state
xmax = np.inf * np.ones(nx)
xmin = -np.inf * np.ones(nx)
# Bounds on control
umax = np.ones(nu)
umin = -np.ones(nu)
# Initialize Model Predictive Controller
mpc = MPC(Ad, Bd, Q, R, q, QT, qT, xmin, xmax, umin, umax, T)
# Simulate in closed loop
nsteps = 10 # Number of steps
for i in range(nsteps):
# Get control action
u = mpc.control(x0)
print(f"Control action: {u}")
# Apply first control input and update to next state
x0 = Ad @ x0 + Bd @ u + 0.01 * np.random.normal(nx, 1) # + noise
x0 = np.maximum(np.minimum(x0, xmax), xmin) # Bound to xmin, xmax
After following the python install instructions, we can run the code yielding output:
------------------------------------------------------------------
SCS v3.1.1 - Splitting Conic Solver
(c) Brendan O'Donoghue, Stanford University, 2012
------------------------------------------------------------------
problem: variables n: 770, constraints m: 1391
cones: z: primal zero / dual free vars: 620
b: box cone vars: 771
settings: eps_abs: 1.0e-05, eps_rel: 1.0e-05, eps_infeas: 1.0e-07
alpha: 1.50, scale: 1.00e-01, adaptive_scale: 1
max_iters: 100000, normalize: 1, rho_x: 1.00e-06
acceleration_lookback: 10, acceleration_interval: 10
lin-sys: sparse-direct
nnz(A): 16390, nnz(P): 770
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 1.74e+01 3.51e+00 3.73e+03 2.35e+03 1.00e-01 3.09e-04
225| 6.23e-11 1.21e-11 1.04e-09 1.09e+03 1.30e+00 2.33e-02
------------------------------------------------------------------
status: solved
timings: total: 2.34e-02s = setup: 9.72e-06s + solve: 2.34e-02s
lin-sys: 1.84e-02s, cones: 1.09e-03s, accel: 6.86e-04s
------------------------------------------------------------------
objective = 1088.948282
------------------------------------------------------------------
Control action: [ 0.41348794 -1. -0.38009805 -1. -0.42945415]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 2.46e+01 6.40e+01 2.05e+03 9.35e+02 1.30e+00 4.82e-04
25| 5.67e-07 2.89e-05 4.55e-04 1.43e+02 1.30e+00 3.84e-03
------------------------------------------------------------------
status: solved
timings: total: 3.87e-03s = setup: 1.59e-05s + solve: 3.85e-03s
lin-sys: 2.81e-03s, cones: 2.09e-04s, accel: 1.12e-04s
------------------------------------------------------------------
objective = 142.600803
------------------------------------------------------------------
Control action: [-0.44966462 -0.03355008 0.52249607 0.19250174 0.14671164]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 9.09e+00 2.36e+01 2.87e+02 9.13e+01 1.30e+00 3.74e-04
25| 1.28e-07 5.65e-06 2.59e-05 1.01e+01 1.30e+00 3.69e-03
------------------------------------------------------------------
status: solved
timings: total: 3.72e-03s = setup: 1.28e-05s + solve: 3.71e-03s
lin-sys: 2.79e-03s, cones: 2.11e-04s, accel: 2.78e-05s
------------------------------------------------------------------
objective = 10.124977
------------------------------------------------------------------
Control action: [ 0.18436581 0.01049508 -0.01660706 -0.10110899 -0.00797351]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 2.97e+00 7.72e+00 2.21e+01 8.98e+00 1.30e+00 4.36e-04
25| 4.95e-08 2.08e-06 2.67e-06 6.17e-01 1.30e+00 3.85e-03
------------------------------------------------------------------
status: solved
timings: total: 3.88e-03s = setup: 1.18e-05s + solve: 3.87e-03s
lin-sys: 2.95e-03s, cones: 2.29e-04s, accel: 2.76e-05s
------------------------------------------------------------------
objective = 0.616727
------------------------------------------------------------------
Control action: [-0.05327529 0.02863548 0.02321702 0.05851298 0.05562412]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 9.21e-01 2.39e+00 1.32e+00 2.03e+00 1.30e+00 3.92e-04
25| 1.79e-08 6.46e-07 6.26e-07 2.25e-01 1.30e+00 3.58e-03
------------------------------------------------------------------
status: solved
timings: total: 3.61e-03s = setup: 1.17e-05s + solve: 3.60e-03s
lin-sys: 2.76e-03s, cones: 1.88e-04s, accel: 2.94e-05s
------------------------------------------------------------------
objective = 0.224881
------------------------------------------------------------------
Control action: [ 0.00742414 0.01912228 -0.03393191 0.03964983 0.00193193]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 3.06e-01 7.95e-01 4.69e-01 7.86e-02 1.30e+00 4.53e-04
25| 6.91e-09 2.38e-07 5.74e-08 4.16e-02 1.30e+00 3.61e-03
------------------------------------------------------------------
status: solved
timings: total: 3.65e-03s = setup: 2.32e-05s + solve: 3.63e-03s
lin-sys: 2.67e-03s, cones: 1.68e-04s, accel: 2.56e-05s
------------------------------------------------------------------
objective = 0.041573
------------------------------------------------------------------
Control action: [-0.01297334 0.02363413 -0.03044127 0.05802441 -0.00469033]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 1.11e-01 2.89e-01 5.02e-02 -3.43e-02 1.30e+00 3.57e-04
25| 4.30e-09 1.81e-07 1.22e-08 -1.47e-02 1.30e+00 3.48e-03
------------------------------------------------------------------
status: solved
timings: total: 3.51e-03s = setup: 1.26e-05s + solve: 3.50e-03s
lin-sys: 2.55e-03s, cones: 1.59e-04s, accel: 1.49e-04s
------------------------------------------------------------------
objective = -0.014719
------------------------------------------------------------------
Control action: [-0.01673083 0.02410207 -0.03265513 0.05713659 0.00074876]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 4.88e-02 1.27e-01 1.10e-01 1.95e-01 1.30e+00 4.38e-04
25| 1.05e-09 3.82e-08 5.28e-08 5.71e-02 1.30e+00 4.01e-03
------------------------------------------------------------------
status: solved
timings: total: 4.04e-03s = setup: 1.17e-05s + solve: 4.03e-03s
lin-sys: 3.09e-03s, cones: 2.11e-04s, accel: 1.81e-05s
------------------------------------------------------------------
objective = 0.057139
------------------------------------------------------------------
Control action: [-0.01873535 0.02513378 -0.03083648 0.0586056 0.00248981]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 2.81e-02 7.30e-02 1.51e-01 3.06e-01 1.30e+00 4.49e-04
25| 5.55e-10 2.30e-08 6.88e-08 1.41e-01 1.30e+00 3.98e-03
------------------------------------------------------------------
status: solved
timings: total: 4.01e-03s = setup: 1.25e-05s + solve: 4.00e-03s
lin-sys: 3.05e-03s, cones: 2.10e-04s, accel: 3.11e-05s
------------------------------------------------------------------
objective = 0.140619
------------------------------------------------------------------
Control action: [-0.01807515 0.02511239 -0.03288644 0.0597179 0.00232987]
------------------------------------------------------------------
iter | pri res | dua res | gap | obj | scale | time (s)
------------------------------------------------------------------
0| 8.98e-03 2.33e-02 2.83e-02 1.86e-01 1.30e+00 3.14e-04
25| 1.54e-10 6.84e-09 1.30e-08 1.56e-01 1.30e+00 3.47e-03
------------------------------------------------------------------
status: solved
timings: total: 3.50e-03s = setup: 1.07e-05s + solve: 3.49e-03s
lin-sys: 2.68e-03s, cones: 1.64e-04s, accel: 2.98e-05s
------------------------------------------------------------------
objective = 0.156128
------------------------------------------------------------------
Control action: [-0.01797391 0.02512434 -0.03330189 0.06002231 0.00188589]