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

The following is the script which I run the fmincon solver in. I am not sure whether the error is resulting because the non-linear constraint is incorrectly defined or whether my initial value x0 does not satisfy the constraints.

X = [0,0,0];
u = [0,0,0];
v_fb = [-4.2426,0,-0.7854];
n = 9;
deltaT = 0.1;
n_t = 1;
X_h_o = [2,2,0,0];
pos = safetyModule(X, u, v_fb, n, deltaT, n_t, X_h_o)

Array indices must be positive integers or logical values.

Error in solution>CBFcon (line 88)
c(i) = -(((X(i) - x_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 3*n) + ((X(i+n) - y_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 4*n) + alpha(sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 ) - D_obs));

Error in solution>@(X_h)CBFcon(X_h,n,deltaT,X_h_o,alpha,D_obs) (line 56)
nonlcon = @(X_h) CBFcon(X_h, n, deltaT, X_h_o, alpha, D_obs);

Error in fmincon (line 650)
[ctmp,ceqtmp] = feval(confcn{3},X,varargin{:});

Error in solution>safetyModule (line 65)
dec = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon);

Caused by:
Failure in initial nonlinear constraint function evaluation. FMINCON cannot continue.

function pos = safetyModule(X, u, v_fb, n, deltaT, n_t, X_h_o)
    %extend system state to match number of discretized time points in
    %horizon including t = 0
    X_h = [X, u, zeros(1, 6 * (n-1))];

    %initial guess (may not be feasible)
    x0 = X_h;

    %define constants
    D_obs = 0.5; %margin of safety
    alpha = 1;

    %adjust variables
    n_t = n_t + 1;

    %decompose v_fb
    xdot_fb = v_fb(1);
    ydot_fb = v_fb(2);
    w_fb = v_fb(3);

    %objective function 
    fun = @(X_h) 0; 
    for i = 1:n
        fun1 = abs(X_h(i+3*n) - xdot_fb) + abs(X_h(i+4*n) - ydot_fb) + abs(X_h(i+5*n) - w_fb); 
        fun = @(X_h) fun(X_h) + fun1; 
    end

    %equality constraints
    Aeq = zeros(3*n, 6 * n);

    Aeq(1,1) = 1; 
    Aeq(2,1+n) = 1;
    Aeq(3,1+2*n) = 1;

    for i = 1:n-1
        Aeq(3+i, i) = -1; Aeq(3+i,i+1) = 1; Aeq(3+i,i+n*3) = -deltaT;% x dynamics
        Aeq(2 + n + i, n + i) = -1; Aeq(2 + n + i, n + i + 1) = 1; Aeq(2 + n + i, 4 * n + i) = -deltaT;% y dynamics
        Aeq(1 + 2*n + i, 2* n + i) = -1; Aeq(1 + 2*n + i, 2 * n + i + 1) = 1; Aeq(1 + 2*n + i, 5 * n + i) = -deltaT;% z dynamics
    end

    beq = zeros(3*n, 1);
    beq(1)=X_h(1);
    beq(2)=X_h(2);
    beq(3)=X_h(3);
    
    %inequality constraints
    nonlcon = @(X_h) CBFcon(X_h, n, deltaT, X_h_o, alpha, D_obs);
    A = [];
    b = [];

    %bounds
    ub = [inf(3*n, 1), 2.5 * ones(3*n, 1)];
    lb = [-inf(3*n, 1), -2.5 * ones(3*n, 1)];
    
    %run the solver
    dec = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon);

    %extract xdot, ydot, and w for first n_p timesteps
    pos = zeros(1, (n_t - 1)* 3);

    j = 1;
    for i = 2:n_t
        pos(j) = dec(i);
        pos(j+1) = dec(i+n);
        pos(j+2) = dec(i+2*n);
        j = j + 3;
    end
    
end

I define my nonlinear constraint in a seperate file:

function [c, ceq] = CBFcon(X, n, deltaT, X_o, alpha, D_obs)
    x_curr = X_o(1);
    y_curr = X_o(3);
    x_vel = X_o(2);
    y_vel = X_o(4);

    for i = 1:n
        c(i) =  -(((X(i) - x_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 3*n)   +  ((X(i+n) - y_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) *  X(i + 4*n)  +  alpha(sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 ) - D_obs));
        x_curr = x_curr + x_vel * deltaT;
        y_curr = y_curr + y_vel * deltaT;
    end

    ceq = [];
end

[Torsten]

I am not sure whether the error is resulting because the non-linear constraint is incorrectly defined or whether my initial value x0 does not satisfy the constraints.

We don't know, either, because you didn't supply the script part in which you call "safetyModule" with appropriate input values.

[Matt J]

I am not sure whether the error is resulting because the non-linear constraint is incorrectly defined or whether my initial value x0 does not satisfy the constraints.

The former. But why not just check it yourself by calling your constraint function,

[c,ceq]=nonlcon(x0)

[Dhruv]

@Torsten

Instead of providing you with the script in which safety Module is called, I think it will be more beneficial to share the values of the input values when safety module is called. Attached is a picture of the workspace variable values.

[Catalytic]

Yes, but providing the values as a screenshot will not work because -

1. It cannot be copy-pasted.
2. It doesn't gives us the values accurately, because the screenshot only displays them to four decimal places

Please attach a .mat file with the values.

Answer 1

[Torsten]

Most probably you mean

c(i) = -(((X(i) - x_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 3*n) + ((X(i+n) - y_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 4*n) + alpha*(sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 ) - D_obs));

instead of

c(i) = -(((X(i) - x_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 3*n) + ((X(i+n) - y_curr)/sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 )) * X(i + 4*n) + alpha(sqrt((X(i) - x_curr)^2 + (X(i+n) - y_curr)^2 ) - D_obs));

[Dhruv]

Thank you, it seems this solved it! Additionally, are you aware if fmincon uses gradient descent or some other algorithm to find the minimum? Just asking for context.

[Torsten]

https://uk.mathworks.com/help/optim/ug/choosing-the-algorithm.html#bsbwxm7

Gradient decent is the simplest possible algorithm for optimization. It's not implemented in "fmincon".

[Dhruv]

Thank you for sharing @Torsten. I no longer have any runtime errors; however, it seems like the output dec of fmincon in my safetyModule function is not correct. My objective function aims to mimimize the difference between "_fb" variables and certain decision variables as such:

%decompose v_fb
    xdot_fb = v_fb(1);
    ydot_fb = v_fb(2);
    w_fb = v_fb(3);

    %objective function 
    fun = @(X_h) 0; 
    for i = 1:n
        fun1 = abs(X_h(i+3*n) - xdot_fb) + abs(X_h(i+4*n) - ydot_fb) + abs(X_h(i+5*n) - w_fb); 
        fun = @(X_h) fun(X_h) + fun1; 
    end

But, the output dec is consistently a vector of all zeros.

[Dhruv]

Perhaps, the initial guess is already a local minimum?

[Torsten]

for i = 1:n
    fun1 = @(X_h) abs(X_h(i+3*n) - xdot_fb) + abs(X_h(i+4*n) - ydot_fb) + abs(X_h(i+5*n) - w_fb);
    fun = @(X_h) fun(X_h) + fun1(X_h); 
end

instead of

for i = 1:n
    fun1 = abs(X_h(i+3*n) - xdot_fb) + abs(X_h(i+4*n) - ydot_fb) + abs(X_h(i+5*n) - w_fb); 
    fun = @(X_h) fun(X_h) + fun1; 
end

And if you mean variables that can only take discrete values by "decision variables", then you cannot solve this problem using "fmincon". "fmincon" can only solve optimization problems where all variables are continuous.

[Dhruv]

Thank you. I actually am working with variables that can only take discrete values :( That being said, I have two questions:

1. What happens if I solve this problem using fmincon? Will I get an answer which is only sightly off?

2. Do you know of any solvers which I can use for my problem which consists of variables with discrete values and also has nonlinear constraints?

[Torsten]

The solution strategy of "fmincon" is based on the differentiability of objective and constraints with respect to the solution variables. This is not given if some or all of the solution variables are discrete.

You will have to use "ga" instead with the discrete variables defined in the "intcon" array. Nonlinear constraints are only allowed as inequalities, but this seems to be the case in your problem.

[Dhruv]

@Torsten I made significant progress on my optimzation problem and tweaked the code a little bit. I am still using fmincon, and have been stuck on a certain issue. My linear inequality constraints defined by A and b are having no impact on my variables, meaning they are not being enforced. The rest of the constraints work perfectly fine. Are you able to determine why this is?

function [pos, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_h_o)
    %extend system state to match number of discretized time points in
    %horizon including t = 0
    X_h = [X, u, ones(1, 4 * (n-1))];
    fprintf('X_h: %d\n', X_h(1:4));

    %initial guess (may not be feasible)
    x0 = X_h;

    %define constants
    D_obs = 0.1; %margin of safety
    alpha = 1;

    %decompose v_fb
    xdot_fb = v_fb(1);
    ydot_fb = v_fb(2);

    %objective function 
    fun = @(X_h) 0; 

    for i = 1:n
        fun1 = @(X_h) abs(X_h(i+2*n) - xdot_fb) + abs(X_h(i+3*n) - ydot_fb);
        fun = @(X_h) fun(X_h) + fun1(X_h); 
    end

    %equality constraints
    Aeq = zeros(2 * n, 4 * n);

    Aeq(1,1) = 1; 
    Aeq(2,1+n) = 1;

    for i = 1:n-1
        Aeq(2+i, i) = -1; Aeq(2+i,i+1) = 1; Aeq(2+i,i+n*2) = -deltaT;% x dynamics
        Aeq(1 + n + i, n + i) = -1; Aeq(1 + n + i, n + i + 1) = 1; Aeq(1 + n + i, 3 * n + i) = -deltaT;% y dynamics
    end

    beq = zeros(2*n, 1);
    beq(1)=X_h(1);
    beq(2)=X_h(2);

%         fprintf('Aeq:\n');
%     for i = 1:size(Aeq, 1)
%         for j = 1:size(Aeq, 2)
%             fprintf('%d\t', Aeq(i, j)); % Change '%d' to '%f' for floating-point numbers
%         end
%         fprintf('\n'); % New line for each row
%     end
    
    %inequality constraints
    nonlcon = @(X_h) CBFcon_h(X_h, n, deltaT, X_h_o, alpha, D_obs);
    A = zeros(4*(n-1),4*n);
    for i = 1:n-1
        %less than or equal to acceleration constraint
        A(i, 2*n + i) = -1; A(i, 2*n + i + 1) = 1;
        A((n-1) + i, 3*n + i) = -1; A((n-1) + i, 3*n + i + 1) = 1;
        
        %greater than or equal to acceleration
        A(2*(n-1) + i, 2*n + i) = 1; A(2*(n-1) + i, 2*n + i + 1) = -1;
        A(3*(n-1) + i, 3*n + i) = 1; A(3*(n-1) + i, 3*n + i + 1) = -1;
    end

    b = deltaT * 0.5 * ones(4*(n-1), 1);
   
         fprintf('A:\n');
    for i = 1:size(A, 1)
        for j = 1:size(A, 2)
            fprintf('%d\t', A(i, j)); % Change '%d' to '%f' for floating-point numbers
        end
        fprintf('\n'); % New line for each row
    end

    %bounds
    ub = [4 * ones(2*n, 1), 2.5 * ones(2*n, 1)];
    lb = [zeros(2*n, 1), -2.5 * ones(2*n, 1)];
    
    %run the solver
    options = optimoptions('fmincon', 'Display', 'off');
    dec = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon, options);

    %extract x, y for the upcoming timestep
    pos = zeros(1, 2);
    pos(1) = dec(2);
    pos(2) = dec(2+n);

    %extract xdot, ydot for upcoming timestep
    u = zeros(1, 2);
    u(1) = dec(2*n + 2);
    u(2) = dec(3*n + 2);

    fprintf('xdot_fb: %d\n', xdot_fb);
    fprintf('ydot_fb: %d\n', ydot_fb);
    fprintf('u(1): %d\n', u(1));
    fprintf('u(2): %d\n', u(2));
end


function [c, ceq] = CBFcon_h(X_h, n, deltaT, X_o, alpha, D_obs)
    x_curr = X_o(1);
    y_curr = X_o(2);
    x_vel = X_o(3);
    y_vel = X_o(4);

    c = zeros(1, n);

    % Loop through iterations
for i = 1:n
    % Calculate distance between X_h(i) and current position
    d = sqrt((X_h(i) - x_curr)^2 + (X_h(i + n) - y_curr)^2);
    
    % Calculate A and B
    A = (X_h(i) - x_curr) / d;
    B = (X_h(i + n) - y_curr) / d;
    
    % Calculate constraint value for c(i)
    c(i) = - (A * X_h(i + 2*n) + B * X_h(i + 3*n) + alpha * (d - D_obs));
    
    % Update current positions using velocity and time step
    x_curr = x_curr + x_vel * deltaT;
    y_curr = y_curr + y_vel * deltaT;
end

% Return empty equality constraint array
ceq = [];
end

Here is my workspace:

[Torsten]

Please provide the input script to "safetyModule_h" you are using.

[Dhruv]

X = [0,0]; %starting state
u = [0,0];%input at t = 0

%starting state of obstacle:
X_o = zeros(1,4);
    X_o(1) = 0;
    X_o(2) = 4;
%     X_o(3) = 0;
%     X_o(4) = -0.4;

position_accuracy = 0.05;

X_des = [3,3]; %desired position 

t_max = 3; %maximum simulation time
n = 2; %horizon
deltaT = 0.05;%timestem length
t = 1; %starting time

while norm(goal(1,2) - X(1,2)) > position_accuracy && t <= t_max

    %describe behavior of obstacle
%     if t > 10 && t <= 12
%             X_o(3) = -2;
%             X_o(4) = 0;
%     end
% 
%     if t > 12
%             X_o(3) = 1;
%             X_o(4) = -1;
%     end

    v_fb = trajectoryController_h(X, X_des);

    fprintf('TIME STEP: %d\n', t);
    
    [X, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_o);

    %can use input to determine theta
    theta = atan(u(2)/u(1));
    
    %print metrics at each timestep:

%      fprintf('Input: ');
%      for i = 1:length(u)
%          fprintf('%d', u(i));
%      end
%      fprintf('\n');
     
% 
%     fprintf('Feedback velocities: ');
%     for i = 1:length(v_fb)
%         fprintf('%d ', v_fb(i));
%     end
%     fprintf('\n');
% 
     fprintf('Robot State: ');
     for i = 1:length(X)
         fprintf('%d ', X(i));
     end
     fprintf('\n \n');

    %archive and plot
    X_graph(t) = X(1);
    Y_graph(t) = X(2);
    cla;
    daspect([1 1 1]); 
    hold on;
    plot(X_o(1), X_o(2), 'or', 'MarkerSize', 5);
    plot(goal(1), goal(2), 'og', 'MarkerSize', 5); 
    plot(X_graph(1:t), Y_graph(1:t), '-b'); %traveled path
    plot([X(1) X(1)+0.2*cos(theta)], [X(2) X(2)+0.2*sin(theta)], '-r'); %heading of robot
    xlim([0, 4]);  
    ylim([0, 4]);
    drawnow;
    pause(deltaT);

    %iterate
    t = t + 1;
    X_o(1) = X_o(1) +  X_o(3)* deltaT;
    X_o(2) = X_o(2) +  X_o(4)* deltaT;

end

%{
trackingController calculates the desired velocity based on current, 
position, desired position, and previous timestep input
%}

function v_fb = trajectoryController_h(X, X_des)

%define proportional constants for P controller
K_x = 1;
K_y = 1;
x = X(1);
y = X(2);
x_des = X_des(1);
y_des = X_des(2);


%extract errors
ex = x_des - x;
ey = y_des - y;

%Proportional controller

vfb_x = K_x * ex;
vfb_y = K_y * ey;

v_fb = [vfb_x vfb_y];

%final stretch constant speed definition:
if (norm(X_des - x,y) < 0.75) 
    mag = norm(v_fb);
    v_fb = (v_fb/mag) * 1; 
end

end

[Torsten]

"goal" is undefined. Please test whether your code is complete here with MATLAB online before posting.

[Dhruv]

I did not assign a value to goal in my code, but rather in the cmd window, since I am experimenting running with different goals. This should not have any effect on my question regarding the linear constraint though. It is still not working.

[Dhruv]

I suppose I could instead just rewrite my linear inequality by writing it out in nonlcon, even though it is linear. But I still don't see why my current implementation isn't sufficient

[Torsten]

In the code below, equality and inequality constraints imposed by Aeq, beq and A,b are respected.

Please give a counterexample.

X = [0,0]; %starting state
u = [0,0];%input at t = 0
%starting state of obstacle:
X_o = zeros(1,4);
    X_o(1) = 0;
    X_o(2) = 4;
%     X_o(3) = 0;
%     X_o(4) = -0.4;
position_accuracy = 0.05;
X_des = [3,3]; %desired position 
t_max = 3; %maximum simulation time
n = 2; %horizon
deltaT = 0.05;%timestem length
t = 1; %starting time
goal(1,1) = 10;
goal(1,2) = 10;
while norm(goal(1,2) - X(1,2)) > position_accuracy && t <= t_max
    %describe behavior of obstacle
%     if t > 10 && t <= 12
%             X_o(3) = -2;
%             X_o(4) = 0;
%     end
% 
%     if t > 12
%             X_o(3) = 1;
%             X_o(4) = -1;
%     end
    v_fb = trajectoryController_h(X, X_des);
    %fprintf('TIME STEP: %d\n', t);
    
    [X, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_o);
    %can use input to determine theta
    theta = atan(u(2)/u(1));
    
    %print metrics at each timestep:
%      fprintf('Input: ');
%      for i = 1:length(u)
%          fprintf('%d', u(i));
%      end
%      fprintf('\n');
     
% 
%     fprintf('Feedback velocities: ');
%     for i = 1:length(v_fb)
%         fprintf('%d ', v_fb(i));
%     end
%     fprintf('\n');
% 
    % fprintf('Robot State: ');
   % for i = 1:length(X)
   %      fprintf('%d ', X(i));
   %  end
   %  fprintf('\n \n');
    %archive and plot
    X_graph(t) = X(1);
    Y_graph(t) = X(2);
    cla;
    daspect([1 1 1]); 
    hold on;
    plot(X_o(1), X_o(2), 'or', 'MarkerSize', 5);
    plot(goal(1), goal(2), 'og', 'MarkerSize', 5); 
    plot(X_graph(1:t), Y_graph(1:t), '-b'); %traveled path
    plot([X(1) X(1)+0.2*cos(theta)], [X(2) X(2)+0.2*sin(theta)], '-r'); %heading of robot
    xlim([0, 4]);  
    ylim([0, 4]);
    drawnow;
    pause(deltaT);
    %iterate
    t = t + 1;
    X_o(1) = X_o(1) +  X_o(3)* deltaT;
    X_o(2) = X_o(2) +  X_o(4)* deltaT;
end

ans = 4x1

-0.0250 -0.0250 -0.0250 -0.0250

ans = 4x1

1.0e-14 * 0.2176 0.2176 -0.0111 -0.0111

ans = 4x1

-0.0250 -0.0250 -0.0250 -0.0250

ans = 4x1

0 0 0 0

ans = 4x1

-0.0250 -0.0250 -0.0250 -0.0250

ans = 4x1

1.0e-16 * 0 0 0.2776 -0.2776

%{
trackingController calculates the desired velocity based on current, 
position, desired position, and previous timestep input
%}
function v_fb = trajectoryController_h(X, X_des)
%define proportional constants for P controller
K_x = 1;
K_y = 1;
x = X(1);
y = X(2);
x_des = X_des(1);
y_des = X_des(2);
%extract errors
ex = x_des - x;
ey = y_des - y;
%Proportional controller
vfb_x = K_x * ex;
vfb_y = K_y * ey;
v_fb = [vfb_x vfb_y];
%final stretch constant speed definition:
if (norm(X_des - x,y) < 0.75) 
    mag = norm(v_fb);
    v_fb = (v_fb/mag) * 1; 
end
end

function [pos, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_h_o)
    %extend system state to match number of discretized time points in
    %horizon including t = 0
    X_h = [X, u, ones(1, 4 * (n-1))];
   % fprintf('X_h: %d\n', X_h(1:4));
    %initial guess (may not be feasible)
    x0 = X_h;
    %define constants
    D_obs = 0.1; %margin of safety
    alpha = 1;
    %decompose v_fb
    xdot_fb = v_fb(1);
    ydot_fb = v_fb(2);
    %objective function 
    fun = @(X_h) 0; 
    for i = 1:n
        fun1 = @(X_h) abs(X_h(i+2*n) - xdot_fb) + abs(X_h(i+3*n) - ydot_fb);
        fun = @(X_h) fun(X_h) + fun1(X_h); 
    end
    %equality constraints
    Aeq = zeros(2 * n, 4 * n);
    Aeq(1,1) = 1; 
    Aeq(2,1+n) = 1;
    for i = 1:n-1
        Aeq(2+i, i) = -1; Aeq(2+i,i+1) = 1; Aeq(2+i,i+n*2) = -deltaT;% x dynamics
        Aeq(1 + n + i, n + i) = -1; Aeq(1 + n + i, n + i + 1) = 1; Aeq(1 + n + i, 3 * n + i) = -deltaT;% y dynamics
    end
    beq = zeros(2*n, 1);
    beq(1)=X_h(1);
    beq(2)=X_h(2);
%         fprintf('Aeq:\n');
%     for i = 1:size(Aeq, 1)
%         for j = 1:size(Aeq, 2)
%             fprintf('%d\t', Aeq(i, j)); % Change '%d' to '%f' for floating-point numbers
%         end
%         fprintf('\n'); % New line for each row
%     end
    
    %inequality constraints
    nonlcon = @(X_h) CBFcon_h(X_h, n, deltaT, X_h_o, alpha, D_obs);
    A = zeros(4*(n-1),4*n);
    for i = 1:n-1
        %less than or equal to acceleration constraint
        A(i, 2*n + i) = -1; A(i, 2*n + i + 1) = 1;
        A((n-1) + i, 3*n + i) = -1; A((n-1) + i, 3*n + i + 1) = 1;
        
        %greater than or equal to acceleration
        A(2*(n-1) + i, 2*n + i) = 1; A(2*(n-1) + i, 2*n + i + 1) = -1;
        A(3*(n-1) + i, 3*n + i) = 1; A(3*(n-1) + i, 3*n + i + 1) = -1;
    end
    b = deltaT * 0.5 * ones(4*(n-1), 1);
   
    %     fprintf('A:\n');
  %  for i = 1:size(A, 1)
  %      for j = 1:size(A, 2)
  %          fprintf('%d\t', A(i, j)); % Change '%d' to '%f' for floating-point numbers
  %      end
   %     fprintf('\n'); % New line for each row
   % end
    %bounds
    ub = [4 * ones(2*n, 1), 2.5 * ones(2*n, 1)];
    lb = [zeros(2*n, 1), -2.5 * ones(2*n, 1)];
    
    %run the solver
    options = optimoptions('fmincon', 'Display', 'off');
    dec = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon, options);
    A*dec.'-b
    Aeq*dec.'-beq
    %extract x, y for the upcoming timestep
    pos = zeros(1, 2);
    pos(1) = dec(2);
    pos(2) = dec(2+n);
    %extract xdot, ydot for upcoming timestep
    u = zeros(1, 2);
    u(1) = dec(2*n + 2);
    u(2) = dec(3*n + 2);
  %  fprintf('xdot_fb: %d\n', xdot_fb);
  %  fprintf('ydot_fb: %d\n', ydot_fb);
  %  fprintf('u(1): %d\n', u(1));
  %  fprintf('u(2): %d\n', u(2));
end
function [c, ceq] = CBFcon_h(X_h, n, deltaT, X_o, alpha, D_obs)
    x_curr = X_o(1);
    y_curr = X_o(2);
    x_vel = X_o(3);
    y_vel = X_o(4);
    c = zeros(1, n);
    % Loop through iterations
for i = 1:n
    % Calculate distance between X_h(i) and current position
    d = sqrt((X_h(i) - x_curr)^2 + (X_h(i + n) - y_curr)^2);
    
    % Calculate A and B
    A = (X_h(i) - x_curr) / d;
    B = (X_h(i + n) - y_curr) / d;
    
    % Calculate constraint value for c(i)
    c(i) = - (A * X_h(i + 2*n) + B * X_h(i + 3*n) + alpha * (d - D_obs));
    
    % Update current positions using velocity and time step
    x_curr = x_curr + x_vel * deltaT;
    y_curr = y_curr + y_vel * deltaT;
end
% Return empty equality constraint array
ceq = [];
end

[Dhruv]

Counterexample:

If you run this, you can see that there is violation of equality constraints beginning at t= 22. I am not able to identify why the violation occurs at t=22. It seems like the problem is feasible.

[Torsten]

Changing the solution method seems to work here:

X = [0,0]; %starting state
u = [0,0];%input at t = 0

%starting state of obstacle:
X_o = zeros(1,4);
    X_o(1) = 2;
    X_o(2) = 2;
    X_o(3) = 0;
    X_o(4) = 0;

position_accuracy = 0.05;

X_des = [4,4]; %desired position 

t_max = 60; %maximum simulation time
n = 4; %horizon
deltaT = 0.1;%timestep length
t = 1; %starting time
D_obs = 0.5;
alpha = 1;

h_values = zeros(1, t_max);
% v_fb_y_values = zeros(1, t_max);
% v_cmd_y_values = zeros(1,t_max);

while norm(X_des - X) > position_accuracy && t <= t_max
    %describe behavior of obstacle------------------
%     if t > 10 && t <= 25
%             X_o(3) = -2;
%             X_o(4) = -2;
%     end
% 
%     if t > 25
%             X_o(3) = 2;
%             X_o(4) = 2;
%     end
    %-------------------------------------------------
    v_fb = trajectoryController_h(X, X_des);



    fprintf('TIME STEP: %d\n', t);
    fprintf('v_fb: %d\n', v_fb);
    fprintf('u value : %d\n', u);
    [X, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_o);
%     v_fb_y_values(t) = v_fb(2);
%     v_cmd_y_values(t) = u(2);
%     %display h--------------------
     d = sqrt((X(1) - X_o(1))^2 + (X(2) - X_o(2))^2);
    
    % Calculate A and B
    A = (X(1) - X_o(1)) / d;
    B = (X(2) - X_o(2)) / d;
    
    % Calculate constraint value for c(i)
    h = - (A * u(1) + B * u(2) + alpha * (d - D_obs));
    h_values(t) = h;

    fprintf('h: %d \n \n ', h);
%     %----------------------------
%     fprintf('v_y: %d \n', u(2));
%     %can use input to determine theta
    theta = atan(u(2)/u(1));
    
%     fprintf('Robot State: ');
%      for i = 1:length(X)
%          fprintf('%d ', X(i));
%      end
%      fprintf('\n \n');

    %archive and plot
    X_graph(t) = X(1);
    Y_graph(t) = X(2);
    cla;
    daspect([1 1 1]); 
    hold on;
    plot(X_o(1), X_o(2), 'or', 'MarkerSize', 5);
    plot(X_des(1), X_des(2), 'og', 'MarkerSize', 5); 
    plot(X_graph(1:t), Y_graph(1:t), '-b'); %traveled path
    % plot([X(1) X(1)+0.2*cos(theta)], [X(2) X(2)+0.2*sin(theta)], '-r'); %heading of robot
    xlim([0, 4]);  
    ylim([0, 4]);
    drawnow;
    pause(deltaT);

    %iterate
    t = t + 1;
    X_o(1) = X_o(1) +  X_o(3)* deltaT;
    X_o(2) = X_o(2) +  X_o(4)* deltaT;

end

TIME STEP: 1

v_fb: 4
v_fb: 4

u value : 0
u value : 0

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x         xdot         y         ydot    
    ___________    ____    ___________    ____    ___________

         1            0    -1.0241e-18       0    -7.8295e-17
         2            0            0.3       0            0.3
         3         0.03            0.6    0.03            0.6
         4         0.09            0.9    0.09            0.9

h: -1.904163e+00 
 
 

TIME STEP: 2

v_fb: 4
v_fb: 4

u value : 3.000000e-01
u value : 3.000000e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot     y      ydot
    ___________    ____    ____    ____    ____

         1            0    0.3        0    0.3 
         2         0.03    0.6     0.03    0.6 
         3         0.09    0.9     0.09    0.9 
         4         0.18    1.2     0.18    1.2 

h: -1.437473e+00 
 
 

TIME STEP: 3

v_fb: 3.970000e+00
v_fb: 3.970000e+00

u value : 6.000000e-01
u value : 6.000000e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x       xdot      y       ydot 
    ___________    ____    ______    ____    ______

         1         0.03       0.6    0.03       0.6
         2         0.09       0.9    0.09       0.9
         3         0.18       1.2    0.18       1.2
         4          0.3    1.2335     0.3    1.4594

h: -9.283557e-01 
 
 

TIME STEP: 4

v_fb: 3.910000e+00
v_fb: 3.910000e+00

u value : 9.000000e-01
u value : 9.000000e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)       x        xdot       y      ydot
    ___________    _______    _______    ____    ____

         1            0.09        0.9    0.09    0.9 
         2            0.18        1.2    0.18    1.2 
         3             0.3     1.1929     0.3    1.5 
         4         0.41929    0.92947    0.45    1.5 

h: -3.768124e-01 
 
 

TIME STEP: 5

v_fb: 3.820000e+00
v_fb: 3.820000e+00

u value : 1.200000e+00
u value : 1.200000e+00

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         0.18    1.2        0.18        1.2
         2          0.3    1.5         0.3     1.1929
         3         0.45    1.5     0.41929    0.92947
         4          0.6    1.5     0.51224     0.7071

h: 0 
 
 

TIME STEP: 6

v_fb: 3.700000e+00
v_fb: 3.700000e+00

u value : 1.500000e+00
u value : 1.192893e+00

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          0.3    1.5         0.3     1.1929
         2         0.45    1.5     0.41929    0.92947
         3          0.6    1.5     0.51224     0.7071
         4         0.75    1.5     0.58295     0.5298

h: 0 
 
 

TIME STEP: 7

v_fb: 3.550000e+00
v_fb: 3.580711e+00

u value : 1.500000e+00
u value : 9.294678e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         0.45    1.5     0.41929    0.92947
         2          0.6    1.5     0.51224     0.7071
         3         0.75    1.5     0.58295     0.5298
         4          0.9    1.5     0.63593    0.39919

h: 0 
 
 

TIME STEP: 8

v_fb: 3.400000e+00
v_fb: 3.487764e+00

u value : 1.500000e+00
u value : 7.070956e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          0.6    1.5     0.51224     0.7071
         2         0.75    1.5     0.58295     0.5298
         3          0.9    1.5     0.63593    0.39919
         4         1.05    1.5     0.67584    0.31419

h: 0 
 
 

TIME STEP: 9

v_fb: 3.250000e+00
v_fb: 3.417054e+00

u value : 1.500000e+00
u value : 5.297955e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         0.75    1.5     0.58295     0.5298
         2          0.9    1.5     0.63593    0.39919
         3         1.05    1.5     0.67584    0.31419
         4          1.2    1.5     0.70726    0.27155

h: 0 
 
 

TIME STEP: 10

v_fb: 3.100000e+00
v_fb: 3.364075e+00

u value : 1.500000e+00
u value : 3.991932e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          0.9    1.5     0.63593    0.39919
         2         1.05    1.5     0.67584    0.31419
         3          1.2    1.5     0.70726    0.27155
         4         1.35    1.5     0.73442    0.26693

h: 0 
 
 

TIME STEP: 11

v_fb: 2.950000e+00
v_fb: 3.324155e+00

u value : 1.500000e+00
u value : 3.141950e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         1.05    1.5     0.67584    0.31419
         2          1.2    1.5     0.70726    0.27155
         3         1.35    1.5     0.73442    0.26693
         4          1.5    1.5     0.76111    0.29611

h: 0 
 
 

TIME STEP: 12

v_fb: 2.800000e+00
v_fb: 3.292736e+00

u value : 1.500000e+00
u value : 2.715483e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          1.2    1.5     0.70726    0.27155
         2         1.35    1.5     0.73442    0.26693
         3          1.5    1.5     0.76111    0.29611
         4         1.65    1.5     0.79072    0.35591

h: 1.110223e-16 
 
 

TIME STEP: 13

v_fb: 2.650000e+00
v_fb: 3.265581e+00

u value : 1.500000e+00
u value : 2.669323e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         1.35    1.5     0.73442    0.26693
         2          1.5    1.5     0.76111    0.29611
         3         1.65    1.5     0.79072    0.35591
         4          1.8    1.5     0.82631    0.44495

h: 0 
 
 

TIME STEP: 14

v_fb: 2.500000e+00
v_fb: 3.238888e+00

u value : 1.500000e+00
u value : 2.961149e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          1.5    1.5     0.76111    0.29611
         2         1.65    1.5     0.79072    0.35591
         3          1.8    1.5     0.82631    0.44495
         4         1.95    1.5     0.87081    0.56449

h: 0 
 
 

TIME STEP: 15

v_fb: 2.350000e+00
v_fb: 3.209276e+00

u value : 1.500000e+00
u value : 3.559116e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         1.65    1.5     0.79072    0.35591
         2          1.8    1.5     0.82631    0.44495
         3         1.95    1.5     0.87081    0.56449
         4          2.1    1.5     0.92726    0.71972

h: 2.220446e-16 
 
 

TIME STEP: 16

v_fb: 2.200000e+00
v_fb: 3.173685e+00

u value : 1.500000e+00
u value : 4.449534e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          1.8    1.5     0.82631    0.44495
         2         1.95    1.5     0.87081    0.56449
         3          2.1    1.5     0.92726    0.71972
         4         2.25    1.5     0.99923    0.92257

h: 1.110223e-16 
 
 

TIME STEP: 17

v_fb: 2.050000e+00
v_fb: 3.129190e+00

u value : 1.500000e+00
u value : 5.644947e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         1.95    1.5     0.87081    0.56449
         2          2.1    1.5     0.92726    0.71972
         3         2.25    1.5     0.99923    0.92257
         4          2.4    1.5      1.0915     1.1987

h: -2.220446e-16 
 
 

TIME STEP: 18

v_fb: 1.900000e+00
v_fb: 3.072740e+00

u value : 1.500000e+00
u value : 7.197234e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1          2.1    1.5     0.92726    0.71972
         2         2.25    1.5     0.99923    0.92257
         3          2.4    1.5      1.0915     1.1987
         4         2.55    1.5      1.2114     1.4987

h: 2.220446e-16 
 
 

TIME STEP: 19

v_fb: 1.750000e+00
v_fb: 3.000768e+00

u value : 1.500000e+00
u value : 9.225674e-01

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot       y        ydot  
    ___________    ____    ____    _______    _______

         1         2.25    1.5     0.99923    0.92257
         2          2.4    1.5      1.0915     1.1987
         3         2.55    1.5      1.2114     1.4987
         4          2.7    1.5      1.3612        1.5

h: 1.110223e-16 
 
 

TIME STEP: 20

v_fb: 1.600000e+00
v_fb: 2.908511e+00

u value : 1.500000e+00
u value : 1.198728e+00

exitflag = 1

ans = logical
   0

ans = logical
   0

    t (horizon)     x      xdot      y        ydot 
    ___________    ____    ____    ______    ______

         1          2.4    1.5     1.0915    1.1987
         2         2.55    1.5     1.2114    1.4987
         3          2.7    1.5     1.3612       1.5
         4         2.85    1.5     1.5112       1.5

h: -9.022949e-02 
 
 

TIME STEP: 21

v_fb: 1.450000e+00
v_fb: 2.788639e+00

u value : 1.500000e+00
u value : 1.498728e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x      xdot      y        ydot 
    ___________    _____    ____    ______    ______

         1          2.55     1.5    1.2114    1.4987
         2           2.7    1.45    1.3612       1.5
         3         2.845    1.45    1.5112       1.5
         4          2.99    1.45    1.6612       1.5

h: -5.076322e-01 
 
 

TIME STEP: 22

v_fb: 1.300000e+00
v_fb: 2.638766e+00

u value : 1.449999e+00
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x      xdot      y       ydot
    ___________    _____    ____    ______    ____

         1           2.7    1.45    1.3612    1.5 
         2         2.845     1.3    1.5112    1.5 
         3         2.975     1.3    1.6612    1.5 
         4         3.105     1.3    1.8112    1.5 

h: -8.504482e-01 
 
 

TIME STEP: 23

v_fb: 1.155000e+00
v_fb: 2.488766e+00

u value : 1.300006e+00
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x       xdot       y       ydot
    ___________    ______    _____    ______    ____

         1          2.845      1.3    1.5112    1.5 
         2          2.975    1.155    1.6612    1.5 
         3         3.0905    1.155    1.8112    1.5 
         4          3.206    1.155    1.9612    1.5 

h: -1.130889e+00 
 
 

TIME STEP: 24

v_fb: 1.024999e+00
v_fb: 2.338766e+00

u value : 1.155000e+00
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x       xdot       y       ydot
    ___________    ______    _____    ______    ____

         1          2.975    1.155    1.6612    1.5 
         2         3.0905    1.025    1.8112    1.5 
         3          3.193    1.025    1.9612    1.5 
         4         3.2955    1.025    2.1112    1.5 

h: -1.360852e+00 
 
 

TIME STEP: 25

v_fb: 9.094994e-01
v_fb: 2.188766e+00

u value : 1.025000e+00
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot       y       ydot
    ___________    ______    ______    ______    ____

         1         3.0905     1.025    1.8112    1.5 
         2          3.193    0.9095    1.9612    1.5 
         3          3.284    0.9095    2.1112    1.5 
         4         3.3749    0.9095    2.2612    1.5 

h: -1.553934e+00 
 
 

TIME STEP: 26

v_fb: 8.069994e-01
v_fb: 2.038766e+00

u value : 9.094994e-01
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot       y       ydot
    ___________    ______    ______    ______    ____

         1          3.193    0.9095    1.9612    1.5 
         2          3.284     0.807    2.1112    1.5 
         3         3.3647     0.807    2.2612    1.5 
         4         3.4454     0.807    2.4112    1.5 

h: -1.722214e+00 
 
 

TIME STEP: 27

v_fb: 7.160495e-01
v_fb: 1.888766e+00

u value : 8.069994e-01
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y       ydot
    ___________    ______    _______    ______    ____

         1          3.284      0.807    2.1112    1.5 
         2         3.3647    0.71605    2.2612    1.5 
         3         3.4363    0.71605    2.4112    1.5 
         4         3.5079    0.71605    2.5612    1.5 

h: -1.874732e+00 
 
 

TIME STEP: 28

v_fb: 6.353496e-01
v_fb: 1.738766e+00

u value : 7.160491e-01
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y       ydot
    ___________    ______    _______    ______    ____

         1         3.3647    0.71605    2.2612    1.5 
         2         3.4363    0.63535    2.4112    1.5 
         3         3.4998    0.63535    2.5612    1.5 
         4         3.5633    0.63535    2.7112    1.5 

h: -2.017667e+00 
 
 

TIME STEP: 29

v_fb: 5.637447e-01
v_fb: 1.588766e+00

u value : 6.353479e-01
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y       ydot
    ___________    ______    _______    ______    ____

         1         3.4363    0.63535    2.4112    1.5 
         2         3.4998    0.56374    2.5612    1.5 
         3         3.5562    0.56374    2.7112    1.5 
         4         3.6125    0.56375    2.8612    1.5 

h: -2.155058e+00 
 
 

TIME STEP: 30

v_fb: 5.002099e-01
v_fb: 1.438766e+00

u value : 5.637444e-01
u value : 1.500000e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot 
    ___________    ______    _______    ______    ______

         1         3.4998    0.56374    2.5612       1.5
         2         3.5562    0.50021    2.7112    1.4388
         3         3.6062    0.50021    2.8551    1.4388
         4         3.6562    0.50021     2.999    1.4388

h: -2.264013e+00 
 
 

TIME STEP: 31

v_fb: 4.438354e-01
v_fb: 1.288766e+00

u value : 5.002103e-01
u value : 1.438765e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot 
    ___________    ______    _______    ______    ______

         1         3.5562    0.50021    2.7112    1.4388
         2         3.6062    0.44383    2.8551    1.2888
         3         3.6506    0.44384     2.984    1.2888
         4          3.695    0.44384    3.1129    1.2888

h: -2.317039e+00 
 
 

TIME STEP: 32

v_fb: 3.938144e-01
v_fb: 1.144889e+00

u value : 4.438336e-01
u value : 1.288766e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot 
    ___________    ______    _______    ______    ______

         1         3.6062    0.44383    2.8551    1.2888
         2         3.6506    0.39381     2.984    1.1449
         3           3.69    0.39381    3.0985    1.1449
         4         3.7293    0.39381     3.213    1.1449

h: -2.346137e+00 
 
 

TIME STEP: 33

v_fb: 3.494310e-01
v_fb: 1.016013e+00

u value : 3.938145e-01
u value : 1.144889e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot 
    ___________    ______    _______    ______    ______

         1         3.6506    0.39381     2.984    1.1449
         2           3.69    0.34943    3.0985     1.016
         3         3.7249    0.34943    3.2001     1.016
         4         3.7598    0.34943    3.3017     1.016

h: -2.362283e+00 
 
 

TIME STEP: 34

v_fb: 3.100496e-01
v_fb: 9.015237e-01

u value : 3.494335e-01
u value : 1.016013e+00

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1           3.69    0.34943    3.0985      1.016
         2         3.7249    0.31005    3.2001    0.90152
         3         3.7559    0.31005    3.2902    0.90152
         4         3.7869    0.31005    3.3804    0.90152

h: -2.370678e+00 
 
 

TIME STEP: 35

v_fb: 2.751062e-01
v_fb: 7.999224e-01

u value : 3.100491e-01
u value : 9.015227e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.7249    0.31005    3.2001    0.90152
         2         3.7559    0.27511    3.2902    0.79992
         3         3.7834    0.27511    3.3702    0.79992
         4         3.8109     0.2751    3.4502    0.79992

h: -2.374310e+00 
 
 

TIME STEP: 36

v_fb: 2.441013e-01
v_fb: 7.097701e-01

u value : 2.751062e-01
u value : 7.999195e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.7559    0.27511    3.2902    0.79992
         2         3.7834     0.2441    3.3702    0.70977
         3         3.8078     0.2441    3.4412    0.70977
         4         3.8322     0.2441    3.5122    0.70977

h: -2.375009e+00 
 
 

TIME STEP: 37

v_fb: 2.165907e-01
v_fb: 6.297782e-01

u value : 2.441005e-01
u value : 7.097704e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.7834     0.2441    3.3702    0.70977
         2         3.8078    0.21659    3.4412    0.62978
         3         3.8295    0.21659    3.5042    0.62978
         4         3.8511    0.21659    3.5672    0.62978

h: -2.373922e+00 
 
 

TIME STEP: 38

v_fb: 1.921807e-01
v_fb: 5.588011e-01

u value : 2.165914e-01
u value : 6.297784e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8078    0.21659    3.4412    0.62978
         2         3.8295    0.19218    3.5042     0.5588
         3         3.8487    0.19218    3.5601     0.5588
         4         3.8679    0.19218    3.6159     0.5588

h: -2.371783e+00 
 
 

TIME STEP: 39

v_fb: 1.705215e-01
v_fb: 4.958233e-01

u value : 1.921807e-01
u value : 5.588009e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8295    0.19218    3.5042     0.5588
         2         3.8487    0.17052    3.5601    0.49582
         3         3.8657    0.17052    3.6096    0.49583
         4         3.8828    0.17052    3.6592    0.49582

h: -2.369068e+00 
 
 

TIME STEP: 40

v_fb: 1.513034e-01
v_fb: 4.399432e-01

u value : 1.705231e-01
u value : 4.958233e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8487    0.17052    3.5601    0.49582
         2         3.8657     0.1513    3.6096    0.43994
         3         3.8809    0.15131    3.6536    0.43994
         4          3.896    0.15129    3.6976    0.43994

h: -2.366078e+00 
 
 

TIME STEP: 41

v_fb: 1.342511e-01
v_fb: 3.903609e-01

u value : 1.513050e-01
u value : 4.399405e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8657     0.1513    3.6096    0.43994
         2         3.8809    0.13425    3.6536    0.39036
         3         3.8943    0.13425    3.6927    0.39036
         4         3.9077    0.13425    3.7317    0.39036

h: -2.363012e+00 
 
 

TIME STEP: 42

v_fb: 1.191206e-01
v_fb: 3.463668e-01

u value : 1.342518e-01
u value : 3.903617e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8809    0.13425    3.6536    0.39036
         2         3.8943    0.11912    3.6927    0.34636
         3         3.9062    0.11912    3.7273    0.34637
         4         3.9181    0.11912    3.7619    0.34637

h: -2.359985e+00 
 
 

TIME STEP: 43

v_fb: 1.056955e-01
v_fb: 3.073307e-01

u value : 1.191209e-01
u value : 3.463591e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x        xdot        y        ydot  
    ___________    ______    _______    ______    _______

         1         3.8943    0.11912    3.6927    0.34636
         2         3.9062     0.1057    3.7273    0.30733
         3         3.9168     0.1057     3.758    0.30733
         4         3.9274     0.1057    3.7888    0.30733

h: -2.357090e+00 
 
 

TIME STEP: 44

v_fb: 9.378336e-02
v_fb: 2.726948e-01

u value : 1.056973e-01
u value : 3.073307e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9062      0.1057    3.7273    0.30733
         2         3.9168    0.093783     3.758    0.27269
         3         3.9262    0.093783    3.7853    0.27269
         4         3.9355    0.093784    3.8126    0.27269

h: -2.354355e+00 
 
 

TIME STEP: 45

v_fb: 8.321363e-02
v_fb: 2.419617e-01

u value : 9.378345e-02
u value : 2.726948e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9168    0.093783     3.758    0.27269
         2         3.9262    0.083214    3.7853    0.24196
         3         3.9345    0.083216    3.8095    0.24196
         4         3.9428    0.083213    3.8337    0.24196

h: -2.351808e+00 
 
 

TIME STEP: 46

v_fb: 7.383529e-02
v_fb: 2.146922e-01

u value : 8.321424e-02
u value : 2.419613e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9262    0.083214    3.7853    0.24196
         2         3.9345    0.073845    3.8095    0.21469
         3         3.9419    0.073835     3.831    0.21469
         4         3.9493    0.073836    3.8524    0.21469

h: -2.349464e+00 
 
 

TIME STEP: 47

v_fb: 6.551386e-02
v_fb: 1.904961e-01

u value : 7.384523e-02
u value : 2.146898e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9345    0.073845    3.8095    0.21469
         2         3.9419    0.065509     3.831     0.1905
         3         3.9484    0.065513      3.85     0.1905
         4          3.955    0.065506    3.8691     0.1905

h: -2.347305e+00 
 
 

TIME STEP: 48

v_fb: 5.812934e-02
v_fb: 1.690271e-01

u value : 6.550924e-02
u value : 1.904977e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9419    0.065509     3.831     0.1905
         2         3.9484    0.058129      3.85    0.16903
         3         3.9542    0.058129    3.8669    0.16903
         4           3.96     0.05813    3.8838    0.16903

h: -2.345345e+00 
 
 

TIME STEP: 49

v_fb: 5.157841e-02
v_fb: 1.499773e-01

u value : 5.812887e-02
u value : 1.690267e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9484    0.058129      3.85    0.16903
         2         3.9542    0.051576    3.8669    0.14998
         3         3.9594    0.051579    3.8819    0.14998
         4         3.9645    0.051579    3.8969    0.14998

h: -2.343566e+00 
 
 

TIME STEP: 50

v_fb: 4.576553e-02
v_fb: 1.330747e-01

u value : 5.157553e-02
u value : 1.499776e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9542    0.051576    3.8669    0.14998
         2         3.9594    0.045778    3.8819    0.13307
         3          3.964    0.045765    3.8952    0.13307
         4         3.9685    0.045768    3.9085    0.13307

h: -2.341970e+00 
 
 

TIME STEP: 51

v_fb: 4.060798e-02
v_fb: 1.180769e-01

u value : 4.577818e-02
u value : 1.330743e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1         3.9594    0.045778    3.8819    0.13307
         2          3.964    0.040608    3.8952    0.11808
         3          3.968    0.040608     3.907    0.11808
         4         3.9721    0.040608    3.9188    0.11808

h: -2.340514e+00 
 
 

TIME STEP: 52

v_fb: 3.603016e-02
v_fb: 1.047695e-01

u value : 4.060850e-02
u value : 1.180773e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y        ydot  
    ___________    ______    ________    ______    _______

         1          3.964    0.040608    3.8952    0.11808
         2          3.968    0.036032     3.907    0.10477
         3         3.9716    0.036029    3.9175    0.10477
         4         3.9752    0.036031     3.928    0.10477

h: -2.339210e+00 
 
 

TIME STEP: 53

v_fb: 3.196931e-02
v_fb: 9.296174e-02

u value : 3.603218e-02
u value : 1.047678e-01

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1          3.968    0.036032     3.907     0.10477
         2         3.9716    0.031968    3.9175    0.092963
         3         3.9748    0.031969    3.9268    0.092964
         4          3.978    0.031969    3.9361    0.092963

h: -2.338041e+00 
 
 

TIME STEP: 54

v_fb: 2.836609e-02
v_fb: 8.248496e-02

u value : 3.196834e-02
u value : 9.296278e-02

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1         3.9716    0.031968    3.9175    0.092963
         2         3.9748    0.028366    3.9268    0.082485
         3         3.9777    0.028366    3.9351    0.082485
         4         3.9805    0.028366    3.9433    0.082484

h: -2.336993e+00 
 
 

TIME STEP: 55

v_fb: 2.516926e-02
v_fb: 7.318868e-02

u value : 2.836612e-02
u value : 8.248527e-02

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1         3.9748    0.028366    3.9268    0.082485
         2         3.9777    0.025169    3.9351    0.073189
         3         3.9802    0.025168    3.9424    0.073189
         4         3.9827     0.02517    3.9497    0.073189

h: -2.336056e+00 
 
 

TIME STEP: 56

v_fb: 2.233264e-02
v_fb: 6.494015e-02

u value : 2.516923e-02
u value : 7.318885e-02

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1         3.9777    0.025169    3.9351    0.073189
         2         3.9802    0.022332    3.9424    0.064939
         3         3.9824    0.022333    3.9489     0.06494
         4         3.9847    0.022332    3.9554    0.064941

h: -2.335217e+00 
 
 

TIME STEP: 57

v_fb: 1.981572e-02
v_fb: 5.762127e-02

u value : 2.233168e-02
u value : 6.493950e-02

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1         3.9802    0.022332    3.9424    0.064939
         2         3.9824    0.019815    3.9489    0.057626
         3         3.9844    0.019816    3.9546    0.057622
         4         3.9864    0.019814    3.9604    0.057621

h: -2.334471e+00 
 
 

TIME STEP: 58

v_fb: 1.758255e-02
v_fb: 5.112732e-02

u value : 1.981450e-02
u value : 5.762561e-02

exitflag = 2

ans = logical
   0

ans = logical
   0

    t (horizon)      x         xdot        y         ydot  
    ___________    ______    ________    ______    ________

         1         3.9824    0.019815    3.9489    0.057626
         2         3.9844    0.017582    3.9546    0.051127
         3         3.9862    0.017583    3.9597    0.051127
         4         3.9879    0.017582    3.9649    0.051127

h: -2.333802e+00 
 
 

% figure;
% plot(1:t_max, h_values(1:t_max));
% xlabel('Timestep');
% ylabel('h Value');
% title('Value of h at Each Timestep');
% 
% figure;
% plot(1:t_max, v_fb_y_values(1:t_max));
% hold on; 
% plot(1:t_max, v_cmd_y_values(1:t_max)); 
% hold on; 
% plot(1:t_max, v_cmd_y_values(1:t_max) - v_fb_y_values(1:t_max)); 
% xlabel('Timestep');
% ylabel('v');
% title('Value of obj at Each Timestep (only for v_y)');
% legend('v_{fb_y}', 'v_{cmd_y}', 'v_{cmd_y}-v_{fb_y}'); 
%{
safetyModule takes in system state, instantaneous input, feedback velocity, no. of
timesteps, timestep duration, no. of timesteps to return, and nearest obstacle's state
%}


% n discetized time points = N timesteps + 1

function [pos, u] = safetyModule_h(X, u, v_fb, n, deltaT, X_h_o)
    %extend system state to match number of discretized time points in
    %horizon including t = 0
    X_h = [X, u, ones(1, 4 * (n-1))];

    %initial guess (may not be feasible)
    x0 = X_h;

    %define constants
    D_obs = 0.5; %margin of safety
    alpha = 1;

    %decompose v_fb
    xdot_fb = v_fb(1);
    ydot_fb = v_fb(2);

    %objective function 
    fun = @(X_h) 0; 

    for i = 1:n
        fun1 = @(X_h) abs(X_h(i+2*n) - xdot_fb) + abs(X_h(i+3*n) - ydot_fb);
        fun = @(X_h) fun(X_h) + fun1(X_h); 
    end

    %equality constraints
    Aeq = zeros(2 * n + 2, 4 * n);

    Aeq(1,1) = 1; 
    Aeq(2,1+n) = 1;

    for i = 1:n-1
        Aeq(2+i, i) = -1; Aeq(2+i,i+1) = 1; Aeq(2+i,i+n*2) = -deltaT;% x dynamics
        Aeq(1 + n + i, n + i) = -1; Aeq(1 + n + i, n + i + 1) = 1; Aeq(1 + n + i, 3 * n + i) = -deltaT;% y dynamics
    end

    %initial velocity constraints
    Aeq(2*n + 1, 2*n + 1) = 1;
    Aeq(2*n + 2, 3*n+1) = 1;

    beq = zeros(2*n + 2, 1);
    beq(1)=X_h(1);
    beq(2)=X_h(2);
    beq(2*n + 1)=X_h(3);
    beq(2*n + 2)=X_h(4);

%         fprintf('Aeq:\n');
%     for i = 1:size(Aeq, 1)
%         for j = 1:size(Aeq, 2)
%             fprintf('%d\t', Aeq(i, j)); % Change '%d' to '%f' for floating-point numbers
%         end
%         fprintf('\n'); % New line for each row
%     end
    
    %inequality constraints
    nonlcon = @(X_h) CBFcon_h(X_h, n, deltaT, X_h_o, alpha, D_obs);
    A = zeros(4*(n-1),4*n);
    for i = 1:n-1
        %less than or equal to acceleration constraint
        A(i, 2*n + i) = -1; A(i, 2*n + i + 1) = 1;
        A((n-1) + i, 3*n + i) = -1; A((n-1) + i, 3*n + i + 1) = 1;
        
        %greater than or equal to acceleration
        A(2*(n-1) + i, 2*n + i) = 1; A(2*(n-1) + i, 2*n + i + 1) = -1;
        A(3*(n-1) + i, 3*n + i) = 1; A(3*(n-1) + i, 3*n + i + 1) = -1;
    end

    b = deltaT * 3 * ones(4*(n-1), 1);
%     fprintf('beq:\n');
%     for i = 1:size(beq, 1)
%         for j = 1:size(beq, 2)
%             fprintf('%d\t', beq(i, j)); % Change '%d' to '%f' for floating-point numbers
%         end
%         fprintf('\n'); % New line for each row
%     end
%    
%          fprintf('Aeq:\n');
%     for i = 1:size(Aeq, 1)
%         for j = 1:size(Aeq, 2)
%             fprintf('%d\t', Aeq(i, j)); % Change '%d' to '%f' for floating-point numbers
%         end
%         fprintf('\n'); % New line for each row
%     end

    %bounds
    ub = [4 * ones(2*n, 1), 1.5 * ones(2*n, 1)];
    lb = [zeros(2*n, 1), -1.5 * ones(2*n, 1)];
    
    %run the solver
    options = optimoptions('fmincon', 'Algorithm','sqp','Display', 'off','MaxIter',10000,'MaxFunEvals',10000);
    [dec,~,exitflag] = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon, options);
    exitflag
    any(A*dec.'-b>1e-2)
    any(abs(Aeq*dec.'-beq)>1e-2)
    %Table
    fields = {'t (horizon)', 'x', 'xdot', 'y', 'ydot'};

    arrayCell = cell(n, numel(fields)); 
    
    for i = 1:n
        newArray = {i, dec(i), dec(i + 2 * n), dec(i + n), dec(i + 3 * n)};
        arrayCell(i, :) = newArray;
    end
    
    T = cell2table(arrayCell, 'VariableNames', fields);
    disp(T);


    %extract x, y for the upcoming timestep
    pos = zeros(1, 2);
    pos(1) = dec(2);
    pos(2) = dec(2+n);

    %extract xdot, ydot for upcoming timestep
    u = zeros(1, 2);
    u(1) = dec(2*n + 2);
    u(2) = dec(3*n + 2);

end

%{
trackingController calculates the desired velocity based on current, 
position, desired position, and previous timestep input
%}

function v_fb = trajectoryController_h(X, X_des)

%define proportional constants for P controller
K_x = 1;
K_y = 1;
x = X(1);
y = X(2);
x_des = X_des(1);
y_des = X_des(2);


%extract errors
ex = x_des - x;
ey = y_des - y;

%Proportional controller

vfb_x = K_x * ex;
vfb_y = K_y * ey;

v_fb = [vfb_x vfb_y];

% final stretch constant speed definition:
% if (norm(X_des - x,y) < 0.75) 
%     mag = norm(v_fb);
%     v_fb = (v_fb/mag) * 0.5; 
% end

end

%{
function takes in robot system state, number of discretized time points in
the horizon, timestep duration, state of the nearest obstacle, and the CBF
constant
%}

%solver makes key assumption that obstacle does not accelerate
function [c, ceq] = CBFcon_h(X_h, n, deltaT, X_o, alpha, D_obs)
    x_curr = X_o(1);
    y_curr = X_o(2);
    x_vel = X_o(3);
    y_vel = X_o(4);
    c = zeros(1, n);
    % Loop through iterations
for i = 1:n
    % Calculate distance between X_h(i) and current position
    d = sqrt((X_h(i) - x_curr)^2 + (X_h(i + n) - y_curr)^2);
    
    % Calculate A and B
    A = (X_h(i) - x_curr) / d;
    B = (X_h(i + n) - y_curr) / d;
    
    % Calculate constraint value for c(i)
    c(i) = - (A * X_h(i + 2*n) + B * X_h(i + 3*n) + alpha * (d - D_obs));
    
    % Update current positions using velocity and time step
    x_curr = x_curr + x_vel * deltaT;
    y_curr = y_curr + y_vel * deltaT;
end
% Return empty equality constraint array
ceq = [];
end

[Dhruv]

Correct me if I am wrong, but you only made changes in the safetyModule_h function, right? Do you know why your revised solution does not violate any constraints, and why mine did?

Additionally, although your revised solution works properly when n is set to 4 in the trajectoryTracker_h file, like it was above. If you set n to a different number, say 10, constraints are once again violated.

[Torsten]

I changed

t_max = 25; %maximum simulation time

to

t_max = 60; %maximum simulation time

and

options = optimoptions('fmincon', 'Display', 'off');
dec = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon, options);

to

options = optimoptions('fmincon', 'Algorithm','sqp','Display', 'off','MaxIter',10000,'MaxFunEvals',10000);
[dec,~,exitflag] = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon, options);
exitflag
any(A*dec.'-b>1e-2)
any(abs(Aeq*dec.'-beq)>1e-2)

I don't understand your code - so I don't know what's going wrong when you set n = 10.

[Sam Chak]

I personally find it difficult to comprehend code without comprehensive annotations explaining how the code functions and without seeing the mathematical principles underlying the algorithm's construction.

Based on my understanding, it appears that the program aims to calculate the steering angle for the particle to navigate from the starting point to the destination while avoiding obstacles, using a custom model predictive control (MPC) method inspired by artificial potential fields. Unfortunately, the code exhibits flaws when an obstacle is positioned in close proximity to the destination.

X       = [0, 0];   % starting state
u       = [0, 0];   % input at t = 0

% starting state of obstacle:
X_o     = zeros(1,4);
X_o(1)  = 3.5;
X_o(2)  = 3.5;

X_des   = [4, 4];   % desired position 

[Dhruv]

@Sam Chak Yes the purpose of the function is correct, but the code doesn't necessarily only exhibit flaws when an obstacle is proximity to the goal, as you can see in the attached picture containing goal position, obstacle position, and the trajectory.

You can see running @Torsten's code that the equality constraints are suddenly violated at a timestep just after the solver is run correctly for many timesteps (look at command window to see exitflag and ans values). I do not understand why constraints are suddenly violated. Is there any information regarding the code I can provide you with to help troubleshoot? I have exhausted most debugging strategies in my arsenal:(

[Torsten]

Did you look at the exitflag from fmincon when the constraints are violated and read about its meaning ?

I printed them for each timestep.

[Dhruv]

Unfortunately, this is all I can find: "Exit condition of the local solver, returned as an integer. Generally, a positive Exitflag corresponds to a local optimum, and a zero or negative Exitflag corresponds to an unsuccessful search for a local minimum."

[Sam Chak]

@Dhruv,

It seems that your code fails to guide the particle to its destination when the destination is positioned within the repulsion radius of an obstacle. Troubleshooting complex codes can be challenging without detailed annotations that clarify the code's functionality and without insight into the underlying mathematical principles governing the algorithm's design.

If you're unable to identify the cause of one or more violated constraints, I recommend revisiting the fundamental mathematical equations and inequalities that form the basis of these constraints. It's crucial to examine the mathematics first and then review the code. Please ensure that you include detailed "comments" for each line in the constraint section, obstacle avoidance section, and steering section.

[Dhruv]

I have attached the most up to date versions of my program. Here is a brief summary of how it works:

**velocities are represented as xdot, ydot

1) Field constraints are set for the particle, goal and obstacles

2) depending on distance between particle and goal, trajectoryController.m returns v_fb

3)v_fb then needs to be refined before being applied to the particle, because v_fb did not take obstacles into account

3) safetyModule includes the objectve function aiming to reduce the difference between v_fb and the actual v applied to the particle over the next n horizon timesteps (horizon timesteps are used for the optimization solver while actual timesteps are referenced for actually plotting the robot position on the graph. constraints include:

• equality constraint- initial position and velocity must be equal to current position and velocity of particle
• equalilty constraint- basic equations of motion (d= r*t) describing evolution of position over the n hoirzon timesteps
• inequality constraint- acceleration (how much velocity variables can vary from one horizon timestep to the next)
• upper bounds and lower bounds for velocity
• nonlinear constraint- safety condition ensuring no colliisions with the obstalces occur

4) Once the optimization program has run the, the x,y posiiton of the robot at the "next" horizon timestep can be plotted in trajectoryTracker, and the x,y velocity at the "next" horizon timestep can be stored to be fed into the safety module at the next actual timestep.

5) The timestep is iterated and the procedure is repeated until the particel reached the goal or simulation times runs out

Now if you run my code, you will be able to see the following data for each timestep in the simulation (h represents value of the expression included in the nonlinear constraint). In certain timesteps (18 in this case) the equality constraint is randomly violated as you can see the x,xdot,y,ydot values for t(horizon) = 2, which were applied to the robot in the previous timestep 17 do not match the corresponding values for t(horizon) = 1 for timestep 18.

I HAVE NO IDEA WHY THIS CONSTRAINT IS RANDOMLY VIOLATED. It seems like often times but not always when, I reduce the number of horizon timesteps (n), the issue is fixed. Still don't understand why... I will gladly explain the code more, but I am just lost since I have no previous experience with solver such as fmincon.

[Torsten]

Here are the meanings of the exitflags from the documentation available under

https://uk.mathworks.com/help/optim/ug/fmincon.html

exitflag — Reason fmincon stopped

integer

Reason fmincon stopped, returned as an integer.

All Algorithms:

1

First-order optimality measure was less than options.OptimalityTolerance, and maximum constraint violation was less than options.ConstraintTolerance.

0

Number of iterations exceeded options.MaxIterations or number of function evaluations exceeded options.MaxFunctionEvaluations.

-1

Stopped by an output function or plot function.

-2

No feasible point was found.

All algorithms except active-set:

2

Change in x was less than options.StepTolerance and maximum constraint violation was less than options.ConstraintTolerance.

trust-region-reflective algorithm only:

3

Change in the objective function value was less than options.FunctionTolerance and maximum constraint violation was less than options.ConstraintTolerance.

active-set algorithm only:

4

Magnitude of the search direction was less than 2*options.StepTolerance and maximum constraint violation was less than options.ConstraintTolerance.

5

Magnitude of directional derivative in search direction was less than 2*options.OptimalityTolerance and maximum constraint violation was less than options.ConstraintTolerance.

interior-point, sqp-legacy, and sqp algorithms:

-3

Objective function at current iteration went below options.ObjectiveLimit and maximum constraint violation was less than options.ConstraintTolerance.