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

Question

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

我知道广义位置的索引 0 到 7 对应于浮动基础姿势。如何计算出每个关节的广义位置指数？根据我的实验，关节 i 的广义位置索引不仅仅是 i+7.

Answer 1

Drake 关节全部派生自一个共同的关节基础 class，记录在案 here. That class has methods velocity_start()、num_velocities()、position_start()、num_positions()提供广义速度和广义位置向量的索引。