1
$\begingroup$

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

Thanks in advance.

$\endgroup$
1
  • 1
    $\begingroup$ I think I found the issue, all you need to do is to run MPC longer, and it will balance at the top. I guess I was blinded by the ability to reach the top in 2sec if I added the terminal constraint, and was expecting that without the terminal constraint it will also be able to reach the top in 2sec. $\endgroup$
    – RoboNoob
    Apr 25 at 18:36

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.