Converting a problem from CVX syntax to YALMIP syntax

[Alessandro Maria Laspina]

Hello, 

I am opting to use YALMIP in favour of CVX because according to the people from the CVX forum YALMIP can generate a problem once, then substitute constants based on user input without having to regenerate the problem again and cause unnecessary additional time. As an example, for my problem, the runtime as reported by the ECOS solver is a mere .01 seconds, but the total matlab tic toc recorded time is over 8 seconds. 

My question is how I can use the optimizer object in YALMIP to set up my problem once and calculate it many times in a successive convex optimization scheme. I know that a cvx problem begins with cvx_begin and ends with cvx_end. Looking at the examples I am unsure how I would call variables. My problem is also time dependent, meaning that my variables change based on their previous history (consider it as a system with non conservative forces i.e. the dynamics are based on the path taken). 

Below you will find the CVX problem. I am using MATLAB 2020a. 

cvx_solver ECOS

cvx_begin 

    variables r(3,N) v(3,N) w(3,N) z(1,N) s(1,N) %declare CVX variables

    maximize( z(N) ) 

    subject to

        r(:,1) == r0; 

        v(:,1) == v0;

        z(1) == log(m_1);

        r(:,N) == rf;

        v(1:2,N) == vf(1:2);

        v(3,N) <= vf(3);

        v(3,N) >= -vf(3);

        w(1:2,N) == zeros(2,1);

        w(3,N) >= 0;

        z(N) >=log(m_2);

        for i=1:N-1

            Ts=dthis(i);

            r(:,i+1) == r(:,i)+Ts*v(:,i)+((Ts^2)/3)*(w(:,i) + g+(w(:,i+1) + g)/2);

            v(:,i+1) == v(:,i) + (Ts/2)*(w(:,i) + g + w(:,i+1) + g);

            z(i+1) == z(i) - (alpha*Ts/2)*(s(i) + s(i+1));

            

            if t(i)<=t1 || t(i)>=t2

            w(3,i) >= norm(w(1:2,i))/(tand(70));

            s(i) >= r1./m_1;

            s(i) <= r2./m_1;

            else

                w(:,i)==zeros(3,1);

            end

            

            s(i+1) >= s(i)-Ts*Tdmax*(m_1);

            s(i+1) <= s(i)+Ts*Tdmax*(m_1);

            

        end

        for i=1:N

            norm(w(:,i))<=s(i);

        end

cvx_end

[Alessandro Maria Laspina]

An explanation on the variables: the for loop propagates from time epoch 1 to time epoch N with a time step Ts, given by a vector that is fixed called dthis. Thus immediately below the subject to line, I call the values, by setting an equality constraint,  at the first array element 1 (thus the first time epoch) and the values at the final time, by setting an equality constraint at array element N. This is a powered descent guidance problem, i.e. imagine a a point mass moving in space starting at position and velocity r0 and v0 and needing to land at the origin (0,0,0) with a z velocity that is within a vf limit and x and y that are strictly 0. 

[Johan Löfberg]

Have you looked at the examples? Your model looks very basic and you should be able to update it with almost no effort

https://yalmip.github.io/tutorial/basics/

[Alessandro Maria Laspina]

I have not, thank you for the link. I jumped directly at the more complicated MPC examples. 

[Alessandro Maria Laspina]

Dear Dr. Löfberg, 

I was able to reformulate the problem without many difficulties but I get an error at the bolded section from matlab in the following code: 

r   = sdpvar(3,N);

v   = sdpvar(3,N);

z   = sdpvar(1,N);

w   = sdpvar(3,N);

s   = sdpvar(1,N);

constraints = [r(:,1) == r0, v(:,1) == v0, z(1) == log(m_wet), r(:,N) == rf, v(1:2,N) == vf(1:2), v(3,N) <= vf(3), v(3,N) >= -vf(3), w(1:2,N) == zeros(2,1), w(3,N) >= 0, z(N) >=log(m_dry)];

objective = 0;

for ii = 1:N-1

    Ts=dthis(ii);

    objective = z(N);

    constraints = [constraints, r(:,ii+1) == r(:,ii)+Ts*v(ii)+((Ts^2)/3)*(w(ii) + g+(w(:,ii+1) + g)/2),...

        v(ii+1) == v(ii) + (Ts/2)*(w(ii) + g + w(ii+1) + g), ...

        z(ii+1) == z(ii) - (alpha*Ts/2)*(s(ii) + s(ii+1))];

    if t(ii)<=t1 || t(ii)>=t2

        constraints=[constraints, w(3,ii)>=norm(w(1:2,ii))/tand(70), r1./m_wet<=s(ii)<=r2./m_wet];

    else

        constraints=[constraints, w(:,ii)==zeros(3,1)];

    end

    

    constraints=[constraints,s(ii)-Ts*Tdmax*m_wet<=s(ii+1)<=s(ii)+Ts*Tdmax*m_wet];

end

for ii=1:N

    constraints=[constraints,norm(w(:,ii)<=s(ii))];

end

% parameters_in = ();

% solutions_out = ((w),(r));

controller = optimizer(constraints, objective,sdpsettings('solver','ecos'));

The error returns: 

Error using norm

First argument must be single or double.

Error in YALMIP_SOCP1_test (line 66)

    constraints=[constraints,norm(w(:,ii)<=s(ii))];

In case I have not defined w correctly, it should be a 3x20 variables (number of states times number of time steps N). I am not sure why in the previous line in the if statement, the error does not arise as I also take the norm of w. 

[Alessandro Maria Laspina]

Please disregard the previous message, I did not place the brackets for the norm correctly. 

[Johan Löfberg]

fyi, matlab sclar extension applies so

w(1:2,N) == 0

works

[Alessandro Maria Laspina]

Thank you. A question regarding input parameters and solutions. 

Say I want to generate the model so that the input values are the starting values of r, v, , whilst obtaining the results for w, r and v. I have defined it like so: 

parameters_in = {r(3,1),v(3,1)};

solutions_out = {w, r, v};

controller = optimizer(constraints,objective,sdpsettings('solver','ecos'),parameters_in,solutions_out);

However, this results in the error:

Error using optimizer/subsref (line 208)

The dimension on a supplied parameter value does not match the definition.

when using the following command:

controller{r0,v0}

where I have defined the initial states as sdpvar variables themselves:

r0socp  = sdpvar(3,1);

v0socp  = sdpvar(3,1);

[Johan Löfberg]

a bit unclear here. You say the optimizer is defined in terms of two scalars r(3,1) and v(3,1). Then you talk about some other variables called r0socp and v0socp which both are 3x1 vectors

and we don't know the dimensions of the data r0 and v0

The message simply says that r0 and r(3,1), and/or v0 and v(3,1) don't have matching dimensions

[Alessandro Maria Laspina]

Let me post the updated code: 

r0=[5000;0;12000];

v0=[100;0;40];

r   = sdpvar(3,N);

v   = sdpvar(3,N);

z   = sdpvar(1,N);

w   = sdpvar(3,N);

s   = sdpvar(1,N);

r0socp  = sdpvar(3,1);

v0socp  = sdpvar(3,1);

% m_wet= sdpvar(1,1);

constraints = [r(:,1) == r0socp, v(:,1) == v0socp, z(1) == log(m_wet), r(:,N) == rf, v(1:2,N) == vf(1:2), v(3,N) <= vf(3), v(3,N) >= -vf(3), w(1:2,N) == 0, w(3,N) >= 0, z(N) >=log(m_dry)];

objective = 0;

for ii = 1:N-1

    Ts=dthis(ii);

    objective = z(N);

    constraints = [constraints, r(:,ii+1) == r(:,ii)+Ts*v(ii)+((Ts^2)/3)*(w(ii) + g+(w(:,ii+1) + g)/2),...

        v(ii+1) == v(ii) + (Ts/2)*(w(ii) + g + w(ii+1) + g), ...

        z(ii+1) == z(ii) - (alpha*Ts/2)*(s(ii) + s(ii+1))];

    if t(ii)<=t1 || t(ii)>=t2

        constraints=[constraints, w(3,ii)>=norm(w(1:2,ii))/tand(70), r1./m_wet<=s(ii)<=r2./m_wet];

    else

        constraints=[constraints, w(:,ii)==0];

    end

    

    constraints=[constraints,s(ii)-Ts*Tdmax*m_wet<=s(ii+1)<=s(ii)+Ts*Tdmax*m_wet];

end

for ii=1:N

    constraints=[constraints,norm(w(:,ii))<=s(ii)];

end

parameters_in = {r(3,1),v(3,1)};

solutions_out = {w, r, v};

controller = optimizer(constraints,objective,sdpsettings('solver','ecos'),parameters_in,solutions_out);

[solutions, diagnostics]=controller{r0,v0} 

[Johan Löfberg]

looks sensible but as it doesn't run I cannot really see

[Johan Löfberg]

the error is precisely the issue I gave in the previous answer

torsdag 18 mars 2021 kl. 18:36:57 UTC+1 skrev alexlas...@gmail.com:

[Alessandro Maria Laspina]

My mistake again, the input should be r0socp and v0socp, not r0 and v0. Strangely enough even when these where not variables it doesn't seem to work by specifying the first values of r and v. Is this intended? Am I better off anyways declaring the initial inputs as variables to reduce overhead? I know I read something about that in the examples...

[Johan Löfberg]

and code generally weird. you define a variable r0socp which you say r(:,1) has to match, but then you never use r0socp anywhere

to me it looks like you simply want an optimizer in r(:,1) and v(:,1)

[Johan Löfberg]

that would make no sense. r0 and v0 are presumably data for which the optimizer should be evaluated. you cannot send decision variables to the optimizer object

[Alessandro Maria Laspina]

r0 and v0 should be the starting positions and velocities of the trajectory. These are positions and velocities in xyz, thus they are 3xN matrices. I did get a solution this time, but the output returns all NaNs. This means I cannot specify r0's and v0's initial values as input variables? 

[Alessandro Maria Laspina]

In the MPC basic problem, the initial value of the x variable is defined as an input  Model predictive control - Basics - YALMIP .   Why can't I do that here? 

[Johan Löfberg]

well you are (modulo the bug that you defined the wrong stuff in the optimizer as you only sent one element from the vector instead of all three)

if you get nan it's probably because you defined an infeasible solution. you should set 'verbose' to 2 so you see all solver logs, and look at the diagnostics code

[Johan Löfberg]

*defined an infeasible problem* ...

[Johan Löfberg]

a bit strange to define the objective to be 0, and then redefine it to be z(N) and not only once but N-1 times...

torsdag 18 mars 2021 kl. 18:03:23 UTC+1 skrev alexlas...@gmail.com:

[Alessandro Maria Laspina]

If I change N to 20, I get a diagnostics result of 4, and it won't give me NaNs, increasing N will result in infeasibility. However, using N=200 with CVX and ecos its solveable and feasible. Could there be any syntax errors that are causing this infeasibility?

[Alessandro Maria Laspina]

yes it was, thank you for spotting that. Changed that as well so its only defined once. 

[Johan Löfberg]

post code that can be run

[Alessandro Maria Laspina]

Here it is: 

yalmip('clear')

clear all

m_dry=100;

m_wet=10000;

tf=200;

N=100;

r0=[5000;0;12000];

v0=[100;0;40];

earthRadius         = 6378145;

gravParam           = 3.986012e14;

initialMass         = m_dry+m_wet;

earthRotRate        = 7.29211585e-5;

SCALE.length       = 1;

SCALE.time         = 1;

SCALE.speed        = 1;

SCALE.acceleration = 1;

SCALE.mass         = 1;

SCALE.force        = 1;

SCALE.area         = 1;

SCALE.volume       = 1;

SCALE.density      = 1;

SCALE.pressure     = 1;

SCALE.gravparam    = 1;

Isp = 317/SCALE.time; % Specific impulse

g0 = 9.80665./SCALE.acceleration; % gravity 

g = [0 ; 0; -9.81]./SCALE.acceleration; % gravity vector

max_throttle = 1.0; % max throttle factor

min_throttle = 0.0; % min throttle factor

Tdmax =10000*(SCALE.time./SCALE.force); % Max throttle rate

T_max = 2 * 600000./SCALE.force; % Max throttle norm

phi = 0; % unused- angle between nozzle and rocket body

T_min=min_throttle*T_max; %Minimum throttle norm

rf=[0;0;0.001]./SCALE.length; % final position

vf=[0;0;6]./SCALE.speed; % final velocity

tf=tf./SCALE.time; % final time

t=linspace(0,tf,N); % time vector

m_dry=m_dry./SCALE.mass; %minimum mass of rocket (dry mass)

m_wet=m_wet./SCALE.mass; %maximum mass of rocket (dry mass+propellant)

r0=r0./SCALE.length; % initial position

v0=v0./SCALE.speed; % final position

dthis=[diff(t) (t(end)-t(end-1))]; % delta t vector

% N=length(t); % number of nodes in CVX problem

Re=earthRadius./SCALE.length; % unused- earth radius

omega=[2*pi/(24*3600);0;0].*SCALE.time; % unused- earth time

alpha = 1 / (Isp * g0 * cosd(phi)); % substitution variable 

r1 = min_throttle * T_max * cosd(phi); % max throttle

r2 = 1 * T_max * cosd(phi); % min throttle

t1=tf*.3;

t2=tf*.7;

% begin YALMIP problem

r   = sdpvar(3,N);

v   = sdpvar(3,N);

z   = sdpvar(1,N);

w   = sdpvar(3,N);

s   = sdpvar(1,N);

% r0socp  = sdpvar(3,1);

% v0socp  = sdpvar(3,1);

% m_wet= sdpvar(1,1);

constraints = [ z(1) == log(m_wet), r(:,N) == rf, v(1:2,N) == vf(1:2), v(3,N) <= vf(3), v(3,N) >= -vf(3), w(1:2,N) == 0, w(3,N) >= 0, z(N) >=log(m_dry)];

    objective = z(N);

for ii = 1:N-1

    Ts=dthis(ii);

    constraints = [constraints, r(:,ii+1) == r(:,ii)+Ts*v(ii)+((Ts^2)/3)*(w(ii) + g+(w(:,ii+1) + g)/2),...

        v(ii+1) == v(ii) + (Ts/2)*(w(ii) + g + w(ii+1) + g), ...

        z(ii+1) == z(ii) - (alpha*Ts/2)*(s(ii) + s(ii+1))];

    if t(ii)<=t1 || t(ii)>=t2

        constraints=[constraints, w(3,ii)>=norm(w(1:2,ii))/tand(70), r1./m_wet<=s(ii)<=r2./m_wet];

    else

        constraints=[constraints, w(:,ii)==0];

    end

    

    constraints=[constraints,s(ii)-Ts*Tdmax*m_wet<=s(ii+1)<=s(ii)+Ts*Tdmax*m_wet];

end

for ii=1:N

    constraints=[constraints,norm(w(:,ii))<=s(ii)];

end

parameters_in = {r(:,1),v(:,1)};

solutions_out = {w, r, v};

controller = optimizer(constraints,objective,sdpsettings('solver','ecos'),parameters_in,solutions_out);

[solutions,diagnostics]=controller{r0,v0}

[Johan Löfberg]

you are referencing w and v and as scalars when you really want v(:,ii) etc

[Alessandro Maria Laspina]

That fixed the problem. Works even faster than CVX. Thanks so much for the help.