What is the recommended way to find the (position/velocity) indices of a free body in the `MultibodyPlant` state vector?

user_9510020 0
Viewed 96

There are a number of use cases where we want to introspect the state vector coming out of the MultibodyPlant get_state_output_port. As a simple example, imagine I am logging the state output port and just want to label my axes (or know which axes to plot).

What is the recommended API for this? For joints, I know that we can iterate through joints (e.g. via get_joints()) and then ask each joint for it’s position/velocities indices. Do we have something similar for free bodies?

I know we can get the actual positions/velocities of a free body in a number of ways, given a context, but didn’t actually see the way to get the indices?

It’s tempting to say “just log with get_state_output_port(model_instance)”, but this particular MultibodyPlant is buried inside a Diagram (like MultibodyPlant).

drake
1 Answers

If you don't mind iterating through all of the bodies (similarly to the joint cast mentioned above), you should be able to call is_floating() on each body, then if that returns true, check floating_positions_start() and floating_velocities_start() for the indices in the state vector. There isn't a way to get the number of positions/velocities, but it looks like has_quaternion_dofs() is being used as a proxy for that functionality.

note: I didn't actually code up a test to make sure that this idea works.

user_97316820
Related