Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 11e61afdc9fdbea03f79e99f889fb2adb23f8b84 / . / third_party / osqp / docs / examples / mpc.rst
blob: f5da713c998782a2781da576002597a23bd242ed [file] [log] [blame]
Model predictive control (MPC)
==============================
We consider the problem of controlling a linear time-invariant dynamical system to some reference state :math:`x_r \in \mathbf{R}^{n_x}`.
To achieve this we use *constrained linear-quadratic MPC*, which solves at each time step the following finite-horizon optimal control problem
.. math::
\begin{array}{ll}
\mbox{minimize} & (x_N-x_r)^T Q_N (x_N-x_r) + \sum_{k=0}^{N-1} (x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k \\
\mbox{subject to} & x_{k+1} = A x_k + B u_k \\
& x_{\rm min} \le x_k \le x_{\rm max} \\
& u_{\rm min} \le u_k \le u_{\rm max} \\
& x_0 = \bar{x}
\end{array}
The states :math:`x_k \in \mathbf{R}^{n_x}` and the inputs :math:`u_k \in \mathbf{R}^{n_u}` are constrained to be between some lower and upper bounds.
The problem is solved repeatedly for varying initial state :math:`\bar{x} \in \mathbf{R}^{n_x}`.
Python
------
.. code:: python
import osqp
import numpy as np
import scipy as sp
from scipy import sparse
# Discrete time model of a quadcopter
Ad = sparse.csc_matrix([
[1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
[0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
[0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
[0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
[0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
[0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
[0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
[0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
])
Bd = sparse.csc_matrix([
[0., -0.0726, 0., 0.0726],
[-0.0726, 0., 0.0726, 0. ],
[-0.0152, 0.0152, -0.0152, 0.0152],
[-0., -0.0006, -0., 0.0006],
[0.0006, 0., -0.0006, 0.0000],
[0.0106, 0.0106, 0.0106, 0.0106],
[0, -1.4512, 0., 1.4512],
[-1.4512, 0., 1.4512, 0. ],
[-0.3049, 0.3049, -0.3049, 0.3049],
[-0., -0.0236, 0., 0.0236],
[0.0236, 0., -0.0236, 0. ],
[0.2107, 0.2107, 0.2107, 0.2107]])
[nx, nu] = Bd.shape
# Constraints
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
# Objective function
Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
QN = Q
R = 0.1*sparse.eye(4)
# Initial and reference states
x0 = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])
# Prediction horizon
N = 10
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)], format='csc')
# - linear objective
q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
np.zeros(N*nu)])
# - linear dynamics
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
ueq = leq
# - input and state constraints
Aineq = sparse.eye((N+1)*nx + N*nu)
lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq], format='csc')
l = np.hstack([leq, lineq])
u = np.hstack([ueq, uineq])
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace
prob.setup(P, q, A, l, u, warm_start=True)
# Simulate in closed loop
nsim = 15
for i in range(nsim):
# Solve
res = prob.solve()
# Check solver status
if res.info.status != 'solved':
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
ctrl = res.x[-N*nu:-(N-1)*nu]
x0 = Ad.dot(x0) + Bd.dot(ctrl)
# Update initial state
l[:nx] = -x0
u[:nx] = -x0
prob.update(l=l, u=u)
Matlab
------
.. code:: matlab
% Discrete time model of a quadcopter
Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
0 1 0 0 0 0 0 0.1 0 0 0 0;
0 0 1 0 0 0 0 0 0.1 0 0 0;
0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
0 0 0 0 0 1 0 0 0 0 0 0.0992;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
0 0 0 0 0 0 0 0 0 0 0 0.9846];
Bd = [0 -0.0726 0 0.0726;
-0.0726 0 0.0726 0;
-0.0152 0.0152 -0.0152 0.0152;
0 -0.0006 -0.0000 0.0006;
0.0006 0 -0.0006 0;
0.0106 0.0106 0.0106 0.0106;
0 -1.4512 0 1.4512;
-1.4512 0 1.4512 0;
-0.3049 0.3049 -0.3049 0.3049;
0 -0.0236 0 0.0236;
0.0236 0 -0.0236 0;
0.2107 0.2107 0.2107 0.2107];
[nx, nu] = size(Bd);
% Constraints
u0 = 10.5916;
umin = [9.6; 9.6; 9.6; 9.6] - u0;
umax = [13; 13; 13; 13] - u0;
xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
xmax = [ pi/6; pi/6; Inf; Inf; Inf; Inf; Inf(6,1)];
% Objective function
Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
QN = Q;
R = 0.1*eye(4);
% Initial and reference states
x0 = zeros(12,1);
xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
% Prediction horizon
N = 10;
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; zeros(N*nu, 1)];
% - linear dynamics
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), Ad);
Bu = kron([sparse(1, N); speye(N)], Bd);
Aeq = [Ax, Bu];
leq = [-x0; zeros(N*nx, 1)];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq];
l = [leq; lineq];
u = [ueq; uineq];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Simulate in closed loop
nsim = 15;
for i = 1 : nsim
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
error('OSQP did not solve the problem!')
end
% Apply first control input to the plant
ctrl = res.x((N+1)*nx+1:(N+1)*nx+nu);
x0 = Ad*x0 + Bd*ctrl;
% Update initial state
l(1:nx) = -x0;
u(1:nx) = -x0;
prob.update('l', l, 'u', u);
end
CVXPY
-----
.. code:: python
from cvxpy import *
import numpy as np
import scipy as sp
from scipy import sparse
# Discrete time model of a quadcopter
Ad = sparse.csc_matrix([
[1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
[0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
[0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
[0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
[0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
[0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
[0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
[0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
])
Bd = sparse.csc_matrix([
[0., -0.0726, 0., 0.0726],
[-0.0726, 0., 0.0726, 0. ],
[-0.0152, 0.0152, -0.0152, 0.0152],
[-0., -0.0006, -0., 0.0006],
[0.0006, 0., -0.0006, 0.0000],
[0.0106, 0.0106, 0.0106, 0.0106],
[0, -1.4512, 0., 1.4512],
[-1.4512, 0., 1.4512, 0. ],
[-0.3049, 0.3049, -0.3049, 0.3049],
[-0., -0.0236, 0., 0.0236],
[0.0236, 0., -0.0236, 0. ],
[0.2107, 0.2107, 0.2107, 0.2107]])
[nx, nu] = Bd.shape
# Constraints
u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0
xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
# Objective function
Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
QN = Q
R = 0.1*sparse.eye(4)
# Initial and reference states
x0 = np.zeros(12)
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])
# Prediction horizon
N = 10
# Define problem
u = Variable((nu, N))
x = Variable((nx, N+1))
x_init = Parameter(nx)
objective = 0
constraints = [x[:,0] == x_init]
for k in range(N):
objective += quad_form(x[:,k] - xr, Q) + quad_form(u[:,k], R)
constraints += [x[:,k+1] == Ad*x[:,k] + Bd*u[:,k]]
constraints += [xmin <= x[:,k], x[:,k] <= xmax]
constraints += [umin <= u[:,k], u[:,k] <= umax]
objective += quad_form(x[:,N] - xr, QN)
prob = Problem(Minimize(objective), constraints)
# Simulate in closed loop
nsim = 15
for i in range(nsim):
x_init.value = x0
prob.solve(solver=OSQP, warm_start=True)
x0 = Ad.dot(x0) + Bd.dot(u[:,0].value)
YALMIP
------
.. code:: matlab
% Discrete time model of a quadcopter
Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
0 1 0 0 0 0 0 0.1 0 0 0 0;
0 0 1 0 0 0 0 0 0.1 0 0 0;
0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
0 0 0 0 0 1 0 0 0 0 0 0.0992;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
0 0 0 0 0 0 0 0 0 0 0 0.9846];
Bd = [0 -0.0726 0 0.0726;
-0.0726 0 0.0726 0;
-0.0152 0.0152 -0.0152 0.0152;
0 -0.0006 -0.0000 0.0006;
0.0006 0 -0.0006 0;
0.0106 0.0106 0.0106 0.0106;
0 -1.4512 0 1.4512;
-1.4512 0 1.4512 0;
-0.3049 0.3049 -0.3049 0.3049;
0 -0.0236 0 0.0236;
0.0236 0 -0.0236 0;
0.2107 0.2107 0.2107 0.2107];
[nx, nu] = size(Bd);
% Constraints
u0 = 10.5916;
umin = [9.6; 9.6; 9.6; 9.6] - u0;
umax = [13; 13; 13; 13] - u0;
xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
xmax = [ pi/6; pi/6; Inf; Inf; Inf; Inf; Inf(6,1)];
% Objective function
Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
QN = Q;
R = 0.1*eye(4);
% Initial and reference states
x0 = zeros(12,1);
xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
% Prediction horizon
N = 10;
% Define problem
u = sdpvar(repmat(nu,1,N), repmat(1,1,N));
x = sdpvar(repmat(nx,1,N+1), repmat(1,1,N+1));
constraints = [xmin <= x{1} <= xmax];
objective = 0;
for k = 1 : N
objective = objective + (x{k}-xr)'*Q*(x{k}-xr) + u{k}'*R*u{k};
constraints = [constraints, x{k+1} == Ad*x{k} + Bd*u{k}];
constraints = [constraints, umin <= u{k}<= umax, xmin <= x{k+1} <= xmax];
end
objective = objective + (x{N+1}-xr)'*QN*(x{N+1}-xr);
options = sdpsettings('solver', 'osqp');
controller = optimizer(constraints, objective, options, x{1}, [u{:}]);
% Simulate in closed loop
nsim = 15;
for i = 1 : nsim
U = controller{x0};
x0 = Ad*x0 + Bd*U(:,1);
end
Julia
------
.. code:: julia
# Add packages - uncomment for first-time setup
# using Pkg; Pkg.add(["SparseArrays", "OSQP"])
using SparseArrays, OSQP
# Utility function
speye(N) = spdiagm(ones(N))
# Discrete time model of a quadcopter
Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
0 1 0 0 0 0 0 0.1 0 0 0 0;
0 0 1 0 0 0 0 0 0.1 0 0 0;
0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
0 0 0 0 0 1 0 0 0 0 0 0.0992;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
0 0 0 0 0 0 0 0 0 0 0 0.9846] |> sparse
Bd = [0 -0.0726 0 0.0726;
-0.0726 0 0.0726 0;
-0.0152 0.0152 -0.0152 0.0152;
0 -0.0006 -0.0000 0.0006;
0.0006 0 -0.0006 0;
0.0106 0.0106 0.0106 0.0106;
0 -1.4512 0 1.4512;
-1.4512 0 1.4512 0;
-0.3049 0.3049 -0.3049 0.3049;
0 -0.0236 0 0.0236;
0.0236 0 -0.0236 0;
0.2107 0.2107 0.2107 0.2107] |> sparse
(nx, nu) = size(Bd)
# Constraints
u0 = 10.5916
umin = [9.6, 9.6, 9.6, 9.6] .- u0
umax = [13, 13, 13, 13] .- u0
xmin = [[-pi/6, -pi/6, -Inf, -Inf, -Inf, -1]; -Inf .* ones(6)]
xmax = [[pi/6, pi/6, Inf, Inf, Inf, Inf]; Inf .* ones(6)]
# Objective function
Q = spdiagm([0, 0, 10, 10, 10, 10, 0, 0, 0, 5, 5, 5])
QN = Q
R = 0.1 * speye(nu)
# Initial and reference states
x0 = zeros(12)
xr = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# Prediction horizon
N = 10
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective
P = blockdiag(kron(speye(N), Q), QN, kron(speye(N), R))
# - linear objective
q = [repeat(-Q * xr, N); -QN * xr; zeros(N*nu)]
# - linear dynamics
Ax = kron(speye(N + 1), -speye(nx)) + kron(spdiagm(-1 => ones(N)), Ad)
Bu = kron([spzeros(1, N); speye(N)], Bd)
Aeq = [Ax Bu]
leq = [-x0; zeros(N * nx)]
ueq = leq
# - input and state constraints
Aineq = speye((N + 1) * nx + N * nu)
lineq = [repeat(xmin, N + 1); repeat(umin, N)]
uineq = [repeat(xmax, N + 1); repeat(umax, N)]
# - OSQP constraints
A, l, u = [Aeq; Aineq], [leq; lineq], [ueq; uineq]
# Create an OSQP model
m = OSQP.Model()
# Setup workspace
OSQP.setup!(m; P=P, q=q, A=A, l=l, u=u, warm_start=true)
# Simulate in closed loop
nsim = 15;
@time for _ in 1 : nsim
# Solve
res = OSQP.solve!(m)
# Check solver status
if res.info.status != :Solved
error("OSQP did not solve the problem!")
end
# Apply first control input to the plant
ctrl = res.x[(N+1)*nx+1:(N+1)*nx+nu]
global x0 = Ad * x0 + Bd * ctrl
# Update initial state
l[1:nx], u[1:nx] = -x0, -x0
OSQP.update!(m; l=l, u=u)
end