# How to perform a cart-pole swing up task using MPC?

I originally thought this was a really simple problem. However, to my surprise I did not find any resource talking about it, the only thing I found was that using Matlab's nonlinear MPC toolbox you could do it. I thus tried to solve it myself. I tried to use a trapezoidal direct collocation at each step with a quadratic cost on both the state error and control, and I got rid of the terminal state constraint. I then noticed that it would not be able to swing it up. Also I noticed that without the terminal state constraint, even the planned state trajectory will not swing the pole to the top.

This is to me a surprising result. Thus, I want to ask if this is really the case or would this be due to a bug in my code? And how would people usually solve this problem using MPC?

My implementation is attached below. I used Matlab to program it since I wanted to used fmincon, instead of spicy.minimize.

%% Cartpole Parameters
m1=1;
m2=0.3;
l=0.5;
g=9.81;

% get next state
state = [0; 0; 0; 0];
tspan = [0, 0.02];
states = [state];

%% Optimization Configurations
N = 100;
dt = 0.02;
T = 2.0;

x0 = [];
for i = 0:N
xtmp = i*dt/T*([1.0; pi; 0.0; 0.0] - state) + state;
x0 = [x0; xtmp];
end

init_guess = [x0; zeros(N+1, 1)];

%% MPC
for i = 1:100
[control, planned_states] = get_control(state, init_guess);
next_state = planned_states(5:8)';
states = [states, next_state'];
state = next_state';
disp(i)
init_guess = [next_state'; planned_states(9:end); 1.0; pi; 0.0; 0.0; control(2:end); 0.0];
end


the get_control function is defined as

function [control, states] = get_control(x_init, x0)
%% Optimization Configurations
N = 100;

%% Set up objective
q1 = ones(1, 4*N+4);
q1(1:4) = 0.5;
q1(end-3:end) = 100.5;
Q1 = diag(q1);

q2 = ones(1, N+1);
q2(1) = 0.5;
q2(end) = 0.5;
Q2 = 0.01*diag(q2);

x_target = [1.0; pi; 0.0; 0.0];
x_targets = repmat(x_target, N+1, 1);
u_targets = zeros(N+1, 1);

targets = [x_targets; u_targets];

Q = blkdiag(Q1, Q2);

fun = @(x)(x-targets).'*Q*(x-targets);

%% Set up lower and upper bound
bound_piece = [10; 10; 10; 10];
state_bnd = repmat(bound_piece, N+1, 1);
control_bnd = 20*ones(N+1, 1);

ub = [state_bnd; control_bnd];
lb = -ub;

%% Set initial and terminal condition
I = eye(4);
zeros1 = zeros(4, 4*N+N+1);
left1 = [I, zeros1];
Aeq = left1;
beq = x_init;

%% Collocation Constraints
nonlcon = @collocateConstraint;

%% fmincon
A = [];
b = [];

options = optimoptions(@fmincon,'Algorithm','sqp','MaxIterations',100);
x = fmincon(fun, x0, A, b, Aeq, beq, lb, ub, nonlcon, options);

control = x(405:end);
end


and the nonlinear constraints collocateConstraint are defined as

function [c, ceq] = collocateConstraint(x)
dt = 0.02;
N = 100;

ceq = [];

for i = 0:N-1
x_i = x((i*4+1):(i+1)*4);
x_ip1 = x((i+1)*4+1:((i+2)*4));

u_i = x(4*N+4+i+1);
u_ip1 = x(4*N+4+i+2);

m1=1;
m2=0.3;
l=0.5;
g=9.81;

denominatori = m1 + m2*(sin(x_i(2))^2);

Ai = [x_i(3);
x_i(4);
(l*m2*sin(x_i(2))*(x_i(4)^2) + m2*g*cos(x_i(2))*sin(x_i(2))) / denominatori;
-(l*m2*cos(x_i(2))*sin(x_i(2))*(x_i(4)^2) + (m1 + m2)*g*sin(x_i(2))) / (l*denominatori)
];

Bi = [0;
0;
1 / denominatori;
-cos(x_i(2))/(l*denominatori)
];

denominatorip1 = m1 + m2*(sin(x_ip1(2))^2);

Aip1 = [x_ip1(3);
x_ip1(4);
(l*m2*sin(x_ip1(2))*(x_ip1(4)^2) + m2*g*cos(x_ip1(2))*sin(x_ip1(2))) / denominatorip1;
-(l*m2*cos(x_ip1(2))*sin(x_ip1(2))*(x_ip1(4)^2) + (m1 + m2)*g*sin(x_ip1(2))) / (l*denominatorip1)
];

Bip1 = [0;
0;
1 / denominatorip1;
-cos(x_ip1(2))/(l*denominatorip1)
];

ceqi = x_ip1 - x_i - (dt/2)*(Bi*u_i + Bip1*u_ip1) - (dt/2)*(Ai + Aip1);

ceq = [ceq; ceqi];
end

c = [];
end