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
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
Thanks in advance.