如何在多个SceneGraph实例中识别并查询同一Geometry？

问题核心 #

在逆运动学并行化开发中，每个SceneGraph实例维护独立的GeometryId空间，无法像BodyIndex那样跨实例直接追踪同一几何结构，需要找到可靠的关联方式，同时明确两个疑问：

• SceneGraph是否能识别不同实例中不同GeometryId对应同一几何？
• GetAllGeometryIds()的返回顺序是否稳定可靠？

可行解决方案 #

1. 基于「BodyIndex + 几何名称」的关联方案 #

由于BodyIndex在不同MultibodyPlant实例中是稳定的（同一模型的同一刚体始终对应相同BodyIndex），而每个几何都挂载在特定刚体的Frame上，且几何名称在解析同一模型时保持一致，因此可以通过「BodyIndex + 几何名称」的组合来关联不同SceneGraph中的同一几何。

具体实现步骤：

1. 先创建一个基准实例，提前记录每个BodyIndex对应的几何名称列表；
2. 对每个新的SceneGraph实例，通过BodyIndex找到对应刚体，获取其FrameId；
3. 查询该Frame下的所有几何，再通过名称匹配找到目标GeometryId。

示例代码：

from pydrake.all import (
    DiagramBuilder, Parser, AddMultibodyPlantSceneGraph
)

# 建立基准映射：BodyIndex -> 对应几何名称列表
builder_ref = DiagramBuilder()
plant_ref, scene_graph_ref = AddMultibodyPlantSceneGraph(builder_ref, 0.0)
Parser(plant=plant_ref).AddModelsFromUrl(
    "package://drake/examples/multibody/cart_pole/cart_pole.sdf"
)
plant_ref.Finalize()
inspector_ref = scene_graph_ref.model_inspector()

body_to_geometries = {}
for body_idx in plant_ref.GetBodyIndices():
    body = plant_ref.get_body(body_idx)
    frame_id = body.body_frame()
    geom_ids = inspector_ref.GetGeometriesOnFrame(frame_id)
    body_to_geometries[body_idx] = [inspector_ref.GetName(gid) for gid in geom_ids]

# 处理多个SceneGraph实例，关联同一几何
for _ in range(3):
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
    Parser(plant=plant).AddModelsFromUrl(
        "package://drake/examples/multibody/cart_pole/cart_pole.sdf"
    )
    plant.Finalize()
    inspector = scene_graph.model_inspector()

    print("当前实例几何关联结果：")
    for body_idx, target_names in body_to_geometries.items():
        body = plant.get_body(body_idx)
        frame_id = body.body_frame()
        current_geom_ids = inspector.GetGeometriesOnFrame(frame_id)
        # 匹配名称找到对应GeometryId
        for name in target_names:
            for gid in current_geom_ids:
                if inspector.GetName(gid) == name:
                    print(f"Body {body_idx} 下的几何 {name} → GeometryId: {gid}")
    print('----')

2. 关于GetAllGeometryIds()顺序的可靠性 #

绝对不要依赖这个顺序。虽然在当前测试环境下，解析同一模型时返回顺序可能暂时相同，但Drake的内部GeometryId分配逻辑并未保证顺序的稳定性——后续版本更新、模型定义的微小调整（比如几何条目顺序修改）都可能打破这个顺序，属于未定义行为，生产代码中禁止依赖。

3. SceneGraph是否能跨实例关联GeometryId？ #

不能。每个SceneGraph实例是完全独立的运行实体，它们的GeometryId空间相互隔离，Drake没有内置机制让不同实例识别这些Id属于同一几何结构，必须通过手动建立外部映射来实现关联。

内容的提问来源于stack exchange，提问作者H.J. Terry Suh