产品:79
联系我们
定义分别为绕Z、Y、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。
2.四元数：
四元数(quaternion)可以看作中学时学的复数的扩充，它有三个虚部：Q=w+xi+yj+zk,
其中w,x,y,z又叫四元数的元素。
3.库卡欧拉角转四元数：
DEFFCT QUATERNION EulerZYXToQuaternion(ABC:IN )
DECL EulerZYX ABC
DECL QUATERNION Q
DECL REAL SINA,COSA
DECL REAL SINB,COSB
DECL REAL SINC,COSC
DECL INT Sum
IF VARSTATE("ABC")<>#INITIALIZED THEN
MsgQuit("Input Euler Angles not initialized!")
LOOP
HALT
ENDLOOP
ENDIF
SINC=SIN(ABC.Rx*0.5)
COSC=COS(ABC.Rx*0.5)
SINB=SIN(ABC.Ry*0.5)
COSB=COS(ABC.Ry*0.5)
SINA=SIN(ABC.Rz*0.5)
COSA=COS(ABC.Rz*0.5)
Q.W=COSC*COSB*COSA+SINC*SINB*SINA
Q.X=SINC*COSB*COSA-COSC*SINB*SINA
Q.Y=COSC*SINB*COSA+SINC*COSB*SINA
Q.Z=COSC*COSB*SINA-SINC*SINB*COSA
Sum=Q.W*Q.W+Q.X*Q.X+Q.Y*Q.Y+Q.Z*Q.Z
IF Sum<>1 THEN
MsgQuit("Sum of Quaternion elements power NOT 1!")
LOOP
HALT
ENDLOOP
ELSE
RETURN (Q)
ENDIF
ENDFCT
更多点击库卡机器人
- 下一篇：库卡机器人轴零点校正工具:EMD
- 上一篇：库卡机器人的零点