I am working on recovering from collision. I have the names of bodies in collision and the frames associated with them and now I want to move the body/frame that is closer to the end effector to get out of collision, but I couldn't find a straightforward way to get this information from a MultiBodyPlant. I could construct another representation of the graph and search through it, but I was wondering if it is possible to maybe get this from drake instead?
The problem is that sometimes the robot ends up in collision with itself or the environment and I want to make a plan to recover it.
From the QueryObject, I am able to get a vector<SignedDistancePair> that gives me the geometry IDs of object instances collision, and unit vector pointing in the direction of fastest increase in collision depth
Then I use a SceneGraphInspector to get the corresponding frame IDs and then use the frame IDs to get the bodies in collision
For now I make the assumption that only two bodies are in collision
Now that I have the two bodies in collision, I want to find the one that is closer to the end effector and is therefore easier to move out of collision
Wow. I think you're right that we don't make this one easy (but we should).
For now, I would think you can call MultibodyPlant::GetJointIndices() and then loop the joints via MultibodyPlant::get_joint() to find the joint Joint::child_body() == collision_body, and then use Joint::parent_body(). And we can open an issue if avoiding that (small?) linear search becomes important?