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

Question

Dhruv on 23 Mar 2024

0
Link

Direct link to this question

https://in.mathworks.com/matlabcentral/answers/2097866-failure-in-initial-nonlinear-constraint-function-evaluation-fmincon-cannot-continue

Edited: Torsten on 28 Mar 2024 at 11:41

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

4 Comments
Show 2 older commentsHide 2 older comments

Dhruv on 23 Mar 2024

@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 on 23 Mar 2024

Edited: Catalytic on 23 Mar 2024

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

It cannot be copy-pasted.
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.

Sign in to comment.

Sign in to answer this question.

Answer 1

Torsten on 23 Mar 2024

0
Link

Direct link to this answer

https://in.mathworks.com/matlabcentral/answers/2097866-failure-in-initial-nonlinear-constraint-function-evaluation-fmincon-cannot-continue#answer_1429936

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));

25 Comments
Show 23 older commentsHide 23 older comments

Dhruv on 26 Mar 2024

Open in MATLAB Online

@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:

Dhruv on 26 Mar 2024

Open in MATLAB Online

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 on 26 Mar 2024

Edited: Torsten on 26 Mar 2024

Open in MATLAB Online

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

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

Torsten on 27 Mar 2024

Edited: Torsten on 27 Mar 2024

Open in MATLAB Online

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 on 28 Mar 2024

Edited: Dhruv on 28 Mar 2024

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 on 28 Mar 2024 at 11:40

Edited: Torsten on 28 Mar 2024 at 11:41

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.

Sign in to comment.

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

4 Comments
Show 2 older commentsHide 2 older comments

Answers (1)

25 Comments
Show 23 older commentsHide 23 older comments

See Also

Categories

Tags

Products

Release

Community Treasure Hunt

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

4 Comments Show 2 older commentsHide 2 older comments

Answers (1)

25 Comments Show 23 older commentsHide 23 older comments

See Also

Categories

Tags

Products

Release

Community Treasure Hunt

4 Comments
Show 2 older commentsHide 2 older comments

25 Comments
Show 23 older commentsHide 23 older comments