Sequential Convex Programming using YALMIP

[kianoosh]

Hi everyone,

I am new to YALMIP. I am working on a problem that requires SCP. I decided to implement the robot arm example in Prof. Stephen Boyd's lecture and compare my results with his code. The aim in this example is to minimize torque and move the arm from theta = [0 -2.9]' to theta = [3 2.9]'.

If you are interested the lecture is found here: http://www.stanford.edu/class/ee364b/lectures/seq_slides.pdf

Prof. Stephen Boyd uses CVX to solve the problem but I use YALMIP with SDPT3 as the solver. Although everything seems to be the same, I get an answer that is different to his. I suspect either my decision variables or cost function are not right, but I don't know much about YALMIP. My code and Prof. Boyd's code are below. (Also don't feel intimidated by the amount of the text below, most of it is system dynamics which I pretty much copied from Prof. Boyd's code). Codes are also attached if you want to run them and see the results(You could need YALMIP and CVX in toolbox folder).

I really appreciate your help.

Prof. Boyd's code:

% robot.m

% Sequential convex optimization example.

% Heuristic for minimizing sum of torques for a robot arm trajectory.

% EE364b, Convex Optimization II, S. Boyd, Stanford University.

% Written by Jacob Mattingley, 2008-04.

% Set simulation parameters.

m1 = 1; m2 = 5; l1 = 1; l2 = 1;

N = 40; T = 10; h = T/N;

startpos = [0 -2.9]'; endpos = [3 2.9]';

taumax = 1.1;

alpha = 0.1; betasucc = 1.1; betafail = 0.5; rhoinit = 90*pi/180;

lambda = 2;

Kmax = 20;

% Set the inital trajectory to linearly interpolate the two points.

ot = [startpos [linspace(startpos(1), endpos(1), N); ...

 linspace(startpos(2), endpos(2), N)] endpos];

otdot = zeros(2, N+2);

otddot = zeros(2, N+2);

for t = 2:N+1

otdot(:,t) = (ot(:,t) - ot(:,t-1))/h;

otddot(:,t) = (ot(:,t+1) - 2*ot(:,t) + ot(:,t-1))/(h^2);

end

etas = zeros(Kmax, 1);

etahats = zeros(Kmax, 1);

Js = zeros(Kmax, 1);

phis = zeros(Kmax, 1);

rhos = zeros(Kmax, 1); rhos(1) = rhoinit;

phihats = zeros(Kmax, 1);

pdec = zeros(Kmax, 1);

adec = zeros(Kmax, 1);

cvx_quiet(true);

% Calculate initial value of phi.

eta = zeros(2, N);

for t = 2:N+1

% Dynamics equations.

t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = 0 - M*otddot(:,t) - W*otdot(:,t);

end

oldphi = 0 + lambda*sum(abs(eta(:)));

for i = 1:Kmax

cvx_begin

variable nt(2, N+2);

variable ntdot(2, N+2);

variable ntddot(2, N+2);

variable tau(2, N+2);

etahat = cvx(zeros(2, N));

% Initial and final conditions.

nt(:,1) == startpos; nt(:,2) == startpos;

nt(:,N+1) == endpos; nt(:,N+2) == endpos;

tau(:, 1) == 0; tau(:, N+2) == 0;

for t = 2:N+1

% Consistency of first derivatives.

ntdot(:,t) == (nt(:,t+1) - nt(:,t-1))/(2*h);

% Consistency of double derivatives.

ntddot(:,t) == (nt(:,t+1) - 2*nt(:,t) + nt(:,t-1))/(h^2);

% Dynamics equations.

t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

etahat(:,t-1) = tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t);

end

% Trust region constraints.

abs(nt(:) - ot(:)) <= rhos(i);

% Torque limit.

abs(tau(:)) <= taumax;

% Second arm can't fold back on first.

nt(2, :) <= pi;

nt(2, :) >= -pi;

minimize(h*sum_square(tau(:)) + lambda*sum(abs(etahat(:))));

cvx_end

% Calculate *actual* torque violations.

eta = zeros(2, N);

for t = 2:N+1

% Dynamics equations.

t1 = nt(1,t); t2 = nt(2,t); t1dot = ntdot(1,t); t2dot = ntdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t);

end

etahats(i) = sum(abs(etahat(:))); etas(i) = sum(abs(eta(:)));

Js(i) = h*sum_square(tau(:));

phihats(i) = Js(i) + lambda*etahats(i); phis(i) = Js(i) + lambda*etas(i);

deltahat = oldphi - phihats(i); delta = oldphi - phis(i);

oldphi = phis(i);

pdec(i) = deltahat; adec(i) = delta;

if delta <= alpha*deltahat

if i ~= Kmax

rhos(i+1) = betafail*rhos(i);

end

% Undo recording of the last step.

if i ~= 1

Js(i) = Js(i-1);

phis(i) = phis(i-1);

etas(i) = etas(i-1);

etahats(i) = etahats(i-1);

oldphi = phis(i);

end

else

if i ~= Kmax

rhos(i+1) = betasucc*rhos(i);

end

% Propagate the system.

ot = nt; otdot = ntdot;

end

disp(cvx_status);

disp('       iter  hatphys  realphys    phihat       phi  deltahat     delta       rho')

disp([i etahats(i) etas(i) phihats(i) phis(i) deltahat delta rhos(i)])

end

Now my code:

m1 = 1; m2 = 5; l1 = 1; l2 = 1;

N = 40; T = 10; h = T/N;

startpos = [0 -2.9]'; endpos = [3 2.9]';

taumax = 1.1;

alpha = 0.1; betasucc = 1.1; betafail = 0.5; rhoinit = 90*pi/180;

lambda = 0.5;

Kmax = 10;

ot = [startpos [linspace(startpos(1), endpos(1), N); ...

 linspace(startpos(2), endpos(2), N)] endpos];

otdot = zeros(2, N+2);

otddot = zeros(2, N+2);

for t = 2:N+1

    otdot(:,t) = (ot(:,t) - ot(:, t-1))/h;

    otddot(:,t) = (ot(:,t+1) - 2*ot(:,t) + ot(:, t-1))/(h^2);

end

etas = zeros(Kmax, 1); %initial dyanamic violation

etahats = zeros(Kmax, 1); %dynamic violation

Js = zeros(Kmax, 1);

phis = zeros(Kmax, 1);

rhos = zeros(Kmax, 1); rhos(1) = rhoinit;

phihats = zeros(Kmax, 1);

pdec = zeros(Kmax, 1);

adec = zeros(Kmax, 1);

% Calculate initial value of phi.

eta = zeros(2, N);

for t = 2:N+1

% Dynamics equations. ->linearized model

t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = double(0 - M*otddot(:,t) - W*otdot(:,t));

end

oldphi = 0 + lambda*sum(abs(eta(:)));

for i=1:Kmax

    obj = 0;

    constraints = [];

    

    nt = sdpvar(2,N+2);

    ntdot = sdpvar(2, N+2);

    ntddot = sdpvar(2, N+2);

    tau = sdpvar(2, N+2);

    

    %disp('etahat set to zeros')

    etahat = sdpvar(2, N);

    

    

    assign(nt, zeros(2, N+2));

    assign(ntdot, zeros(2, N+2));

    assign(ntddot, zeros(2, N+2));

    assign(tau, zeros(2, N+2));

    assign(etahat, zeros(2, N));

    

    constraints = [nt(:,1) == startpos, nt(:,2) == startpos ...

        nt(:, N+1) == endpos, nt(:, N+2) == endpos, tau(:,1) == 0 ...

        tau(:,1) == 0, tau(:, N+2) == 0];

    

    for t  = 2: N+1

        %disp('Im in this damn  loop but im not doing my damn job')

        constraints = [constraints, ntdot(:,t) == (nt(:, t+1) - nt(:, t-1))/(2*h)];

        constraints = [constraints, ntddot(:,t) == (nt(:,t) - 2*nt(:, t) + nt(:, t-1))/(h^2)];

        

        %Dynamic Equations

        t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

        M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

             m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

        W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

             m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

         

        etahat(:,t-1) =(tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t));

    end

    constraints = [constraints, abs(tau(:)) <= taumax, ...

        abs(nt(:) - ot(:))<= rhos(i), nt(2,:)>= -pi, nt(2, :)<=pi];

    

     s = 0;

     for j = 1:N+2

         s = s+ tau(1,j)^2 + tau(2,j)^2;

     end

    %disp('etahat = ')

    %disp(etahat)

    obj = h*s + lambda*sum(abs(etahat(:)));%h*sum_square(tau(:)) +

    disp('solvesdp called')

    options = sdpsettings('solver', 'sdpt3','usex0',1);

    diag = solvesdp(constraints, obj, options);

    

    eta = sdpvar(2, N);

    assign(eta, zeros(2, N));

    %eta = zeros(2, N);

    for t = 2: N+1

        % Dynamics equations.

t1 = nt(1,t); t2 = nt(2,t); t1dot = ntdot(1,t); t2dot = ntdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t);

    end  

    

etahats(i) = double(sum(abs(etahat(:)))); etas(i) = double(sum(abs(eta(:))));

Js(i) = h*s;%sum_square(tau(:)); %torque squared cost function

phihats(i) = Js(i) + etahats(i);

    

    phis(i) = Js(i) + lambda*etas(i);

deltahat = oldphi - phihats(i); delta = oldphi - phis(i);

oldphi = phis(i);

pdec(i) = deltahat; adec(i) = delta;

if delta <= alpha*deltahat

if i ~= Kmax

rhos(i+1) = betafail*rhos(i); %update rho

end

% Undo recording of the last step.

if i ~= 1

Js(i) = Js(i-1);

phis(i) = phis(i-1);

etas(i) = etas(i-1);

etahats(i) = etahats(i-1);

oldphi = phis(i);

end

else

if i ~= Kmax

rhos(i+1) = betasucc*rhos(i);

end

% Propagate the system.

ot = nt; otdot = ntdot;

    end

    

    disp('       iter  hatphys  realphys    phihat       phi  deltahat     delta       rho')

disp([i etahats(i) etas(i) phihats(i) phis(i) deltahat delta rhos(i)])

    disp('tau = ')

    disp(double(tau));

    

end

[Johan Löfberg]

You are not solving the same problem. 

To begin with, with YALMIP you use

 constraints = [constraints, ntddot(:,t) == (nt(:,t) - 2*nt(:, t) + nt(:, t-1))/(h^2)];

while with CVX you have

ntddot(:,t) == (nt(:,t+1) - 2*nt(:,t) + nt(:,t-1))/(h^2);

There might be more. Start over again, and copy the code directly. The changes you have to do to go from CVX to YALMIP are minimal.

In addition to that, there are some issues that I react on

1. SDPT3 does not exploit initial guesses, so no reason to use assign and usex0

2. This is horribly inefficient

    eta = sdpvar(2, N);

    assign(eta, zeros(2, N));

    %eta = zeros(2, N);

    for t = 2: N+1

        % Dynamics equations.

t1 = nt(1,t); t2 = nt(2,t); t1dot = ntdot(1,t); t2dot = ntdot(2,t);

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t);

    end  

etahats(i) = double(sum(abs(etahat(:)))); etago s(i) = double(sum(abs(eta(:))));

    

This will force YALMIP to work with polynomial trigonometric symbolic expressions since you cast as double as the last step. Much better to start by casting as double, i.e., something like this

    eta = zeros(2, N);

    for t = 2: N+1

        % Dynamics equations.

t1 = double(nt(1,t)); etc...                

M = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

W = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

eta(:,t-1) = double(tau(:,t) - M*ntddot(:,t) - W*ntdot(:,t));

    end  

etahats(i) = double(sum(abs(etahat(:)))); etas(i) = (sum(abs(eta(:))));

    

3. Vectorize!

s = tau(:)'*tau

or

s = sum(tau(:).^2);

4. If you define sdpvars inside a loop, use a yalmip('clear'), or better, why not define them outside the loop

That's a start.

Your plan of attack

1. Define all constants in a separate file so you can be sure both files work with the same data

2. Copy the CVX code, and update to YALMIP code

3. Make the small changes recommended above

[Johan Löfberg]

and I don't see the reason to use SDPT3 here. Every problem is a QP so for efficiency you should use an efficient QP solver.

[Johan Löfberg]

As an expanded example, this is a perfect case where you should learn about parameterized optimizer objects in YALMIP.

http://users.isy.liu.se/johanl/yalmip/pmwiki.php?n=Blog.Beta-version-of-a-more-general-optimizer

By using this approach, the problem can be simulated much faster. With gurobi as the underlying solver, with Kmax=25, the problem can be simulated in less than a second. With SDPT3 it takes significantly more, since it is a poor choice of solver here, but at least modelling overhead is eliminated in the simulation (remaining bugs in your YALMIP model is left here, so make sure your model is correct to begin with)

clear all
setupdata

% Setup a parameterized optimizer object
yalmip('clear')

nt = sdpvar(2,N+2);
ntdot = sdpvar(2, N+2);
ntddot = sdpvar(2, N+2);
tau = sdpvar(2, N+2);

etahat = sdpvar(2, N);
% These will be suitable parameters since they change in every iteration
rhobound = sdpvar(1);
M = sdpvar(repmat(2,1,N),repmat(2,1,N));
W = sdpvar(repmat(2,1,N),repmat(2,1,N));

constraints = [nt(:,1) == startpos, nt(:,2) == startpos ...
    nt(:, N+1) == endpos, nt(:, N+2) == endpos, tau(:,1) == 0 ...
    tau(:,1) == 0, tau(:, N+2) == 0];

etahat = [];

for t  = 2: N+1

    constraints = [constraints, ntdot(:,t) == (nt(:, t+1) - nt(:, t-1))/(2*h)];

    constraints = [constraints, ntddot(:,t) == (nt(:,t+1) - 2*nt(:, t) + nt(:, t-1))/(h^2)];
    etahat = [etahat tau(:,t)-M{t-1}*ntddot(:,t)-W{t-1}*ntdot(:,t)];

end
constraints = [constraints, abs(tau(:)) <= taumax,

               abs(nt(:) - ot(:))<= rhobound,

               nt(2,:)>= -pi, nt(2, :)<=pi];

s = tau(:)'*tau(:);
obj = h*s + lambda*norm(etahat(:),1);

ops = sdpsettings('solver','+gurobi');
P = optimizer(constraints,obj,ops,{M{:},W{:},rhobound},{nt,ntdot});

tic

for i = 1:Kmax
   

    for t = 2:N+1  

        t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

        Mdata{t-1} = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));
            m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];
        Wdata{t-1} = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;
            m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];
    end
   
    solution = P{{Mdata{:},Wdata{:},rhos(i)}};
    rhos(i+1) = betasucc*rhos(i);
    ot = solution{1};
    otdot = solution{2};
end
toc

[kianoosh]

Thank you very much Associate Prof. Löfberg. I really appreciate the time you took to have a look at my question.

I had another question now. Using the method you outlined above based on the optimizer object, does it mean that I don't have to do anything else like 

   

 phihats(i) = Js(i) + etahats(i);
   
    phis(i) = Js(i) + lambda*etas(i);


 deltahat = oldphi - phihats(i); delta = oldphi - phis(i);
 oldphi = phis(i);


 pdec(i) = deltahat; adec(i) = delta;


 if delta <= alpha*deltahat
 if i ~= Kmax
 rhos(i+1) = betafail*rhos(i); %update rho
 end


 % Undo recording of the last step.
 if i ~= 1
 Js(i) = Js(i-1);
 phis(i) = phis(i-1);
 etas(i) = etas(i-1);
 etahats(i) = etahats(i-1);
 oldphi = phis(i);
 end
 else
 if i ~= Kmax
 rhos(i+1) = betasucc*rhos(i);
 end
 % Propagate the system.
 ot = nt; otdot = ntdot;
    end



   

as Prof. Boyd's algorithm suggests? I am asking because I really do not know about the inner workings of the functions here but when I am comparing the result I obtain from this code with the result that Prof. Boyd's code produces they are different. This is despite the fact that you corrected the typo in the model. I made some improvements in the code you posted so that I can see how input torque is. Now it looks like this (Edits are highlighted with line of stars):

s = sum(tau(:).^2);

obj = h*s + lambda*norm(etahat(:),1);

%ops = sdpsettings('solver','+gurobi');

P = optimizer(constraints,obj,[],{M{:},W{:},rhobound},{nt,ntdot, tau}); %**********

tic

for i = 1:Kmax

    

    for t = 2:N+1  

        t1 = ot(1,t); t2 = ot(2,t); t1dot = otdot(1,t); t2dot = otdot(2,t);

        M{t-1} = [(m1 + m2)*l1^2 m2*l1*l2*(sin(t1)*sin(t2) + cos(t1)*cos(t2));

            m2*l1*l2*(sin(t1)*sin(t2)+cos(t1)*cos(t2)) m2*l2^2];

        W{t-1} = [0 m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t2dot;

            m2*l1*l2*(sin(t1)*cos(t2) - cos(t1)*sin(t2))*t1dot 0;];

    end

    

    solution = P{{M{:},W{:},rhos(i)}};

    rhos(i+1) = betasucc*rhos(i);

    ot = solution{1};

    otdot = solution{2};

    tau = solution{3}; %***********

end

toc

Maybe I am making a silly mistake that I can't see, because this problem is really simple to me or maybe I am just unfamiliar with how YALMIP works.

Anyhow I appreciate your insight and patience.

[Johan Löfberg]

I only showed you the parts relevant to setting up and solving the optimization problems. I hope you realize the rest only is used to save results etc.

I showed you *one* obvious bug in your code. There might be many more, hence your solutions would differ. Start over from scratch, use a joint file to setup constants, and copy relevant code and convert it to YALMIP. And don't use the optimizer approach until you know you have the correct model.