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

Question

我有一个 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 的所有权？我是否缺少一些简单的辅助函数，它为 ModelInstance 提供 source_id？或者另一种查询所有权的方式而不是 BelongsToSource?

Answer 1

一个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;

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

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

drake