6R机械臂正逆解程序

%% dgtikine6s
% 适用于****机器人
function theta=dgtikine6s2（robotTorient）
    L=robot.links;
    T = inv（robot.base） * T * inv（robot.tool）;
    y=T（24）;x=T（14）;
    % theta（1）
    theta（1）=atan（y/x）;
    if  orient（1）==‘w‘                                                     %所在平面在X轴的正方向
        theta（1）=atan（y/x）;
    else                                                                   %所在平面在Y轴的负方向
       theta（1）=pi+theta（1）;
    end
    T1=trotz（-theta（1））*T;                                                 %将机械臂旋转至y=0的（xz）平面
    l1=L（1）.a; l2=L（2）.a;   l3=L（3）.a;  l4=L（4）.d;
    x4=T1（14）;  y4=T1（24）; z4=T1（34）;
    x2=l1;z2=0;                                                            %关节2所在的坐标
    l11=sqrt（（x4-x2）^2+（z4-z2）^2）;                                         %Z2取零
    l34=sqrt（l3^2+l4^2）;                                                   %关节4到关节3的直线距离即l‘
    theta1=-angle（（x4-x2）+（z4-z2）*1i）;
    theta2=acos（（l2^2+l11^2-l34^2）/（2*l2*l11））;
    theta（2）=theta1-theta2;
    %theta（2）
    if orient（2）==‘u‘                                                      %u为zx平面x轴正方向的方位
         theta（2）=theta（2）;    
    else 
        theta（2）=-2*angle（（x4-x2）+（z4-z2）*1i）-theta（2）;%（姿态2的解）
     e