Squashed 'third_party/osqp/' content from commit 33454b3e23
Change-Id: I056df0582ca06664e86554c341a94c47ab932001
git-subtree-dir: third_party/osqp
git-subtree-split: 33454b3e236f1f44193bfbbb6b8c8e71f8f04e9a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/docs/examples/mpc.rst b/docs/examples/mpc.rst
new file mode 100644
index 0000000..f5da713
--- /dev/null
+++ b/docs/examples/mpc.rst
@@ -0,0 +1,489 @@
+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