一、数据结构
KUKA的位姿类型数据有一种为 E6POS，比如下面对于此类型对象 P 的赋值：
P = {X 1740.00,Y 0.0,Z 1945.00,A 0.0,B 90.0000,C 0.0,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
其结构如下：
- X、Y、Z ： 工具坐标系的原点相对于法兰坐标系的位置
- A、B、C ： 工具坐标系相对于法兰坐标系的姿态
- S ：
- T ：
- E1-E6 ： 外轴的数据（角度或距离）
关于欧拉角，KUKA是这样规定的
- 角度 A ：Z 轴的旋转
- 角度 B ：Y 轴的旋转
- 角度 C ：X 轴的旋转
二、角度跳变
试分析这三个对象 P1、P2 、P3，是否是同一个位姿
DEF angle( )
E6POS P1,P2,P3
P1={X 1740.00,Y 0.0,Z 1945.00,A 0.0,B 90.0000,C 0.0,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
P2={X 1740.00,Y 0.0,Z 1945.00,A 90.0000,B 90.0000,C 90.0000,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
P3={X 1740.00,Y 0.0,Z 1945.00,A -90.0000,B 90.0000,C -90.0000,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
$TOOL=$NULLFRAME
$BASE=$NULLFRAME
BAS(#INITMOV,100)
PTP P1
PTP P2
PTP P3
END
让我们逐步变换看看结果，显而易见,X、Y、Z 数值都是一样的，那么我们只关注 A、B、C 的不同。
对于P1
P1={X 1740.00,Y 0.0,Z 1945.00,A 0.0,B 90.0000,C 0.0,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
对于P2
P2={X 1740.00,Y 0.0,Z 1945.00,A 90.0000,B 90.0000,C 90.0000,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
对于P3
P3={X 1740.00,Y 0.0,Z 1945.00,A -90.0000,B 90.0000,C -90.0000,S 2,T 2,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
结论是同一个。
关于图片中的工具，使用的是 https://cours.etsmtl.ca/sys827/EulerAngles.html
三、比较位置
有了上面的铺垫，那么，如何比较两个E6POS类型的对象在空间里是同一个呢？就留给看官吧
怎么比较两个e6pos 有同样的姿态
需要考虑姿态的跳变，要用的逆运算
通过矩阵算子就可以实现。查看最终工具坐标系坐标轴在参考坐标系中的矢量表示就行。但如何就同一个点不同姿态来计算旋转的方位角ABC，这个如何求解？
你是说“如何比较两个E6POS类型的对象在空间里是同一个呢？” 的答案吗？不是很明白你的意思