Failure in initial objective function evaluation. LSQNONLIN cannot continue.
6 views (last 30 days)
Show older comments
%%%% 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
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.
1 Comment
Accepted Answer
Walter Roberson
on 21 Mar 2024
%%%% 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
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
0 Comments
More Answers (0)
See Also
Categories
Find more on Bodies in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!