1、问题来源
想这样一个问题：机械臂在当前位姿下，给定一个位姿变换，求解变换后的位姿。即UR机械臂中pose_trans(p_from, p_from_to)接口。
姿态变换：
第一个变量p_from用于变换第二个自变量p_from_to，然后返回结果。这意味着结果是从p_from坐标系开始，然后坐标系移动p_from_to生成的姿态。
此函数可从两个不同的角度来看。无论是哪个函数变换，都是通过参数p_from转变或旋转p_from_to。或者此函数用于获取先移动p_from，然后从那里移动p_from_to时生成的姿态。
如果姿态被视为变换矩阵，则如下所示：
T_world->to = T_world->from * T_from->to
T_x->to = T_x->from * T_from->to
参数：
p_fom：起始姿态（空间矢量）
p_from_to：相对于起始姿态的姿态变化（空间矢量）
返回值：
生成的姿态（空间矢量）
2、相关知识点
2.1 变换——平移和旋转的总结
包含姿态和位置信息的4×4齐次变换矩阵，作为表示坐标系的一般工具。齐次变换矩阵的三个定义：
1>它是坐标系的描述.
表示相对于坐标系{A}坐标系{B}。特别是，
的各列是坐标系{B}主轴方向单位上的单位矢量，APBORG确定了{B}的原点。
之前知识温习：
5_机械臂运动学基础_矩阵
6_机械臂运动学_刚体转动的描述
7机器人位姿的数学描述与坐标变
2>它是变换映射。
是映射BP->AP.
3>它是变换算子。T将AP1变换AP2。
即坐标系和变换都可以用位置矢量加上姿态来描述。一般来说，坐标系主要用于描述，而变换常用来表示映射或算子。变换是平移和旋转的组合。
2.2 混合变换
已知坐标系{C}相对于坐标系{B}，并且已知坐标系{B}相对于坐标系{A}.将CP变换成BP：
BP =
CP ①
然后将BP变换成AP
AP=
BP ②
AP=
CP ③
由此定义
=
注意，熟练使用上下标可使运算简化。由已知{B}和{C}的描述，因此可求得
为：
