如何从Drake获取体坐标系到惯性系的RigidTransform？

获取体坐标系到质心系的RigidTransform #

在Drake中，你可以通过MultibodyPlant中Link对象的GetBodyFrameToCentroidalFrameTransform()方法直接获取从体坐标系到质心系（即你提到的惯性系）的RigidTransform。

需要注意：SDFormat里的inertial/pose定义的是质心系相对于体坐标系的位姿（即把质心系坐标转换为体坐标系坐标的变换），而该API返回的是体坐标系到质心系的变换，二者互为逆变换。

包围椭球的坐标系转换 #

若你的包围椭球是在体坐标系下定义的，只需通过上述RigidTransform对椭球参数进行转换，即可得到质心系下的表示：

• 椭球中心：将体坐标系下的中心坐标通过变换的平移和旋转映射到质心系
• 椭球姿态：将体坐标系下的椭球旋转与变换的旋转部分相乘
• 半轴长度保持不变（缩放属性不受坐标系平移/旋转影响）

关于SpatialInertia的坐标系说明 #

你的理解是正确的：

• SDFormat中inertial的质量、惯性张量等属性，是以质心为基准，在pose指定的质心系下表达的
• Drake的SpatialInertia类中的E系正是这个质心系；如果SDFormat未指定inertial/pose（默认位姿为原点），则质心系与体坐标系重合，此时变换为单位矩阵。

内容的提问来源于stack exchange，提问作者Nicholas Pfaff