Drake中如何将多体植物的广义坐标向量映射到刚体树的广义坐标向量？答案

【问题标题】：How to map the generalized coordinate vector of the multi-body plant to the generalized coordinate vector of the rigid body tree in Drake?Drake中如何将多体植物的广义坐标向量映射到刚体树的广义坐标向量？
【发布时间】：2021-05-31 02:59:02
【问题描述】：

人形机器人可以用 URDF 文件中的运动学树来描述。我发现刚体树和多体植物的广义坐标向量中元素的顺序是不同的。网上有一段代码sn -p 展示了实现映射的方法。

# q_rbt = X*q_mbp
# rigid_body_tree is an instance of RigidBodyTree()
# multi_body_plant is an instance of MultibodyPlant()
B_rbt = rigid_body_tree.B 
B_mbp = multi_body_plant.MakeActuationMatrix()
X = np.dot(B_rbt[6:,:],B_mbp[6:,:].T)

但是，新 Drake 中已弃用 RigidBodyTree。因此，我现在如何实现映射？另外，我很好奇为什么Drake没有对广义坐标向量使用相同的顺序。

【问题讨论】：

标签： drake

【解决方案1】：

您可能会喜欢我在此littledog example 中使用的工作流程。我有一个 PR 可以直接在 Drake 中启用该工作流程。您可以通过this issue 跟踪进度。

一般来说，即使我们没有在 Drake 中以特定方式实现它，我也会推荐这个建议，如果你有像人形机器人这样复杂的东西，不要尝试使用基于索引的向量.找到一种通过元素名称访问元素的方法，或者通过它们的联合访问器在 Drake 中访问元素。使用原始向量很容易出错。

【讨论】：