NaN's cannot be converted to logicals. Failure in initial nonlinear constraint function evaluation. FMINCON cannot continue.

[D Duda]

Dear Johan,

I got following error in my system for case #3 and #4 (see below), which appears just after optimiser function is complete.

I am using expanded model, based on simulink MPC controller, with 6 inputs and 6 outputs. 6DoF manipulator.

Part of the model works fine. 

But when trying adding new part of the code, I am facing an error. And I am unable to resolve it since Thursday.

I did try to follow other discussions on the forum on similar issue, but seams I can not getting anything of that, to make it work.

I got sqrt() function in the loop, which already replaced for sqrtm (). That thx to some forum topics I have read.

Error Message

Error I got, just after optimiser () is finalised

An error occurred while running the simulation and the simulation was terminated

Caused by:

Error due to multiple causes.

NaN's cannot be converted to logicals.

Failure in initial nonlinear constraint function evaluation. FMINCON cannot continue.

Error in 'Simulation/MPC Controller2' while evaluating expression.

Working and Issues Cases

Below is a snippet of the code, where I have commented up as following

% #1 -- working case

% #2 -- working case

% #3 -- Already broken case

% #4 -- Desired but broken case

        % #1 -- working case

%         VectorP (1) =  650 ; % forward (x)

%         VectorP (2) =  -99 ; % right (y)

%         VectorP (3) =  32 ; % up (z)

        

        % #2 -- working case

        % + 0.00001 % to avoid case with division by 0

%         VectorP (1) =  r (1) + 0.00001 - 150 ; % forward (x)

%         VectorP (2) =  r (2) + 0.00001 - 0; % right (y)

%         VectorP (3) =  r (3) + 0.00001 - 0 ; % up (z)

        % #3 -- Already broken case

        % + 0.00001 % to avoid case with division by 0

        VectorP (1) =  r (1) + 0.00001 - 150 ; % forward (x)

        VectorP (2) =  r (2) + 0.00001 - 0; % right (y)

        VectorP (3) =  r (3) + 0.00001 - mtxR0z ; % up (z)

        

        % #4 -- Desired but broken case

        % + 0.00001 % to avoid case with division by 0

%         VectorP (1) =  r (1) + 0.00001 - mtxR0x ; % forward (x)

%         VectorP (2) =  r (2) + 0.00001 - mtxR0y; % right (y)

%         VectorP (3) =  r (3) + 0.00001 - mtxR0z ; % up (z)

Each of 4 case examples, vary from each other, by adding parameter, toward desired case.

In case #3, is attached mtxR0z parameter, which is 'z' output of matrix calculations, of Forward Kinematic (FK) function.
Full FK calculations are depended on x+1 output parameters of the controller.

Attaching also project zip file with the issue on case #3, where cases are indicated in lines 276 to 291 of the controller.m file.
Also full code of controller.m file is on the bottom, where around 2/3 down I have commented what discussed above.


Assist Needed

Hence my question if, if is there any solution, to make it work? I read about similar issue somewhere, that problem may not be solvable, if I understood correctly?
Your additional guidance would be most appreciated.


Best Regards.

Attachments

% Uses YALMIP libraries (development version)

% based on MPC controller

% 

% Example of 3DoF manipulator

% Author: Dobromil K Duda

% 28 April 2017

function uout = Controller (currentx,xyzSetpoint,t)

 

disp (clock) ;

persistent Controller

 

persistent runPassNumber ; % define pass counter

% timer

persistent tic0

persistent toc0

        

if t == 0

    

    disp ('Solveer initialized') ;

    

    tic0 = tic ;   

    

    runPassNumber = 0 ; % initiate pass counter

    

    L0x = 80 ;

    L0y = 0 ;

    L0z = 0 ;

    % links dimentions

    L1x = 137.29 ;

    L1y = 35.36 ;

    L1z = -32.01 ;

    L2x = 413.25 ;

    L2y = 301.53 ;

    L2z = 0 ;

    L3x = 161 ;

    L3y = -84.85 ;

    L3z = 0 ;

    L4x = 44.25 ;

    L4y = -129.3 ;

    L4z = 0 ;

    L5x = 18.45 ;

    L5y = 278.05 ;

    L5z = 0 ;

    L6x = 150 ;

    L6y = 0 ;

    L6z = 0 ;

        

    

    % YALMIP state space initail matrixes 

    [A, B, C, D] = ABC( ) ;

    Plant = ss (A,B,C,D) ;

 

    % Compute discrete-time dynamics

    Ts = 0.1 ;

    Gd = c2d(Plant,Ts) ;

    Ad = Gd.A ;

    Bd = Gd.B ;

    

    % Define data for MPC controller

    N = 10;

    Q = 10 ;

    R = 0.1 ;

    

    % Avoid explosion of internally defined variables in YALMIP

    yalmip('clear')

    

    %nu = 3 ; % Number of inputs

    %nx = 6 ; % Number of states   

    

    nu = 6 ; % Number of inputs

    nx = 6 ; % Number of states  

   

    u = sdpvar(repmat(nu,1,N),repmat(1,1,N)) ;

    x = sdpvar(repmat(nx,1,N+1),repmat(1,1,N+1)) ;

    

    %r = sdpvar (3,1) ;

    r = sdpvar (6,1) ;

     

    EEPos = sdpvar (3,1) ; % End Effector position xyz

    

    % EEPos = sdpvar (3,1) ; % End Effector position xyz

      

    DesiredOientation = sdpvar (3,1) ;

    

    VectorP = sdpvar (3,1) ; 

    % Define simple standard MPC controller

    % Current state is known so we replace this

    constraints = [];

    objective = 0;

    

    for k = 1:N

        

        objective = objective + (r-C*x{k})'*Q*(r-C*x{k})+u{k}'*R*u{k};        

           

        % YALMIP constrains

        % Dynamics

        constraints = [constraints, x{k+1} == Ad*x{k}+Bd*u{k}];

         

        

        % ---------------------------------------- %

        % Forward kinematics Solver

        % ---------------------------------------- %

        

        

        % joints inputs (radians)

        j1 = x{k+1}(1) ;

        j2 = x{k+1}(2) ;

        j3 = x{k+1}(3) ;

        j4 = x{k+1}(4) ;

        j5 = x{k+1}(5) ;

        j6 = x{k+1}(6) ;

        % sinus cosinus

        % calculate Forward Kinematic for 6DoF manipulator

        c1 = cos(j1);

        s1 = sin(j1);

        c2 = cos(j2);

        s2 = sin(j2);

        c3 = cos(j3);

        s3 = sin(j3);

        c4 = cos(j4);

        s4 = sin(j4);

        c5 = cos(j5);

        s5 = sin(j5);

        c6 = cos(j6);

        s6 = sin(j6);

        % ---- 6 DoF

        mtxR6x = -s6*L6y+c6*L6z+L5x ;

        mtxR6y = L6x+L5y ;

        mtxR6z = c6*L6y+s6*L6z+L5z ;

        mtxR5x = s5*mtxR6x+c5*mtxR6y+L4x ;

        mtxR5y = -c5*mtxR6x+s5*mtxR6y+L4y ;

        mtxR5z = mtxR6z+L4z ;

        % ---- 5 DoF

 

        % replace with above, if changing from 5Dof to 6DoF

%         mtxR6x = 0 ;

%         mtxR6y = 0 ;

%         mtxR6z = 0 ;

%     

%         mtxR5x = s5*+c5*mtxR6y+L4x ;

%         mtxR5y = -c5*mtxR6x+s5*mtxR6y+L4y ;

%         mtxR5z = mtxR6z+L4z ;

%         

%         % ---- 5 DoF

% 

        mtxR4x = c4*mtxR5x+s4*mtxR5z+L3x ;

        mtxR4y = mtxR5y+L3y ;

        mtxR4z = -s4*mtxR5x+c4*mtxR5z+L3z ;

        mtxR3x = c3*mtxR4x-s3*mtxR4y+L2x ;

        mtxR3y = s3*mtxR4x+c3*mtxR4y+L2y ;

        mtxR3z = mtxR4z+L2z ;

        mtxR2x = c2*mtxR3x-s2*mtxR3y+L1x ;

        mtxR2y = s2*mtxR3x+c2*mtxR3y+L1y ;

        mtxR2z = mtxR3z+L1z ;

        mtxR1x = c1*mtxR2x-s1*mtxR2z+L0x ;

        mtxR1y = mtxR2y+L0y ;

        mtxR1z = s1*mtxR2x+c1*mtxR2z+L0z ;

        mtxR0x = mtxR1x ;

        mtxR0y = -mtxR1y ;

        mtxR0z = -mtxR1z ;

        

        % end effector position xyz

        EEPos(1) = mtxR0x ; % x

        EEPos(2) = mtxR0y ; % y

        EEPos(3) = mtxR0z ; % z

 

        % ---------------------------------------- %

        % End Effector Constraints

        % ---------------------------------------- %

        

%        tolerance = 0.1 ;

         tolerance = 1 ; % xyz setpoint deadband +- cm

         constraints = [constraints, r(1) + tolerance >= EEPos(1) >= r(1) - tolerance ] ; % x

         constraints = [constraints, r(2) + tolerance >= EEPos(2) >= r(2) - tolerance ] ; % y

         constraints = [constraints, r(3) + tolerance >= EEPos(3) >= r(3) - tolerance ] ; % z

          

        % ---------------------------------------- %

        % u Constraints

        % ---------------------------------------- %        

        

        uLim = 15 ; % u limit

        constraints = [constraints, uLim >= u{1}(1) >= -uLim ] ; %

        constraints = [constraints, uLim >= u{1}(2) >= -uLim ] ; %

        constraints = [constraints, uLim >= u{1}(3) >= -uLim ] ; %

        constraints = [constraints, uLim >= u{1}(4) >= -uLim ] ; %

        constraints = [constraints, uLim >= u{1}(5) >= -uLim ] ; %

        %constraints = [constraints, uLim >= u{1}(6) >= -uLim ] ; %

        % ---------------------------------------- %

        % Joints Constraints (as x)

        % ---------------------------------------- %

        

        jointsLim = pi() / 2 ; % higher constrain then better

        constraints = [constraints, jointsLim >= x{k+1}(1) >= -jointsLim ] ;

        constraints = [constraints, jointsLim >= x{k+1}(2) >= -jointsLim ] ;

        constraints = [constraints, jointsLim >= x{k+1}(3) >= -jointsLim ] ;

        constraints = [constraints, jointsLim >= x{k+1}(4) >= -jointsLim ] ;

        constraints = [constraints, jointsLim >= x{k+1}(5) >= -jointsLim ] ;

         

        

        % ---------------------------------------- %

        % Pitch Roll Yaw Setpoint (Reference to End Effector)

        % ---------------------------------------- %

        

        % ---- 5 DoF

 

        % replace with above, if changing from 5Dof to 6DoF

        mtxR6x = 0 ;

        mtxR6y = 0 ;

        mtxR6z = 0 ;

    

        mtxR5x = s5*+c5*mtxR6y+L4x ;

        mtxR5y = -c5*mtxR6x+s5*mtxR6y+L4y ;

        mtxR5z = mtxR6z+L4z ;

        

        % ---- 5 DoF

        mtxR4x = c4*mtxR5x+s4*mtxR5z+L3x ;

        mtxR4y = mtxR5y+L3y ;

        mtxR4z = -s4*mtxR5x+c4*mtxR5z+L3z ;

        mtxR3x = c3*mtxR4x-s3*mtxR4y+L2x ;

        mtxR3y = s3*mtxR4x+c3*mtxR4y+L2y ;

        mtxR3z = mtxR4z+L2z ;

        mtxR2x = c2*mtxR3x-s2*mtxR3y+L1x ;

        mtxR2y = s2*mtxR3x+c2*mtxR3y+L1y ;

        mtxR2z = mtxR3z+L1z ;

        mtxR1x = c1*mtxR2x-s1*mtxR2z+L0x ;

        mtxR1y = mtxR2y+L0y ;

        mtxR1z = s1*mtxR2x+c1*mtxR2z+L0z ;

        mtxR0x = mtxR1x ;

        mtxR0y = -mtxR1y ;

        mtxR0z = -mtxR1z ;

        

        % end effector position xyz

        %EEPos(1) = mtxR0x ; % x

        %EEPos(2) = mtxR0y ; % y

        %EEPos(3) = mtxR0z ; % z

%         constraints = [constraints, 0 < mtxR0x > 0 ] ;

%         constraints = [constraints, 0 < mtxR0y > 0 ] ;

%         constraints = [constraints, 0 < mtxR0z > 0 ] ;

        

        % #1 -- working case

%         VectorP (1) =  650 ; % forward (x)

%         VectorP (2) =  -99 ; % right (y)

%         VectorP (3) =  32 ; % up (z)

        

        % #2 -- working case

        % + 0.00001 % to avoid case with division by 0

%         VectorP (1) =  r (1) + 0.00001 - 150 ; % forward (x)

%         VectorP (2) =  r (2) + 0.00001 - 0; % right (y)

%         VectorP (3) =  r (3) + 0.00001 - 0 ; % up (z)

        % #3 -- Already broken case

        % + 0.00001 % to avoid case with division by 0

        VectorP (1) =  r (1) + 0.00001 - 150 ; % forward (x)

        VectorP (2) =  r (2) + 0.00001 - 0; % right (y)

        VectorP (3) =  r (3) + 0.00001 - mtxR0z ; % up (z)

        

        % #4 -- Desired but broken case

        % + 0.00001 % to avoid case with division by 0

%         VectorP (1) =  r (1) + 0.00001 - mtxR0x ; % forward (x)

%         VectorP (2) =  r (2) + 0.00001 - mtxR0y; % right (y)

%         VectorP (3) =  r (3) + 0.00001 - mtxR0z ; % up (z)

        

        % https://yalmip.github.io/squareroots/ 

        % YALMIP sqrtm() instead sqrt()        

        d = sqrtm(VectorP(1)^2 + VectorP(2)^2 + VectorP(3)^2) ;    

        Fi = acos(VectorP(3)/d) ;   % elevation angle % yaw    

        Theta = atan(VectorP(1)/VectorP(2)) ; % azimuth % pitch

        % -------- alternative solution, also do not work

        

         %VectorP (1) =  800 + 0.00001 + 150 ; % forward (x)

%         VectorP (2) =  -300 + 0.00001  ; % right (y)

%         VectorP (3) =  32 + 0.00001  ; % up (z)

        % https://yalmip.github.io/squareroots/ 

        % YALMIP sqrtm() instead sqrt()

%        P = sqrtm ( VectorP (1)^2 + VectorP (2)^2 + VectorP (3)^2 ) ;

        

%        Gamma = acos (VectorP (3)/P) ;

 

        % theta - represents spherical angle between vector 'P' and 'z' axis (up)     

        % which in other words, rotates around 'y' axis of perpendicular vector, to vector 'P'

%        Theta = gamma ; % radians 

 

        % ensure that x axis points toward 0 angle and z axis towar pi()/2 =

        % 90 degrees

%        Theta = pi()/2 - theta ; 

 

        % Fi - represents angle between projetcet vector of vector 'P' on plane

        % XY and 'x' axis (forward)

        % which in other words, rotates around 'z' axis

%        Fi = acos (VectorP (1) / sqrt (VectorP(1)^2 + VectorP(2)^2) ) ; % radians sqrtm

%        Fi = acos (VectorP (1) / sqrtm (VectorP(1)^2 + VectorP(2)^2) ) ; % radians 

        

        % ---------------------------------------- %

        % EE Orientation Constraints

        % ---------------------------------------- %

        

        

        orientationTolerance = pi () * 2 ;

        constraints = [constraints, r(4) + orientationTolerance >= Theta >= r(4) - orientationTolerance ] ; % EE yaw

        constraints = [constraints, r(5) + orientationTolerance >= Fi >= r(5) - orientationTolerance ] ; % EE pitch

        constraints = [constraints, r(6) + orientationTolerance >= 0 >= r(6) - orientationTolerance ] ; % EE roll

          

    end

    

    parameters_in = {x{1},r} ;

    

    solutions_out = {u{1}};

    

    % ops = sdpsettings('verbose',2); % option, debug and prints outs

    ops = []; % option disabled, uubcomment above, to enable options

    

    disp ('Optimizer Started') ;

    

    % Define an optimizer object which solves the problem for a particular

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

    disp ('Optimizer Complete') ;

    

    disp ('Controller Started') ;

    

    % And use it here too

    uout = Controller{{currentx,xyzSetpoint}};

    

    disp ('Controller Complete') ;

    

    % debugging

    toc ;

    toc (tic0) ;

    %b = toc ;

    %executionTime = b - a ;

    % disp (['Execution time: ' executionTime]) ;

    

else    

   

    % debugging

    disp (['#' num2str(runPassNumber) ' - Controller at run time executed at t: ' num2str(t)]) ;

    % a = tic ; 

    tic ;

    runPassNumber = runPassNumber + 1 ;

    

    % controller

    % Almost no overhead

    uout = Controller{{currentx,xyzSetpoint}};

    

    % debugging

    % b = toc ;

    % executionTime = b - a ;

    % disp (['Execution time: ' executionTime]) ;

    toc ;

    toc (tic0) ;

    

end

project zip file:

https://goo.gl/MjbrDV

[Johan Löfberg]

Supply a reproducible example which doesn't involve simulink, i.e., a simple call Controller(stuff,stuff,stuff)

[Johan Löfberg]

however, this

  d = sqrtm(VectorP(1)^2 + VectorP(2)^2 + VectorP(3)^2) ;    

       Fi = acos(VectorP(3)/d) ;   % elevation angle % yaw    

       Theta = atan(VectorP(1)/VectorP(2)) ;

is a typical place for failure as it might be that the solver is in an initial iteration where d or VectorP(2) is zero, and boom, NaN

Try to write the model is a singularity-free form, i.e, instead of a = b/c, introduce new variable a, and the constraint a*c == b. Also, in case of trigonometrics, it is not uncommon that you can write weird trigs using quadratic functions and equalities etc.

[D Duda]

Thank you very much for suggestions.

I will definitely look into mentioned cases. I will see, what I can figure out of it.

Also, I was trying avoid VectorP (1:3) from being 0 at initial conditions, hence added 0.00001. But however, is not guaranteed condition, obviously.

Yes, pointed by you code, is exactly new, part,  which causes me whole hiccup. 

Need more investigation here then.

Requested Code Without Simulink

And in response to your earlier query regarding reproducible code without simulink, Attaching below (just in case something more obvious from the error log)

Files and code on the bottom.

Executing command outputs following error (in red):

>> ControllerReproducable ([0 0 0 0 0 0]',[800 -100 32 0 0 0]',0)

         2017            4           28           12           22       49.915

Solveer initialized

Optimizer Started

Optimizer Complete

Controller Started

Error using  & 

NaN's cannot be converted to logicals.

Error in apply_recursive_differentiation (line 129)

active = find(monomsj & dx);

Error in fmincon_con (line 90)

    dx =

    apply_recursive_differentiation(model,xevaled,requested,model.Crecursivederivativeprecompute);

    

Error in fmincon_con_liftlayer (line 4)

    [g,geq,dg,dgeq,xevaled] = fmincon_con(x,model);

Error in fmincon (line 639)

      [ctmp,ceqtmp,initVals.gnc,initVals.gnceq] = feval(confcn{3},X,varargin{:});

Error in callfmincon (line 64)

[xout,fmin,flag,output,lambda] =

fmincon('fmincon_fun_liftlayer',model.x0,model.A,model.b,model.Aeq,model.beq,model.lb,model.ub,callback_con,model.options.fmincon,model);

Error in optimizer/subsref (line 306)

                eval(['output = ' self.model.solver.call '(self.model);']);

Error in ControllerReproducable (line 363)

    uout = Controller{{currentx,xyzSetpoint}};

Caused by:

    Failure in initial nonlinear constraint function evaluation. FMINCON cannot continue.

Attachements

Matlab zip file project (no simulink)
https://goo.gl/UhyVyn

or, copy paste (hopefully works)

To call below code, need execute following line

ControllerReproducable ([0 0 0 0 0 0]',[800 -100 32 0 0 0]',0)

% Uses YALMIP libraries (development version)

% based on MPC controller

% 

% Example of 3DoF manipulator

% Author: Dobromil K Duda

% 28 April 2017

function uout = ControllerReproducable (currentx,xyzSetpoint,t)

 

disp (clock) ;

persistent Controller

 

persistent runPassNumber ; % define pass counter

% timer

persistent tic0

persistent toc0

        

% if t == 0

    % [A, B, C, D] = ABC( ) ;

    A = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1] * 0.2 ;

    %A = ones (6,6) * -0.5 ;

    B = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1] * 1.0; % states gain, see state-space scope

    C = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1] * 1 ;

    %D = zeros (6,5) ;

    D = zeros (6,6) ;

% else    

%    

%     % debugging

%     disp (['#' num2str(runPassNumber) ' - Controller at run time executed at t: ' num2str(t)]) ;

%     % a = tic ; 

%     tic ;

%     runPassNumber = runPassNumber + 1 ;

%     

%     % controller

%     % Almost no overhead

%     uout = Controller{{currentx,xyzSetpoint}};

%     

%     % debugging

%     % b = toc ;

%     % executionTime = b - a ;

%     % disp (['Execution time: ' executionTime]) ;

%     toc ;

%     toc (tic0) ;

%     

% end

[D Duda]

Good news,

I have replaced

 d = sqrtm(VectorP(1)^2 + VectorP(2)^2 + VectorP(3)^2) ;    
Fi = acos(VectorP(3)/d) ;   % elevation angle % yaw    
Theta = atan(VectorP(1)/VectorP(2)) ;

for 

d = sqrtm(VectorP(1)^2 + VectorP(2)^2 + VectorP(3)^2) ;   

        

% singularity-free form

constraints = [constraints, VectorP3bySqrt * d == VectorP(3) ] ;

         

Fi = acos(VectorP3bySqrt) ;   % elevation angle % yaw    

        

% singularity-free form

constraints = [constraints, VectorP12 * VectorP(2) == VectorP(1) ] ;

Theta = atan(VectorP12) ; % azimuth % pitch

and adding before for loop, on the top variables definitions

VectorP3bySqrt = sdpvar (1,1) ;     

VectorP12 = sdpvar (1,1) ; 

Haven't checked yet, if received result are correct, but at least no more error. :)

Many thx.

[D Duda]

Just to add, as I read past days on forum.

I know you have mentioned before in some post, about  avoiding division when possible, as solver don't like it. 

Hence that made more sense, for implementation in this case.