Failure in initial objective function evaluation. LSQNONLIN cannot continue.
3 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
athena1738
am 21 Mär. 2024
Bearbeitet: Torsten
am 22 Mär. 2024
%%%% 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 Kommentar
Akzeptierte Antwort
Walter Roberson
am 21 Mär. 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 Kommentare
Weitere Antworten (0)
Siehe auch
Kategorien
Mehr zu Bodies finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!