nijie.m

机械手运动学方程求解 。分别输入机械臂的末端坐标

%机械手运动学方程求解     
l1=1;l2=0.8;        %假设连杆1的长度为 1m，连杆2的长度为 0.8 m。
      x=input('enter x(-1.5 to 1.5):')     %分别输入机械臂的末端坐标，输入范围定为（-1.5，1.5）和（0，1.6）
while (x>1.5) | (x<-1.5)
    x=input('x must in -1.5 to 1.5,Please re-type:')
end
        
y=input('enter y(0 to 1.6):')
while (y>1.6) | (y<0)
    y=input('y must in 0 to 1.6,Please re-type:')
end
o2=acos((x^2+y^2-l1^2-l2^2)/(2*l1*l2));               %     0<o2<pi
o22=-(acos((x^2+y^2-l1^2-l2^2)/(2*l1*l2)));           %  -pi<o2<0
r1=acos((x^2+y^2+l1^2-l2^2)/(2*l1*sqrt(x^2+y^2)));     % 0<r1<pi
r2=-acos((x^2+y^2+l1^2-l2^2)/(2*l1*sqrt(x^2+y^2)));     % -pi<r2<0
b=atan(y/x);
o1=b-r1;
o12=b+r1;
o1j=[o1;o2]                                    %第一组角度解
o2j=[o12;o22]                                  %第二组角度解