几何角色变更问题：移除Proximity角色时源ID未注册报错

1. 类型混淆导致筛选错误
   你错误地将ModelInspector::GetFrameGroup()返回的GroupId直接与ModelInstanceIndex比较。两者是不同类型的标识符，即使数值偶然匹配，也会导致逻辑错误，可能选中不属于目标机器人模型的几何，甚至是不属于MultibodyPlant的几何（这类几何的source id可能为0，而source 0未注册，触发报错）。

2. 不必要的手动注册操作
   调用plant->RegisterAsSourceForSceneGraph(scene_graph)返回错误是正常现象——AddMultibodyPlantSceneGraph函数已经自动完成了MultibodyPlant与SceneGraph的注册关联，无需手动重复注册。

修正几何筛选逻辑，确保只处理目标机器人模型下的几何，具体代码修改如下：

drake::systems::DiagramBuilder<double> builder;
drake::multibody::MultibodyPlant<double>* plant{};
drake::geometry::SceneGraph<double>* scene_graph{};
std::tie(plant, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, timestep);

drake::multibody::Parser parser(plant, scene_graph);
const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");

plant->Finalize();
const auto source_id = plant->get_source_id().value();
const auto& inspector = scene_graph->model_inspector();

// 正确获取目标模型下的所有Frame
const auto& model_frames = plant->GetModelInstanceFrames(robot_model_index);
for (const auto& frame : model_frames) {
    // 获取该Frame对应的SceneGraph FrameId
    const auto frame_id = plant->GetBodyFrameIdOrThrow(frame.body().index());
    // 获取该Frame下的所有几何ID
    const auto geom_ids = inspector.GetGeometries(frame_id);
    for (const auto geom_id : geom_ids) {
        // 确认几何属于MultibodyPlant的source（可选，但更安全）
        if (inspector.GetSourceId(geom_id) == source_id) {
            scene_graph->RemoveRole(source_id, geom_id, drake::geometry::Role::kProximity);
        }
    }
}

关键修正点 #

• 通过plant->GetModelInstanceFrames()直接获取目标模型的所有Frame，避免类型混淆问题。
• 利用plant->GetBodyFrameIdOrThrow()将MultibodyPlant的Frame映射到SceneGraph的FrameId，确保几何归属正确。
• 可选地通过inspector.GetSourceId(geom_id)验证几何所属source，进一步避免错误操作。

内容的提问来源于stack exchange，提问作者alvin