`MultibodyPlant::GetCollisionGeometriesForBody` ignores collision geometries registered directly to SceneGraph

RobotLocomotion / drake

Model-based design and verification for robotics.

Other

3.19k stars 1.24k forks source link

What happened?

In writing a quick collision avoidance test, I found that the MultibodyPlant<T>::GetCollisionGeometriesForBody only reports the collision geometries that MultibodyPlant has intercepted: https://github.com/RobotLocomotion/drake/blob/a43526fdcb0a7c5893b717e4d5ce27e85d8d05db/multibody/plant/multibody_plant.cc#L774-L777

I was able to fix this by changing code from using plant.GetCollisionGeometriesForBody() to something like:

auto& inspector = scene_graph.model_inspector();
const auto frame_id = plant.GetBodyFrameIdOrThrow(body_index);
const std::vector<drake::geometry::GeometryId> geometries =
    inspector.GetGeometries(frame_id, drake::geometry::Role::kProximity);

Assigning @amcastro-tri per component chart; \cc @sherm1 @calderpg-tri

Version

293df0ffb23dfde4b66132bdeb9c2ccff78985ff

What operating system are you using?

Ubuntu 20.04

What installation option are you using?

compiled from source code using Bazel

Relevant log output

No response

This is a known problem. It has been tentatively tackled in the past (#13545). I even have an extant branch that tries to disentangle it. However, it is a big, ugly monster.

Generally, all code would be better off ignoring the MbP code for accessing collision geometries to directly query SG as you have done. This will make the code more portable in the future (when we finally get the will to untie this Gordian knot).

However, it's worth noting that you have a landmine here. If you're assigning geometry to the frame associated with an MbP body (without allowing MbP to "intercept" it), then if you actually run simulation and experience contact on that geometry, the simulation will crash. And the error message won't be overly helpful.

RobotLocomotion / drake