《运动学反解程序代码》由会员分享，可在线阅读，更多相关《运动学反解程序代码（4页珍藏版）》请在金锄头文库上搜索。
1、Int call_mod rbt6p_nposition(double targ6, double posi6)/ 运动学反解函数/参数说明：targ,为x, y, z的位置/为alfa, bata, game的角度, 单位mm/度/posi 为 6 个轴的长度double X=targ0;double y=targ1;double Z=targ2;double Param=3.141592653589793238462643383279/1580;double sA=sin(targ3*Param);double cA=cos(targ3*Param);double sB=sin(targ4
2、*Param);double cB=cos(targ4*Param);double sG=sin(torg5*Param);double cG=cos(targ5*Param);double Rdow n=dow n_r; 底平台半径double R u p= u p_r ; /上平台半径double UpR63;double poledown66;for(int i=0; i6;i+)UpRi0 =Rup*cos(up_anglei*Param);UpRi1 =Rup*sin(up_anglei*Param);UpRi2=0;Poledowni0=Rdown*cos(down_anglei*
3、Param);poledowni1=Rdown*sin(down_anglei*Param);poledowni2=length;/ 初始高度poledowni3=min_len;poledowni4=max_len;poledowni5=targi;Double absR3;for(i=0;i6;i+)/ 求出动平台各个铰点的 x，y，z 坐标absR0=cB*cG*UpRi0-cB*sG*UpRi1+sB*UpRi2+X;absR1=(sA*sB*cG+cA*sG)*UpRi0+(-sA*sB*sG+cA*cG)*UpRi1-sA* cB*UpR i2+Y;absR2=(-cA*sB*cG
4、+sA*sG)*UpRi0+(cA*sB*sG+sA*cG)*UpRi1+cA*cB*UpR i2+Z;double pldx=poledowni0;double pldy=poledowni1;double pldz=poledowni2;/ 计算动平台各个铰点与静平台对应铰点的距离，就是对应轴的长度Double h=poledowni2;double z1， z2;double minz=poledowni3;double maxz=poledowni4;/ 求杆长doubleB=(pldz-absR2)*(pldz-absR2)+(pldx-absR0)*(pldx-absR0)+(pldy-abs Rl)*(pldy-absR1);/doubleB=1*1-(pldx-absR0)*(pldx-absR0)-(pldy-absR1)*(pldy-absR1); if(B=minz)&(z10)z1=sqrt(B); if(z1maxz) return 0;else;poledowni 5=z1;posii =poledowni5;return 1;
