1.RAPID对于位姿Pose运算，有矩阵乘法PoseMult（pose1，pose2）和矩阵求逆PoseInv（pose1）
2.对于PoseMult，解释如下
3.对于PoseInv，解释如下
4.对于Pose3：=PoseMult(Pose1,pose2)，已知Pose1和Pose3，要计算Pose2，可以利用矩阵左乘计算
Pose3= Pose1 * Pose2
(Pose1)-1*Pose3=Pose2
即Pose2=(Pose1)-1*Pose3
Pose2:=PoseMult(PoseInv(Pose1),Pose3)
5.姿态数据orient，可以直接使用*运算，形式同位姿
Orient1=orient2*orient3
Orient3=(orient2)-1*orient1
6.对于姿态求逆，RAPID未提供标准函数
7.四元数求逆，实质就是获取四元数的共轭四元数，即
也就是o(q1,q2,q3,q4)的逆为o(q1,-q2,-q3,-q4)
8.编写求四元数逆的代码如下：
FUNC orient OriInv(orient o)
o.q2:=-o.q2;
o.q3:=-o.q3;
o.q4:=-o.q4;
RETURN o;
ENDFUNC