MPC with CasADi

% CasADi -- A symbolic framework for dynamic optimization.

% Copyright (C) 2010-2014 Joel Andersson, Joris Gillis, Moritz Diehl,

% K.U. Leuven. All rights reserved.

% Copyright (C) 2011-2014 Greg Horn

% An implementation of direct multiple shooting

% Joel Andersson, 2016

% Applied to a mass spring damper. Siampis/Catto 2021

%

% The system equation is:

% m*z_dot_dot + c*z_dot + k*z = F

%

% Rearranged as:

% z_dot_dot + c/m*z_dot + k/m*z = F

%

% x1 = z

% x2 = z_dot

%

% x1_dot = z_dot = x2

% x2_dot = z_dot_dot = 1/m*f - k/m*x1 - c/m*x2

%

% therefore:

%

% x1_dot = x2

% x2_dot = 1/m*f - k/m*x1 - c/m*x2

%

% Matrix: X_dot = Ax + Bu

%

% State Space:

% | x1_dot | = | 0 1 | | x1 | + | 0 |*F

% | x2_dot | = | -k/m -c/m | | x2 | + | 1/m |

%

% y = C*X

% where C*X is the output we want

%

% For displacement output:

%

% y = | 1 0 | * | x1 |

% | x2 |

%

% For velocity output:

%

% y = | 0 1 | * | x1 |

% | x2 |

clear; clc; close all

import casadi.*

% ============================

T = 10; % Time horizon in seconds

N = 20; % number of control intervals

m = 280; % 1/4 car mass (simplistic) in kg

k = 50000; % Spring rate in N/m

c = 3000; % Damping coefficient Ns/m

% Declare model variables

% Mass-spring-damper: z_dot_dot + c/m*z_dot + k/m*z = u/m

x1 = SX.sym('x1');

x2 = SX.sym('x2');

x = [x1; x2];

u = SX.sym('u');

% Model equations

xdot = [x2; 1/m*u - k/m*x1 - c/m*x2];

% Objective term

x1_ref = 0.1; % x1 reference value

x2_ref = 0; % x2 reference value

qx1 = 100; % relative weight on x1 error

qx2 = 1; % relative weight on x2 error

L = qx1*(x1-x1_ref)^2 + qx2*(x2-x2_ref)^2;

% Continuous time dynamics

f = Function('f', {x, u}, {xdot, L});

% ============================

% Fixed step Runge-Kutta 4 integrator

M = 4; % RK4 steps per interval

DT = T/N/M;

% f = Function('f', {x, u}, {xdot, L});

X0 = MX.sym('X0', 2);

U = MX.sym('U');

X = X0;

Q = 0;

for j=1:M

[k1, k1_q] = f(X, U);

[k2, k2_q] = f(X + DT/2 * k1, U);

[k3, k3_q] = f(X + DT/2 * k2, U);

[k4, k4_q] = f(X + DT * k3, U);

X=X+DT/6*(k1 +2*k2 +2*k3 +k4);

Q = Q + DT/6*(k1_q + 2*k2_q + 2*k3_q + k4_q);

end

F = Function('F', {X0, U}, {X, Q}, {'x0','p'}, {'xf', 'qf'});

% ============================

% Start with an empty NLP

w={};

w0 = [];

lbw = [];

ubw = [];

J = 0;

g={};

lbg = [];

ubg = [];

% "Lift" initial conditions

x1_init = 0.2; % initial condition for x1

x2_init = -0.3; % initial condition for x2

Xk = MX.sym('X0', 2);

w = {w{:}, Xk};

lbw = [lbw; x1_init; x2_init];

ubw = [ubw; x1_init; x2_init];

w0 = [w0; x1_init; x2_init]; % initial guess for solution

% Formulate the NLP

u_lowerbound = -10000;

u_upperbound = 10000;

x1_lowerbound = -inf;

x1_upperbound = inf;

x2_lowerbound = -inf;

x2_upperbound = inf;

for k=0:N-1

% New NLP variable for the control

Uk = MX.sym(['U_' num2str(k)]); % Input variable for stage k

w = {w{:}, Uk};

lbw = [lbw; u_lowerbound]; % PROBLEM DEPENDENT in terms of the size only

ubw = [ubw; u_upperbound]; % PROBLEM DEPENDENT in terms of the size only

w0 = [w0; 0]; % initial guess for solution

% Integrate till the end of the interval (using what we have

% constructed with the RK4 from above)

Fk = F('x0', Xk, 'p', Uk);

Xk_end = Fk.xf;

J=J+Fk.qf;

% New NLP variable for state at end of interval

Xk = MX.sym(['X_' num2str(k+1)], 2);

w = [w, {Xk}];

lbw = [lbw; x1_lowerbound; x2_lowerbound]; % PROBLEM DEPENDENT in terms of the size only

ubw = [ubw; x1_upperbound; x2_upperbound]; % PROBLEM DEPENDENT in terms of the size only

w0 = [w0; 0; 0]; % initial guess for solution

% Add equality constraint

g = [g, {Xk_end-Xk}]; % multiple shooting

lbg = [lbg; 0; 0]; % PROBLEM DEPENDENT in terms of the size only

ubg = [ubg; 0; 0]; % PROBLEM DEPENDENT in terms of the size only

end

% Create an NLP solver

prob = struct('f', J, 'x', vertcat(w{:}), 'g', vertcat(g{:}));

solver = nlpsol('solver', 'ipopt', prob);

% Solve the NLP

sol = solver('x0', w0, 'lbx', lbw, 'ubx', ubw,...

'lbg', lbg, 'ubg', ubg);

w_opt = full(sol.x);

% ============================

% Plot the solution

x1_opt = w_opt(1:3:end);

x2_opt = w_opt(2:3:end);

u_opt = w_opt(3:3:end);

tgrid = linspace(0, T, N+1);

% clf;

% hold on

% plot(tgrid, x1_opt, '--')

% plot(tgrid, x2_opt, '-')

% stairs(tgrid, [u_opt; nan], '-.')

% xlabel('t')

% legend('x1','x2','u')

figure

subplot(2,2,1)

plot(tgrid, x1_opt, ...

[0, T], [x1_lowerbound x1_lowerbound], 'r--', ...

[0, T], [x1_upperbound x1_upperbound], 'r--')

xlabel('t')

ylabel('x1')

subplot(2,2,2)

plot(tgrid, x2_opt, ...

[0, T], [x2_lowerbound x2_lowerbound], 'r--', ...

[0, T], [x2_upperbound x2_upperbound], 'r--')

xlabel('t')

ylabel('x2')

subplot(2,2,3)

stairs(tgrid, [u_opt; nan], '-.')

hold on

plot([0, T], [u_lowerbound u_lowerbound], 'r--', ...

[0, T], [u_upperbound u_upperbound], 'r--')

xlabel('t')

ylabel('u')