运动正解与逆解-matlab的编程

%¾ØÕóÖвÎÊý¶¨Ò壺symstheta1theta2theta3theta4theta5theta6symsdhl2l3l4b1b2b3b4b5b6%¸÷Æë´Î±ä»»¾ØÕó£ºb1=[cos(theta1)0-sin(theta1)0;sin(theta1)0cos(theta1)0;0-10h;0001]b1=[cos(theta1),0,-sin(theta1),0][sin(theta1),0,cos(theta1),0][0,-1,0,h][0,0,0,1]b2=[cos(theta2)0sin(theta2)l2*cos(theta2);sin(theta2)0-cos(theta2)l2*sin(theta2);0100;0001]b2=[cos(theta2),0,sin(theta2),l2*cos(theta2)][sin(theta2),0,-cos(theta2),l2*sin(theta2)][0,1,0,0][0,0,0,1]b3=[cos(theta3)0-sin(theta3)l3*cos(theta3);sin(theta3)0cos(theta3)l3*sin(theta3);0-100;0001]b3=[cos(theta3),0,-sin(theta3),l3*cos(theta3)][sin(theta3),0,cos(theta3),l3*sin(theta3)][0,-1,0,0][0,0,0,1]b4=[cos(theta4)0sin(theta4)l4*cos(theta4);sin(theta4)0-cos(theta4)l4*sin(theta4);0100;0001]b4=[cos(theta4),0,sin(theta4),l4*cos(theta4)][sin(theta4),0,-cos(theta4),l4*sin(theta4)][0,1,0,0][0,0,0,1]b5=[cos(theta5)0-sin(theta5)0;sin(theta5)0cos(theta5)0;0-100;0001]b5=[cos(theta5),0,-sin(theta5),0][sin(theta5),0,cos(theta5),0][0,-1,0,0][0,0,0,1]b6=[cos(theta6)-sin(theta6)00;sin(theta6)cos(theta6)00;001d;0001]b6=[cos(theta6),-sin(theta6),0,0][sin(theta6),cos(theta6),0,0][0,0,1,d][0,0,0,1]%»úеÊÖÔ˶¯Ñ§Õý½â£ºb1*b2*b3*b4*b5*b6ans=[sin(theta6)*(sin(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))-cos(theta1)*cos(theta4)*sin(theta2))-cos(theta6)*(cos(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))+cos(theta1)*sin(theta2)*sin(theta4))+sin(theta5)*(cos(theta3)*sin(theta1)+cos(theta1)*cos(theta2)*sin(theta3))),cos(theta6)*(sin(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))-cos(theta1)*cos(theta4)*sin(theta2))+sin(theta6)*(cos(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))+cos(theta1)*sin(theta2)*sin(theta4))+sin(theta5)*(cos(theta3)*sin(theta1)+cos(theta1)*cos(theta2)*sin(theta3))),sin(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))+cos(theta1)*sin(theta2)*sin(theta4))-cos(theta5)*(cos(theta3)*sin(theta1)+cos(theta1)*cos(theta2)*sin(theta3)),d*(sin(theta5)*(cos(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))+cos(theta1)*sin(theta2)*sin(theta4))-cos(theta5)*(cos(theta3)*sin(theta1)+cos(theta1)*cos(theta2)*sin(theta3)))-l4*cos(theta4)*(sin(theta1)*sin(theta3)-cos(theta1)*cos(theta2)*cos(theta3))+l2*cos(theta1)*cos(theta2)-l3*sin(theta1)*sin(theta3)+l3*cos(theta1)*cos(theta2)*cos(theta3)-l4*cos(theta1)*sin(theta2)*sin(theta4)][cos(theta6)*(cos(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))-sin(theta1)*sin(theta2)*sin(theta4))+sin(theta5)*(cos(theta1)*cos(theta3)-cos(theta2)*sin(theta1)*sin(theta3)))-sin(theta6)*(sin(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))+cos(theta4)*sin(theta1)*sin(theta2)),-cos(theta6)*(sin(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))+cos(theta4)*sin(theta1)*sin(theta2))-sin(theta6)*(cos(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))-sin(theta1)*sin(theta2)*sin(theta4))+sin(theta5)*(cos(theta1)*cos(theta3)-cos(theta2)*sin(theta1)*sin(theta3))),cos(theta5)*(cos(theta1)*cos(theta3)-cos(theta2)*sin(theta1)*sin(theta3))-sin(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))-sin(theta1)*sin(theta2)*sin(theta4)),l4*cos(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))-d*(sin(theta5)*(cos(theta4)*(cos(theta1)*sin(theta3)+cos(theta2)*cos(theta3)*sin(theta1))-sin(theta1)*sin(theta2)*sin(theta4))-cos(theta5)*(cos(theta1)*cos(theta3)-cos(theta2)*sin(theta1)*sin(theta3)))+l2*cos(theta2)*sin(theta1)+l3*cos(theta1)*sin(theta3)+l3*cos(theta2)*cos(theta3)*sin(theta1)-l4*sin(theta1)*sin(theta2)*sin(theta4)][-cos(theta6)*(cos(theta5)*(cos(theta2)*sin(theta4)+cos(theta3)*cos(theta4)*sin(theta2))-sin(theta2)*sin(theta3)*sin(theta5))-sin(theta6)*(cos(theta2)*cos(theta4)-cos(theta3)*sin(theta2)*sin(theta4)),sin(theta6)*(cos(theta5)*(cos(theta2)*sin(theta4)+cos(theta3)*cos(theta4)*sin(theta2))-sin(theta2)*sin(theta3)*sin(theta5))-cos(theta6)*(cos(theta2)*cos(theta4)-cos(theta3)*sin(theta2)*sin(theta4)),sin(theta5)*(cos(theta2)*sin(theta4)+cos(theta3)*cos(theta4)*sin(theta2))+cos(theta5)*sin(theta2)*sin(theta3),h-l2*sin(theta2)+d*(sin(theta5)*(cos(theta2)*sin(theta4)+cos(theta3)*cos(theta4)*sin(theta2))+cos(theta5)*sin(theta2)*sin(theta3))-l3*cos(theta3)*sin(theta2)-l4*cos(theta2)*sin(theta4)-l4*cos(theta3)*cos(theta4)*sin(theta2)][0,0,0,1]%Õý½âÑéË㣺symstheta1theta2theta3theta4theta5theta6dl2l3l4theta1=0,theta2=0,theta3=0,theta4=0,theta5=0,theta6=0£»x=d*(sin(theta5).*(cos(theta4).*(sin(theta1).*sin(theta3)-cos(theta1).*cos(theta2).*cos(theta3))+cos(theta1).*sin(theta2).*sin(theta4))-cos(theta5).*(cos(theta3).*sin(theta1)+cos(theta1).*cos(theta2).*sin(theta3)))-l4*cos(theta4).*(sin(theta1).*sin(theta3)-cos(theta1).*cos(theta2).*cos(theta3))+l2*cos(theta1).*cos(theta2)-l3*sin(theta1).*sin(theta3)+l3*cos(theta1).*cos(theta2).*cos(theta3)-l4*cos