target=[2.5,-1.7];
syms theta1;syms theta2,syms theta3;
l1=1; l2=1.5; l3=2;
joint1=l1.*[cos(theta1),sin(theta1)];
joint2=joint1+l2.*[cos(theta2),sin(theta2)];
endpoint1=joint2+l3.*[cos(theta3),sin(theta3)];
theta=[theta1,theta2,theta3];
J=jacobian(endpoint1,theta);
theta=[0.00001,0.00001,0.00001];
step=1000;
endpoint=computeend(theta);
E=target-endpoint;
k=0;
while k<step&&norm(E)>eps
J_num=subs(J,{theta1,theta2,theta3},{theta(1),theta(2),theta(3)});
J_inv=pinv(J_num);
delta=J_inv*E';
if(max(delta)>pi/90.0)
delta=delta*(pi/90.0)/max(delta);
end
theta_min=-2*pi;theta_max=4*pi;
theta=theta+delta';
for i=1:3
if(theta(i)<theta_min)
theta(i)=theta(i)+2*pi;
end
if(theta(i)>theta_max)
theta(i)=theta(i)-4*pi;
end
end
endpoint=computeend(theta);
E=target-endpoint;
k=k+1;
end
joint1=l1.*[cos(theta(1)),sin(theta(1))];
joint2=joint1+l2.*[cos(theta(2)),sin(theta(2))];
joint3=target;
a1=sqrt(joint1(1)^2+joint1(2)^2);
a2=sqrt((joint2(1)-joint1(1))^2+(joint2(2)-joint1(2))^2);
a3=sqrt((joint2(1)-joint3(1))^2+(joint2(2)-joint3(2))^2);
X=[0,joint1(1),joint2(1),joint3(1)];
Y=[0,joint1(2),joint2(2),joint3(2)];
figure(1);
plot(target(1),target(2),'o',joint1(1),joint1(2),'*',joint2(1),joint2(2),'*',endpoint(1),endpoint(2),'*');
line(X,Y);
评论0