Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 11e61afdc9fdbea03f79e99f889fb2adb23f8b84 / . / third_party / osqp / docs / examples / mpc.rst
blob: f5da713c998782a2781da576002597a23bd242ed [file] [log] [blame]
Austin Schuh9049e202022-02-20 17:34:16 -0800[diff] [blame]1Model predictive control (MPC)
2==============================
3
4We consider the problem of controlling a linear time-invariant dynamical system to some reference state :math:`x_r \in \mathbf{R}^{n_x}`.
5To achieve this we use *constrained linear-quadratic MPC*, which solves at each time step the following finite-horizon optimal control problem
6
7.. math::
8 \begin{array}{ll}
9 \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 \\
10 \mbox{subject to} & x_{k+1} = A x_k + B u_k \\
11 & x_{\rm min} \le x_k \le x_{\rm max} \\
12 & u_{\rm min} \le u_k \le u_{\rm max} \\
13 & x_0 = \bar{x}
14 \end{array}
15
16The 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.
17The problem is solved repeatedly for varying initial state :math:`\bar{x} \in \mathbf{R}^{n_x}`.
18
19
20Python
21------
22
23.. code:: python
24
25 import osqp
26 import numpy as np
27 import scipy as sp
28 from scipy import sparse
29
30 # Discrete time model of a quadcopter
31 Ad = sparse.csc_matrix([
32 [1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
33 [0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
34 [0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
35 [0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
36 [0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
37 [0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
38 [0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
39 [0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
40 [0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
41 [0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
42 [0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
43 [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
44 ])
45 Bd = sparse.csc_matrix([
46 [0., -0.0726, 0., 0.0726],
47 [-0.0726, 0., 0.0726, 0. ],
48 [-0.0152, 0.0152, -0.0152, 0.0152],
49 [-0., -0.0006, -0., 0.0006],
50 [0.0006, 0., -0.0006, 0.0000],
51 [0.0106, 0.0106, 0.0106, 0.0106],
52 [0, -1.4512, 0., 1.4512],
53 [-1.4512, 0., 1.4512, 0. ],
54 [-0.3049, 0.3049, -0.3049, 0.3049],
55 [-0., -0.0236, 0., 0.0236],
56 [0.0236, 0., -0.0236, 0. ],
57 [0.2107, 0.2107, 0.2107, 0.2107]])
58 [nx, nu] = Bd.shape
59
60 # Constraints
61 u0 = 10.5916
62 umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
63 umax = np.array([13., 13., 13., 13.]) - u0
64 xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
65 -np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
66 xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
67 np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
68
69 # Objective function
70 Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
71 QN = Q
72 R = 0.1*sparse.eye(4)
73
74 # Initial and reference states
75 x0 = np.zeros(12)
76 xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])
77
78 # Prediction horizon
79 N = 10
80
81 # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
82 # - quadratic objective
83 P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
84 sparse.kron(sparse.eye(N), R)], format='csc')
85 # - linear objective
86 q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
87 np.zeros(N*nu)])
88 # - linear dynamics
89 Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
90 Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd)
91 Aeq = sparse.hstack([Ax, Bu])
92 leq = np.hstack([-x0, np.zeros(N*nx)])
93 ueq = leq
94 # - input and state constraints
95 Aineq = sparse.eye((N+1)*nx + N*nu)
96 lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
97 uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
98 # - OSQP constraints
99 A = sparse.vstack([Aeq, Aineq], format='csc')
100 l = np.hstack([leq, lineq])
101 u = np.hstack([ueq, uineq])
102
103 # Create an OSQP object
104 prob = osqp.OSQP()
105
106 # Setup workspace
107 prob.setup(P, q, A, l, u, warm_start=True)
108
109 # Simulate in closed loop
110 nsim = 15
111 for i in range(nsim):
112 # Solve
113 res = prob.solve()
114
115 # Check solver status
116 if res.info.status != 'solved':
117 raise ValueError('OSQP did not solve the problem!')
118
119 # Apply first control input to the plant
120 ctrl = res.x[-N*nu:-(N-1)*nu]
121 x0 = Ad.dot(x0) + Bd.dot(ctrl)
122
123 # Update initial state
124 l[:nx] = -x0
125 u[:nx] = -x0
126 prob.update(l=l, u=u)
127
128
129
130Matlab
131------
132
133.. code:: matlab
134
135 % Discrete time model of a quadcopter
136 Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
137 0 1 0 0 0 0 0 0.1 0 0 0 0;
138 0 0 1 0 0 0 0 0 0.1 0 0 0;
139 0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
140 0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
141 0 0 0 0 0 1 0 0 0 0 0 0.0992;
142 0 0 0 0 0 0 1 0 0 0 0 0;
143 0 0 0 0 0 0 0 1 0 0 0 0;
144 0 0 0 0 0 0 0 0 1 0 0 0;
145 0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
146 0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
147 0 0 0 0 0 0 0 0 0 0 0 0.9846];
148 Bd = [0 -0.0726 0 0.0726;
149 -0.0726 0 0.0726 0;
150 -0.0152 0.0152 -0.0152 0.0152;
151 0 -0.0006 -0.0000 0.0006;
152 0.0006 0 -0.0006 0;
153 0.0106 0.0106 0.0106 0.0106;
154 0 -1.4512 0 1.4512;
155 -1.4512 0 1.4512 0;
156 -0.3049 0.3049 -0.3049 0.3049;
157 0 -0.0236 0 0.0236;
158 0.0236 0 -0.0236 0;
159 0.2107 0.2107 0.2107 0.2107];
160 [nx, nu] = size(Bd);
161
162 % Constraints
163 u0 = 10.5916;
164 umin = [9.6; 9.6; 9.6; 9.6] - u0;
165 umax = [13; 13; 13; 13] - u0;
166 xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
167 xmax = [ pi/6; pi/6; Inf; Inf; Inf; Inf; Inf(6,1)];
168
169 % Objective function
170 Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
171 QN = Q;
172 R = 0.1*eye(4);
173
174 % Initial and reference states
175 x0 = zeros(12,1);
176 xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
177
178 % Prediction horizon
179 N = 10;
180
181 % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
182 % - quadratic objective
183 P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
184 % - linear objective
185 q = [repmat(-Q*xr, N, 1); -QN*xr; zeros(N*nu, 1)];
186 % - linear dynamics
187 Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), Ad);
188 Bu = kron([sparse(1, N); speye(N)], Bd);
189 Aeq = [Ax, Bu];
190 leq = [-x0; zeros(N*nx, 1)];
191 ueq = leq;
192 % - input and state constraints
193 Aineq = speye((N+1)*nx + N*nu);
194 lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
195 uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
196 % - OSQP constraints
197 A = [Aeq; Aineq];
198 l = [leq; lineq];
199 u = [ueq; uineq];
200
201 % Create an OSQP object
202 prob = osqp;
203
204 % Setup workspace
205 prob.setup(P, q, A, l, u, 'warm_start', true);
206
207 % Simulate in closed loop
208 nsim = 15;
209 for i = 1 : nsim
210 % Solve
211 res = prob.solve();
212
213 % Check solver status
214 if ~strcmp(res.info.status, 'solved')
215 error('OSQP did not solve the problem!')
216 end
217
218 % Apply first control input to the plant
219 ctrl = res.x((N+1)*nx+1:(N+1)*nx+nu);
220 x0 = Ad*x0 + Bd*ctrl;
221
222 % Update initial state
223 l(1:nx) = -x0;
224 u(1:nx) = -x0;
225 prob.update('l', l, 'u', u);
226 end
227
228
229
230CVXPY
231-----
232
233.. code:: python
234
235 from cvxpy import *
236 import numpy as np
237 import scipy as sp
238 from scipy import sparse
239
240 # Discrete time model of a quadcopter
241 Ad = sparse.csc_matrix([
242 [1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ],
243 [0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ],
244 [0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ],
245 [0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ],
246 [0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ],
247 [0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992],
248 [0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ],
249 [0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ],
250 [0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ],
251 [0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ],
252 [0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ],
253 [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846]
254 ])
255 Bd = sparse.csc_matrix([
256 [0., -0.0726, 0., 0.0726],
257 [-0.0726, 0., 0.0726, 0. ],
258 [-0.0152, 0.0152, -0.0152, 0.0152],
259 [-0., -0.0006, -0., 0.0006],
260 [0.0006, 0., -0.0006, 0.0000],
261 [0.0106, 0.0106, 0.0106, 0.0106],
262 [0, -1.4512, 0., 1.4512],
263 [-1.4512, 0., 1.4512, 0. ],
264 [-0.3049, 0.3049, -0.3049, 0.3049],
265 [-0., -0.0236, 0., 0.0236],
266 [0.0236, 0., -0.0236, 0. ],
267 [0.2107, 0.2107, 0.2107, 0.2107]])
268 [nx, nu] = Bd.shape
269
270 # Constraints
271 u0 = 10.5916
272 umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
273 umax = np.array([13., 13., 13., 13.]) - u0
274 xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
275 -np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
276 xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
277 np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])
278
279 # Objective function
280 Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])
281 QN = Q
282 R = 0.1*sparse.eye(4)
283
284 # Initial and reference states
285 x0 = np.zeros(12)
286 xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.])
287
288 # Prediction horizon
289 N = 10
290
291 # Define problem
292 u = Variable((nu, N))
293 x = Variable((nx, N+1))
294 x_init = Parameter(nx)
295 objective = 0
296 constraints = [x[:,0] == x_init]
297 for k in range(N):
298 objective += quad_form(x[:,k] - xr, Q) + quad_form(u[:,k], R)
299 constraints += [x[:,k+1] == Ad*x[:,k] + Bd*u[:,k]]
300 constraints += [xmin <= x[:,k], x[:,k] <= xmax]
301 constraints += [umin <= u[:,k], u[:,k] <= umax]
302 objective += quad_form(x[:,N] - xr, QN)
303 prob = Problem(Minimize(objective), constraints)
304
305 # Simulate in closed loop
306 nsim = 15
307 for i in range(nsim):
308 x_init.value = x0
309 prob.solve(solver=OSQP, warm_start=True)
310 x0 = Ad.dot(x0) + Bd.dot(u[:,0].value)
311
312
313
314YALMIP
315------
316
317.. code:: matlab
318
319 % Discrete time model of a quadcopter
320 Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
321 0 1 0 0 0 0 0 0.1 0 0 0 0;
322 0 0 1 0 0 0 0 0 0.1 0 0 0;
323 0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
324 0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
325 0 0 0 0 0 1 0 0 0 0 0 0.0992;
326 0 0 0 0 0 0 1 0 0 0 0 0;
327 0 0 0 0 0 0 0 1 0 0 0 0;
328 0 0 0 0 0 0 0 0 1 0 0 0;
329 0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
330 0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
331 0 0 0 0 0 0 0 0 0 0 0 0.9846];
332 Bd = [0 -0.0726 0 0.0726;
333 -0.0726 0 0.0726 0;
334 -0.0152 0.0152 -0.0152 0.0152;
335 0 -0.0006 -0.0000 0.0006;
336 0.0006 0 -0.0006 0;
337 0.0106 0.0106 0.0106 0.0106;
338 0 -1.4512 0 1.4512;
339 -1.4512 0 1.4512 0;
340 -0.3049 0.3049 -0.3049 0.3049;
341 0 -0.0236 0 0.0236;
342 0.0236 0 -0.0236 0;
343 0.2107 0.2107 0.2107 0.2107];
344 [nx, nu] = size(Bd);
345
346 % Constraints
347 u0 = 10.5916;
348 umin = [9.6; 9.6; 9.6; 9.6] - u0;
349 umax = [13; 13; 13; 13] - u0;
350 xmin = [-pi/6; -pi/6; -Inf; -Inf; -Inf; -1; -Inf(6,1)];
351 xmax = [ pi/6; pi/6; Inf; Inf; Inf; Inf; Inf(6,1)];
352
353 % Objective function
354 Q = diag([0 0 10 10 10 10 0 0 0 5 5 5]);
355 QN = Q;
356 R = 0.1*eye(4);
357
358 % Initial and reference states
359 x0 = zeros(12,1);
360 xr = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];
361
362 % Prediction horizon
363 N = 10;
364
365 % Define problem
366 u = sdpvar(repmat(nu,1,N), repmat(1,1,N));
367 x = sdpvar(repmat(nx,1,N+1), repmat(1,1,N+1));
368 constraints = [xmin <= x{1} <= xmax];
369 objective = 0;
370 for k = 1 : N
371 objective = objective + (x{k}-xr)'*Q*(x{k}-xr) + u{k}'*R*u{k};
372 constraints = [constraints, x{k+1} == Ad*x{k} + Bd*u{k}];
373 constraints = [constraints, umin <= u{k}<= umax, xmin <= x{k+1} <= xmax];
374 end
375 objective = objective + (x{N+1}-xr)'*QN*(x{N+1}-xr);
376 options = sdpsettings('solver', 'osqp');
377 controller = optimizer(constraints, objective, options, x{1}, [u{:}]);
378
379 % Simulate in closed loop
380 nsim = 15;
381 for i = 1 : nsim
382 U = controller{x0};
383 x0 = Ad*x0 + Bd*U(:,1);
384 end
385
386
387
388Julia
389------
390
391.. code:: julia
392
393 # Add packages - uncomment for first-time setup
394 # using Pkg; Pkg.add(["SparseArrays", "OSQP"])
395
396 using SparseArrays, OSQP
397
398 # Utility function
399 speye(N) = spdiagm(ones(N))
400
401 # Discrete time model of a quadcopter
402 Ad = [1 0 0 0 0 0 0.1 0 0 0 0 0;
403 0 1 0 0 0 0 0 0.1 0 0 0 0;
404 0 0 1 0 0 0 0 0 0.1 0 0 0;
405 0.0488 0 0 1 0 0 0.0016 0 0 0.0992 0 0;
406 0 -0.0488 0 0 1 0 0 -0.0016 0 0 0.0992 0;
407 0 0 0 0 0 1 0 0 0 0 0 0.0992;
408 0 0 0 0 0 0 1 0 0 0 0 0;
409 0 0 0 0 0 0 0 1 0 0 0 0;
410 0 0 0 0 0 0 0 0 1 0 0 0;
411 0.9734 0 0 0 0 0 0.0488 0 0 0.9846 0 0;
412 0 -0.9734 0 0 0 0 0 -0.0488 0 0 0.9846 0;
413 0 0 0 0 0 0 0 0 0 0 0 0.9846] |> sparse
414 Bd = [0 -0.0726 0 0.0726;
415 -0.0726 0 0.0726 0;
416 -0.0152 0.0152 -0.0152 0.0152;
417 0 -0.0006 -0.0000 0.0006;
418 0.0006 0 -0.0006 0;
419 0.0106 0.0106 0.0106 0.0106;
420 0 -1.4512 0 1.4512;
421 -1.4512 0 1.4512 0;
422 -0.3049 0.3049 -0.3049 0.3049;
423 0 -0.0236 0 0.0236;
424 0.0236 0 -0.0236 0;
425 0.2107 0.2107 0.2107 0.2107] |> sparse
426 (nx, nu) = size(Bd)
427
428 # Constraints
429 u0 = 10.5916
430 umin = [9.6, 9.6, 9.6, 9.6] .- u0
431 umax = [13, 13, 13, 13] .- u0
432 xmin = [[-pi/6, -pi/6, -Inf, -Inf, -Inf, -1]; -Inf .* ones(6)]
433 xmax = [[pi/6, pi/6, Inf, Inf, Inf, Inf]; Inf .* ones(6)]
434
435 # Objective function
436 Q = spdiagm([0, 0, 10, 10, 10, 10, 0, 0, 0, 5, 5, 5])
437 QN = Q
438 R = 0.1 * speye(nu)
439
440 # Initial and reference states
441 x0 = zeros(12)
442 xr = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
443
444 # Prediction horizon
445 N = 10
446
447 # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
448 # - quadratic objective
449 P = blockdiag(kron(speye(N), Q), QN, kron(speye(N), R))
450 # - linear objective
451 q = [repeat(-Q * xr, N); -QN * xr; zeros(N*nu)]
452 # - linear dynamics
453 Ax = kron(speye(N + 1), -speye(nx)) + kron(spdiagm(-1 => ones(N)), Ad)
454 Bu = kron([spzeros(1, N); speye(N)], Bd)
455 Aeq = [Ax Bu]
456 leq = [-x0; zeros(N * nx)]
457 ueq = leq
458 # - input and state constraints
459 Aineq = speye((N + 1) * nx + N * nu)
460 lineq = [repeat(xmin, N + 1); repeat(umin, N)]
461 uineq = [repeat(xmax, N + 1); repeat(umax, N)]
462 # - OSQP constraints
463 A, l, u = [Aeq; Aineq], [leq; lineq], [ueq; uineq]
464
465 # Create an OSQP model
466 m = OSQP.Model()
467
468 # Setup workspace
469 OSQP.setup!(m; P=P, q=q, A=A, l=l, u=u, warm_start=true)
470
471 # Simulate in closed loop
472 nsim = 15;
473 @time for _ in 1 : nsim
474 # Solve
475 res = OSQP.solve!(m)
476
477 # Check solver status
478 if res.info.status != :Solved
479 error("OSQP did not solve the problem!")
480 end
481
482 # Apply first control input to the plant
483 ctrl = res.x[(N+1)*nx+1:(N+1)*nx+nu]
484 global x0 = Ad * x0 + Bd * ctrl
485
486 # Update initial state
487 l[1:nx], u[1:nx] = -x0, -x0
488 OSQP.update!(m; l=l, u=u)
489 end