0

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

Gavin
  • 91
  • 1
  • 9
  • Workflow question: Why are you doing this via indexing rather than using distances and gradients? (is it b/c distances / gradients are ill-conditioned when trying to recover from "in-collision"?) – Eric Cousineau Jun 15 '21 at 14:24

1 Answers1

1

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?

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • Thank you, @Russ! I had `GetJointIndices` and was trying to find a way to get a `Joint` from the index but I didn't try searching in snake case. In general, is there a convention `drake` follows as to what kind of function names are typically in snake case? (I'll make this a separate question if you think it should be) – Gavin Jun 14 '21 at 16:32
  • 1
    the lower case is for very cheap functions... O(1) and less that 5 lines long https://drake.mit.edu/styleguide/cppguide.html#Function_Names – Russ Tedrake Jun 14 '21 at 19:29