%%?dgtikine6s
%?適用于****機(jī)器人
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軸的負(fù)方向
???????theta（1）=pi+theta（1）;
????end
????T1=trotz（-theta（1））*T;?????????????????????????????????????????????????%將機(jī)械臂旋轉(zhuǎn)至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;????????????????????????????????????????????????????????????%關(guān)節(jié)2所在的坐標(biāo)
????l11=sqrt（（x4-x2）^2+（z4-z2）^2）;?????????????????????????????????????????%Z2取零
????l34=sqrt（l3^2+l4^2）;???????????????????????????????????????????????????%關(guān)節(jié)4到關(guān)節(jié)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）;%（姿態(tài)2的解）
?????e