Issue adding a constraint with a norm

[Guillermo]

Dear Professor Löfberg,

First of all, thank your for your time. I am a MSc student doing his master thesis. I am using YALMIP+SeDuMi as a controller for a 2-D vehicle with MPC. My code was working fine until I tried to add a constraint in the angle between the thrust vector and the velocity vector of my vehicle. Attached there is a simplification of the code I have. The error I get is related with the norm I am using to obtain the components of the unit vectors with the same directions as the velocity and thrust (lines 57-64). YALMIP says:

Error using optimizer (line 193)

Failed exporting the model: Model creation failed (Only 1- and inf-norms can be used in a nonconvex fashion)

Error in test_for_forum (line 77)

controller = optimizer(constraints, objective, ops, parameters_in, solutions_out);

However, I do not see where the nonconvexity source is. Any comment regarding this issue will be more than helpful.

Thank you again,

Guillermo

[Johan Löfberg]

2-norm is an operator which is modelled using SOCP-cones in YALMIP, and thus requires convex model.

You have a very nasty nonlinear model involving this norm. In fact, this whole block

    tx_unitVector = u{1,k}(1,1) / u{1,k}(3,1);

    ty_unitVector = u{1,k}(2,1) / u{1,k}(3,1);

    V_mag = norm([st{1,k}(3,1), st{1,k}(4,1)]);

    vx_unitVector = st{1,k}(3,1) / V_mag;

    vy_unitVector = st{1,k}(4,1) / V_mag;

    

    constraints = [constraints, tan_ten_deg >= (vy_unitVector - ty_unitVector) / (vx_unitVector - tx_unitVector)]; % The angle between thrust and velocity has to be less than 10 degrees    

    constraints = [constraints, tan_ten_deg >= -(vy_unitVector - ty_unitVector) / (vx_unitVector - tx_unitVector)];

is really ugly. You will have to start by getting rid of the norm operator (write using squares and sqrtm instead), and to get something which actually will work you will have to reformulate it to get rid of singularity-inducing divisions etc, i.e. try to write using, preferable, quadratic/bilinear equalities etc i.e.  tan_ten_deg*(vx_unitVector - tx_unitVector) >= (vy_unitVector - ty_unitVector) and adding new variables and constraints to write tx_unitVector*u{1,k}(3,1) == u{1,k}(1,1) etc

SeduMi is completely out of the picture. You have a nonconvex nonlinear model

Better yet, you might want to try to reformulation your model, to get rid of these things altogether (variables in another coordinate system or similar trick)

[Johan Löfberg]

and never use optimizer construct before you actually now you can solve the problem as expected. optimizer assumes you know eactly what you are doing, and that you know that the problem can be solved by the solver you have specified. This can blow up hard (luckily here a layer caught the norm issue in the model)

[Guillermo]

Thank you so much for your comments and advice, it was really helpful. I have been working on what you said but there are some issues that I am not able to solve. I changed my approach to calculate the velocity angle from the state vector, being it now [x, y, vx, vy, vMagnitude, cosAlpha, sinAlpha], and adding the following constraints:

    % ---- New part

    constraints = [constraints, st{1,k}(5,1) - sqrtm([st{1,k}(3,1)^2, st{1,k}(4,1)^2]) == 0];   % V - ||[Vx, Vy]|| == 0

    constraints = [constraints, st{1,k}(5,1) * st{1,k}(6,1) - st{1,k}(3,1)  == 0];                        % V * cosAlpha - Vx == 0

    constraints = [constraints, st{1,k}(5,1) * st{1,k}(7,1) - st{1,k}(4,1)  == 0];                        % V * sinAlpha - Vy == 0

    constraints = [constraints, st{1,k}(6,1)^2 + st{1,k}(7,1)^2 - 1   == 0];                               % cosAlpha^2 + sinAlpha^2 - 1 == 0

    % ---- New part

This was a first step to later on calculate the angle of the thrust and then limit the difference between them, but I have an issue with the first constraint. The optimizer says: 

Error using optimizer (line 313)

Parameters are currently only allowed to enter function such as exp, sin etc as exp(a), sin(b) etc.

Error in test_for_forum2 (line 88)

controller = optimizer(constraints, objective, ops, parameters_in, solutions_out);

but I am not using any of those functions. Is it because of the squares? It does not happen when I remove the first of the new constraints (which could be actually removed as I believe the other 3 would be enough to obtain the sinAlpha, the cosAlpha and the vMagnitude), but the problem becomes infeasible even for really simple initial conditions. I attached the matlab script in case you want to have a look at it.

Thank you again,

Guillermo

[Johan Löfberg]

Your comment is not consistent with code

constraints = [constraints,
st{1,k}(5,1) - sqrtm([st{1,k}(3,1)^2, st{1,k}(4,1)^2]) == 0];   % V - ||[Vx, Vy]|| == 0

your comment says

constraints = [constraints, st{1,k}(5,1) - sqrtm(st{1,k}(3,1)^2 + st{1,k}(4,1)^2) == 0];

But as it says, you cannot have parameters inside nonlinear callback operators (i.e some st-variable is a parameter). However, you can get rid of that by simply squaring the expression (which is better anyway as you get rid of the non-smoothness of square root)

constraints = [constraints, st{1,k}(5,1)^2 == st{1,k}(3,1)^2 + st{1,k}(4,1)^2];

[Guillermo]

That's true, I was applying sqrtm wrongly in the comment. Sorry for that.

Thank you for your suggesting about squaring the expression about the magnitude of the velocity. It helped a lot. Now I am struggling with the solutions that the different solvers give for the same problem. I simplify it so I am not applying any new constraint, but only calculating the square of the magnitude of the velocity. The st vector is [x, y, vx, vy, v2] where v2 is a variable representing v^2 (MATLAB code attached). The constraints would look like this (note that st{1,k}(1,5) is not squared anymore):

for k = 1:Np

    % Cost function

    objective = objective + u{1,k}(3,1);

    % If we assume the control is constant on every time step we can integrate easily this model

    constraints = [constraints, st{1,k+1}(1,1) == u{1,k}(1,1)*dt^2 + st{1,k}(3,1)*dt + st{1,k}(1,1)];

    constraints = [constraints, st{1,k+1}(2,1) == (u{1,k}(2,1)-env)*dt^2 + st{1,k}(4,1)*dt + st{1,k}(2,1)];

    constraints = [constraints, st{1,k+1}(3,1) == u{1,k}(1,1)*dt + st{1,k}(3,1)];

    constraints = [constraints, st{1,k+1}(4,1) == (u{1,k}(2,1)-env)*dt + st{1,k}(4,1)];

    constraints = [constraints, st{1,k}(5,1)   == st{1,k}(3,1)^2 + st{1,k}(4,1)^2]; % V^2 == Vx^2 + Vy^2

    % Limits on the control

    constraints = [constraints, norm([u{1,k}(1,1),u{1,k}(2,1)]) <= u{1,k}(3,1)];

    constraints = [constraints, 0 <= st{1,k}(2,1)]; % The Y must be always positive

    constraints = [constraints, 0 <= u{1,k}(2,1)]; % The thrust in Y must be positive semidefinite.

%     constraints = [constraints, 0 <= st{1,k}(5,1) <= 100]; 

end

% Terminal time constraints

constraints = [constraints, st{1,Np+1}(1,1) == xf];

constraints = [constraints, st{1,Np+1}(2,1) == yf];

constraints = [constraints, st{1,Np+1}(3,1) == Vxf];

constraints = [constraints, st{1,Np+1}(4,1) == Vyf];

constraints = [constraints, st{1,Np+1}(5,1) == Vxf^2 + Vyf^2];

The results with SeDuMi and MOSEK are practically the same (the output marks it is "Successfully solved"):

As you can see, the last row, which is the v2, is not calculated, but I do not understand why. The constraint  st{1,k}(5,1)   == st{1,k}(3,1)^2 + st{1,k}(4,1)^2; % V^2 == Vx^2 + Vy^2  is basically a cone and Vx and Vy are defined by the constraints above.

However, if I run the code with fmincon the correct values are shown  in the 5th row (the output marks it is "Maximum iterations or time limit exceeded "):

so I am not sure what I am doing wrong..

Any advise would be more than appreciated.

Thank you for all your help.

Guillermo 

[Guillermo]

I also went at some point through the "Debugging unbounded models" and the "Debugging infeasible models" and I was able to solve some of the issues I had. I also got the idea of artificially constraint the value of v2.

If I add the commented constraint  %     constraints = [constraints, 0 <= st{1,k}(5,1) <= 100];  , then the value of v2 goes to 50 instead of 0, but it is not calculated anyway.

Thanks again,

Guillermo

[Johan Löfberg]

SeDuMI and mosek cannot solve this model as it involves quadratic equalities.

You are using an optimizer construct violating the most important step: Never use optimizer until you know everythings works with optimize. When you use optimizer, yalmip assumes you know that the solver actually supports the problems class you will obtain once the parameters are fixed. If it doesn't, the result in undefined.

This is clear if you simply send the model to sedumi (i.e. allowing the solver to optimizer over parameters too) (warning slightly more informative in develop branch)

>> optimize(constraints,objective,ops)

Warning: Solver not applicable (sedumi does not support quadratic equality constraints)

ans = 

  struct with fields:

    solvertime: 0

          info: 'Solver not applicable (sedumi does not support quadratic equalit…'

       problem: -4

    yalmiptime: 2.2210

[Johan Löfberg]

and with fmincon, well you are hoping to solve a nonconvex nonlinear program using a local nonlinear solver. All you can do is hope...

[Johan Löfberg]

Your model is weird though. The stuff that kills you are model are 

 constraints = [constraints, st{1,k}(5,1)   == st{1,k}(3,1)^2 + st{1,k}(4,1)^2]; 

constraints = [constraints, st{1,Np+1}(5,1) == Vxf^2 + Vyf^2];

however, st{1,:}(5,1)  is never used in the model, so adding those equalities makes no sense.

If you would like to have st{1,k}(5,1) <= 100, you should not add that by first defining st{1,k}(5,1)  by a nonconvex quadratic equality, to some variable and then constrain that value linearly, but simply add the convex quadratic st{1,k}(3,1)^2 + st{1,k}(4,1)^2 <= 100