如何将联合索引映射到广义位置的相应索引？

Question

鉴于 floatng 基本姿势和按其相应关节索引排序的关节值列表，我想构建一个广义位置向量，以便我可以setPosition 植物。

我知道广义位置的索引0 到7 对应于浮动基本姿势。如何计算每个关节的广义位置索引？从我的实验来看，关节i的广义位置索引不仅仅是i+7。

Answer 1

Drake 关节都派生自一个通用的 Joint 基类，记录在 here。该类具有方法velocity_start()、num_velocities()、position_start()、num_positions()，它们提供广义速度和广义位置向量的索引。