How to map the generalized coordinate vector of the multi-body plant to the generalized coordinate vector of the rigid body tree in Drake?

Question

A humanoid robot can be described by a kinematic tree in a URDF file. I found that the order of the elements in the generalized coordinate vectors of the rigid body tree and the multi-body plant are different. There is a code snippet online showing the way to achieve the mapping.

# q_rbt = X*q_mbp
# rigid_body_tree is an instance of RigidBodyTree()
# multi_body_plant is an instance of MultibodyPlant()
B_rbt = rigid_body_tree.B 
B_mbp = multi_body_plant.MakeActuationMatrix()
X = np.dot(B_rbt[6:,:],B_mbp[6:,:].T)

However, RigidBodyTree has been deprecated in the new Drake. Hence, how can I achieve the mapping now? In addition, I am curious about why Drake does not use the same order for the generalized coordinate vectors.

Answer 1

You might like the workflow I used in this littledog example. I have a PR in flight to enable that workflow directly in Drake. You can track the progress on in with this issue.

In general the recommendation, and I would recommend this even if we hadn’t implemented it a particular way in Drake, is that if you have something as complicated as humanoid, don’t try to work with the vectors based on indices. Find a way to access the elements via their names, or in Drake via their joint accessors. Working with the raw vector is very error prone.