drake - 在“MultibodyPlant”状态向量中查找自由体（位置/速度）索引的推荐方法是什么？

Question

在许多用例中，我们想要内省来自MultibodyPlant get_state_output_port. 作为一个简单的例子，假设我正在记录状态输出端口并且只想标记我的轴（或知道要绘制哪些轴）。

推荐的 API 是什么？对于关节，我知道我们可以遍历关节（例如 via get_joints()），然后询问每个关节的位置/速度指数。对于自由体，我们有类似的东西吗？

我知道在给定上下文的情况下，我们可以通过多种方式获得自由体的实际位置/速度，但实际上并没有看到获取索引的方法？

说“just log with ”很诱人get_state_output_port(model_instance)，但这个细节 MultibodyPlant隐藏在Diagram(like MultibodyPlant) 中。

Answer 1

如果您不介意遍历所有主体（类似于上面提到的联合演员），您应该能够调用is_floating()每个主体，然后如果返回 true，请检查状态向量floating_positions_start()中floating_velocities_start()的索引。没有办法获得位置/速度的数量，但它看起来has_quaternion_dofs()被用作该功能的代理。

注意：我实际上并没有编写测试代码来确保这个想法有效。