MultibodyPlan 中的 ModelInstance 是否有唯一的 source_id 来确定 geometry_id 是否属于它？

如何解决MultibodyPlan 中的 ModelInstance 是否有唯一的 source_id 来确定 geometry_id 是否属于它？

我有一个 MultibodyPlant，里面有几个机器人，每个都是一个 ModelInstance，它们使用 AddModelInstance 添加到工厂中。在碰撞期间，我想确定碰撞对象（由 geometry_id 标记）属于这些模型实例中的哪一个。

我正在尝试类似的东西

// 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)};

如何在多体工厂中查询给定 geometry_id 对 ModelInstance 的所有权？我是否缺少一些为 source_id 提供 ModelInstance 的简单辅助函数？或者另一种查询所有权而不是 BelongsToSource 的方式？

解决方法

MultibodyPlant 具有唯一的 SourceId。因此，所有通过 MBP 注册的几何图形（例如，通过解析）都将与 plant 的 SourceId 相关联。但有好消息。当 MultibodyPlant 用 SceneGraph 注册框架和几何图形时，它存储框架的模型实例索引。 SceneGraph 称其为“框架组”。

因此，如果您的目标是将 GeometryId 映射到 ModelInstanceIndex，则必须执行以下操作：

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;