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

EricCousineau-TRI commented 2 years ago

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

SeanCurtis-TRI commented 2 years ago

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.

jwnimmer-tri commented 2 weeks ago

This is working as intended.

RobotLocomotion / drake