IK.rar_ik_robot_动力学 机器人_机器人_机器人IK

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