Given the floatng base pose and a list of joint values ordered by their corresponding joint index, I want to construct a generalized position vector so I can setPosition the plant.
I am aware that indices 0 to 7 of the generalized positions corresponds to the floating base pose. How do I figure out the generalized position index of each joint? From my experimentation, the generalized position index of joint i is not simply i+7.
Drake joints all derive from a common Joint base class, documented here. That class has methods velocity_start(), num_velocities(), position_start(), num_positions() that provide indexes into the generalized velocity and generalized position vectors.