Failure in initial objective function evaluation. LSQNONLIN cannot continue.

%%%% code for biggest astroid possible
clc
clear all
close all

%%% This example shows how to do the inverse kinematics for a 2 link manipulator and do an animation.
% <<three_link.png>>

%%% movie parameters
movieWrite = 0;
movieName = 'manipulator_drawing3.avi';

%%% link parameters (m) 
l1 = 1; l2 = 0.5; l3 = 0.5;

delay = 0.05; %increase the value to slow down animation

%%% parameters of the circle
x_center = 1; y_center = 2; a = 4;

%%%% t is the parameter used to describe the astroid 
t = linspace(0,2*pi,51);
n = length(t);
x_ref_all = x_center+a*cos(t).^3;        
y_ref_all = y_center+a*sin(t).^3;

%%%% initial guess for lsqnonlin %%%
theta10 = 1.5; 
theta20 = 2.5; 
theta30 = 0.5; 


%%% Solve for the values of theta that give the required end-effector pose.
%fsolve solves for the roots for the equation F = [x-x_ref; y-y_ref];
for i=1:n
    x_ref = x_ref_all(i);
    y_ref = y_ref_all(i);
    param = [l1 l2 l3 x_ref y_ref];

   
    options = optimoptions('lsqnonlin','Display','off');
    %x0=initial guess; lb=lower bounds; ub=upper bouds (wrt X_0)
    [X,resnorm,residual,exitflag] = lsqnonlin(@fn_end_effector_position,[theta10,theta20,theta30],[-pi/2,0,0],[pi/2,pi,pi/2],options);
    %%%%%%%%%% save the solution %%%%%%%%%%
    theta1(i) = X(1);
    theta2(i) = X(2);
    theta3(i) = X(3);
    
    theta10 = X(1);
    theta20 = X(2);
    theta30 = X(3);
    disp(['Exitflag after running lsqnonlin = ', num2str(exitflag) ]) %Tells if fsolve converged or not
                   %1 means converged else not converged
                   %Type help fsolve to know more about what different 
                   %EXITFLAG mean.
end               

Not enough input arguments.

Error in solution>fn_end_effector_position (line 126)
l1 = param(1);

Error in lsqnonlin (line 242)
initVals.F = feval(funfcn{3},xCurrent,varargin{:});

Caused by:
Failure in initial objective function evaluation. LSQNONLIN cannot continue.

figure(1)
plot(theta1,'r'); hold on
plot(theta2,'b-');
plot(theta3,'m-.');
ylabel('position');
legend('theta1','theta2','theta3','Location','Best');

figure(2) 
x_drawn = [];
y_drawn = [];

if (movieWrite)
   mov = VideoWriter(movieName); 
   open(mov);
end

for i=1:n
 
%%%% origin of the world/fixed frame %%%
x_O0 = 0; y_O0 = 0;

%%%%%%% end of link1 in world frame %%%
x_P = l1*cos(theta1(i)); 
y_P = l1*sin(theta1(i));

%%%% end of link2 in world frame %%%
x_Q = l2*cos(theta1(i)+theta2(i));
y_Q = l2*sin(theta1(i)+theta2(i));

%%%% end of link3 in world frame %%%
x_R = l1*cos(theta1(i))+l2*cos(theta1(i)+theta2(i))+l3*cos(theta1(i)+theta2(i)+theta3(i));
y_R = l1*sin(theta1(i))+l2*sin(theta1(i)+theta2(i))+l3*sin(theta1(i)+theta2(i)+theta3(i));

x_drawn = [x_drawn; x_R];
y_drawn = [y_drawn; y_R];

%Plot the point where we want the end-effector
plot(x_drawn(1:i),y_drawn(1:i),'k'); 

%%%%%%%% draw line to generate the manipulator
line([x_O0 x_P],[y_O0 y_P],'Linewidth',5,'Color','r');
line([x_P x_Q],[y_P y_Q],'Linewidth',5,'Color','g');
line([x_Q x_R],[y_Q y_R],'Linewidth',5,'Color','b');

xlabel('x');
ylabel('y');
grid on; %if you want the grid to show up.

%These set the x and y limits for the axis (will need adjustment)
xlim([-15 15]); 
ylim([-15 15]);

pause(delay);

if (movieWrite)
    axis off %does not show axis
    set(gcf,'Color',[1,1,1]) %set background to white
    writeVideo(mov,getframe);
end

end

if (movieWrite)
    close(mov);
end

The function included is:

function F = fn_end_effector_position(theta,param)

l1 = param(1); 
l2 = param(2);
l3 = param(3);
x_ref = param(4);
y_ref = param(5);

% Get individual theta's from input
theta1 = theta(1);
theta2 = theta(2);
theta3 = theta(3);

% Location of end of link 3 wrt frame 0
x_R = l1*cos(theta1)+l2*cos(theta1+theta2)+l3*cos(theta1+theta2+theta3);
y_R = l1*sin(theta1)+l2*sin(theta1+theta2)+l3*sin(theta1+theta2+theta3);

%x,y of end of link3, F = 0 is the output
F = [x_R-x_ref; y_R-y_ref; 0]; %the extra 0 is needed to get lsqnonlin to work 
end

After I run the code for the astroid, it gives me this list of errors:

Not enough input arguments.

Error in fn_end_effector_position (line 3)

l1 = param(1);

Error in lsqnonlin (line 242)

initVals.F = feval(funfcn{3},xCurrent,varargin{:});

Error in lsqnonlin_fwd_inv_kin (line 43)

[X,resnorm,residual,exitflag] = lsqnonlin('fn_end_effector_position',[theta10,theta20,theta30],[-pi/2,0,0],[pi/2,pi,pi/2],options);

Caused by:

Failure in initial objective function evaluation. LSQNONLIN cannot continue.

-Any ideas on what I did wrong? Thank you in advance.

[Torsten]

So you want to adjust three parameters to satisfy two equations ? Usually, this problem will have more than one solution.

Answer 1

[Walter Roberson]

%%%% code for biggest astroid possible
%%% This example shows how to do the inverse kinematics for a 2 link manipulator and do an animation.
% <<three_link.png>>

%%% movie parameters
movieWrite = 0;
movieName = 'manipulator_drawing3.avi';

%%% link parameters (m) 
l1 = 1; l2 = 0.5; l3 = 0.5;

delay = 0.05; %increase the value to slow down animation

%%% parameters of the circle
x_center = 1; y_center = 2; a = 4;

%%%% t is the parameter used to describe the astroid 
t = linspace(0,2*pi,51);
n = length(t);
x_ref_all = x_center+a*cos(t).^3;        
y_ref_all = y_center+a*sin(t).^3;

%%%% initial guess for lsqnonlin %%%
theta10 = 1.5; 
theta20 = 2.5; 
theta30 = 0.5; 


%%% Solve for the values of theta that give the required end-effector pose.
%fsolve solves for the roots for the equation F = [x-x_ref; y-y_ref];
for i=1:n
    x_ref = x_ref_all(i);
    y_ref = y_ref_all(i);
    param = [l1 l2 l3 x_ref y_ref];

   
    options = optimoptions('lsqnonlin','Display','off');
    %x0=initial guess; lb=lower bounds; ub=upper bouds (wrt X_0)
    [X,resnorm,residual,exitflag] = lsqnonlin(@(theta)fn_end_effector_position(theta,param), [theta10,theta20,theta30],[-pi/2,0,0],[pi/2,pi,pi/2],options);
    %%%%%%%%%% save the solution %%%%%%%%%%
    theta1(i) = X(1);
    theta2(i) = X(2);
    theta3(i) = X(3);
    
    theta10 = X(1);
    theta20 = X(2);
    theta30 = X(3);
    disp(['Exitflag after running lsqnonlin = ', num2str(exitflag) ]) %Tells if fsolve converged or not
                   %1 means converged else not converged
                   %Type help fsolve to know more about what different 
                   %EXITFLAG mean.
end               

Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 2
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 2
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 1
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3
Exitflag after running lsqnonlin = 3

figure(1)
plot(theta1,'r'); hold on
plot(theta2,'b-');
plot(theta3,'m-.');
ylabel('position');
legend('theta1','theta2','theta3','Location','Best');

figure(2) 
x_drawn = [];
y_drawn = [];

if (movieWrite)
   mov = VideoWriter(movieName); 
   open(mov);
end

for i=1:n
 
%%%% origin of the world/fixed frame %%%
x_O0 = 0; y_O0 = 0;

%%%%%%% end of link1 in world frame %%%
x_P = l1*cos(theta1(i)); 
y_P = l1*sin(theta1(i));

%%%% end of link2 in world frame %%%
x_Q = l2*cos(theta1(i)+theta2(i));
y_Q = l2*sin(theta1(i)+theta2(i));

%%%% end of link3 in world frame %%%
x_R = l1*cos(theta1(i))+l2*cos(theta1(i)+theta2(i))+l3*cos(theta1(i)+theta2(i)+theta3(i));
y_R = l1*sin(theta1(i))+l2*sin(theta1(i)+theta2(i))+l3*sin(theta1(i)+theta2(i)+theta3(i));

x_drawn = [x_drawn; x_R];
y_drawn = [y_drawn; y_R];

%Plot the point where we want the end-effector
plot(x_drawn(1:i),y_drawn(1:i),'k'); 

%%%%%%%% draw line to generate the manipulator
line([x_O0 x_P],[y_O0 y_P],'Linewidth',5,'Color','r');
line([x_P x_Q],[y_P y_Q],'Linewidth',5,'Color','g');
line([x_Q x_R],[y_Q y_R],'Linewidth',5,'Color','b');

xlabel('x');
ylabel('y');
grid on; %if you want the grid to show up.

%These set the x and y limits for the axis (will need adjustment)
xlim([-15 15]); 
ylim([-15 15]);

pause(delay);

if (movieWrite)
    axis off %does not show axis
    set(gcf,'Color',[1,1,1]) %set background to white
    writeVideo(mov,getframe);
end

end

if (movieWrite)
    close(mov);
end

function F = fn_end_effector_position(theta,param)

l1 = param(1); 
l2 = param(2);
l3 = param(3);
x_ref = param(4);
y_ref = param(5);

% Get individual theta's from input
theta1 = theta(1);
theta2 = theta(2);
theta3 = theta(3);

% Location of end of link 3 wrt frame 0
x_R = l1*cos(theta1)+l2*cos(theta1+theta2)+l3*cos(theta1+theta2+theta3);
y_R = l1*sin(theta1)+l2*sin(theta1+theta2)+l3*sin(theta1+theta2+theta3);

%x,y of end of link3, F = 0 is the output
F = [x_R-x_ref; y_R-y_ref; 0]; %the extra 0 is needed to get lsqnonlin to work 
end