Does a ModelInstance in a MultibodyPlan have a unique source_id for determining if a geometry_id belongs to it?

Question

I have a MultibodyPlant which has several robots in it, each of which is a ModelInstance and they are added to the plant using AddModelInstance. During collision, I would like to determine which of these model instances a colliding object (tagged by a geometry_id) belongs to.

I was trying something like

// register the model instance
const auto robot_model_index {
          Parser(robots_plant_, scene_graph_)
              .AddModelFromFile(entity->urdf, entity->name)};
/*
lots of code
*/
const auto& query_object {
      query_port.Eval<QueryObject<double>>(*plant_context_)};
const auto& inspector {query_object.inspector()};
// determine the source_id of a given model_instance
// does not work
const auto& my_robot_id {robot_model_index.get_source_id().value()};

// query ownership
bool belongs_to_my_robot {inspector.BelongsToSource(signed_distance_pair.id_A, robot_id)};

How can I query ownership of a given geometry_id to a ModelInstance within a multibody plant? Am I missing some easy helper function which gives a source_id for a ModelInstance? Or another way to query ownership rather than BelongsToSource?

Answer 1

A MultibodyPlant has a unique SourceId. So, all geometries registered through the MBP (e.g., via parsing), will be associated with the plant's SourceId. But there's good news. When MultibodyPlant registers frames and geometries with SceneGraph, it stores the model instance index with the frame. SceneGraph calls it the "frame group".

So, if your goal is to map GeometryId to ModelInstanceIndex, you'd have to do the following:

const ModelInstanceIndex robot_model_index =
    Parser(robots_plant_, scene_graph_)
        .AddModelFromFile(entity->urdf, entity->name);

// Lots of code.
const auto& query_object {
      query_port.Eval<QueryObject<double>>(*plant_context_)};
const auto& inspector {query_object.inspector()};
const SignedDistancePair<double>& signed_distance_pair = ...;

const FrameId frame_id =
    inspector.GetFrameId(signed_distance_pair .id_A);
// The frame group of a frame registered by MBP *is* its
// model instance index.
const ModelInstanceIndex geo_model_instance_index =
    inspector.GetFrameGroup(frame_id);
bool belongs_to_my_robot = geo_model_instance_index == robot_id;