Drake：为什么 MultibodyPlant 中的同一个 link 有多个 geometryId？

Question

我有 6 个真实的 links（命名为左远端、左近端、右远端、右近端、手掌和球）+ 2 个虚拟 links（因此球可以自由移动在 x-y 平面中）在我的 URDF 中。我想知道哪个 geometryId 对应于哪个 link 以便我可以检查联系。但是，系统中注册的geometryId远远多于link个。因此，我尝试通过 geometryIds 打印出 body 名称，并发现多个具有相同 body 名称的 geometryIds。然后从 Drake 的 render_multibody_plant.ipynb 教程中我看到了这一行 geometry_label = inspector.GetPerceptionProperties(geometry_id).GetProperty("label", "id") 所以我为我的 geometryIds 打印了同样的内容。但是，其中一些 returns None。当我为那些不是 None 的人打印出 int(geometry_label) 时，我得到正好 6 个数字（但对于某些 links 是相同数字的倍数！）。我不明白那些额外的 geometry_ids 是做什么用的，也不知道如何找到我真正关心的 geometryIds。

相关代码如下：

builder = DiagramBuilder()
# plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.00001)
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.)
file_name = FindResource("models/myhand.urdf")
gripper_model = Parser(plant).AddModelFromFile(file_name)
file_name = FindResource("models/sphere.urdf")
object_model = Parser(plant).AddModelFromFile(file_name)
scene_graph_context = scene_graph.AllocateContext()
plant.Finalize()
plant.set_name('hand')


controller = ConstantVectorSource([-7, 7])
torque_system = builder.AddSystem(controller)
builder.Connect(torque_system.get_output_port(0), plant.get_actuation_input_port())

# Setup visualization
T_xy = np.array([[ 1., 0., 0., 0.], [ 0., 1., 0., 0.], [ 0., 0., 0., 1.]])
T_xz = np.array([[ 1., 0., 0., 0.], [ 0., 0., 1., 0.], [ 0., 0., 0., 1.]])
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, T_VW=T_xy, xlim=[-0.3, 0.3], ylim=[-0.3, 0.3], show=plt_is_interactive))

builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))


diagram = builder.Build()

diagram_context = diagram.CreateDefaultContext()
scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
plant_context = plant.GetMyContextFromRoot(diagram_context) 

# Set up a simulator to run this diagram
simulator = Simulator(diagram)
context = simulator.get_mutable_context()

# simulate from zero to duration
simulator.Initialize()


# Simulate
duration = 0.5 if get_ipython() else 0.1 # sets a shorter duration during testing
context.SetTime(0.0)

AdvanceToAndVisualize(simulator, visualizer, duration)

query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
inspector = query_object.inspector()


for geometry_id in inspector.GetAllGeometryIds():
    body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
    print(body.name())

    geometry_label = inspector.GetPerceptionProperties(
        geometry_id)
    if geometry_label != None:
        print(int(geometry_label.GetProperty("label", "id")))

打印语句的输出如下：

ball
ball
6
right_distal
right_distal
5
right_distal
5
left_distal
left_distal
4
left_distal
4
right_proximal
right_proximal
3
right_proximal
3
left_proximal
left_proximal
2
left_proximal
2
palm
palm
1

注意：运行 Jupyter Notebook 上的 PyDrake

Answer 1

我希望您的 body 中的每个视觉元素都有一个单独的几何 ID（不是每个 body 一个）。您的 URDF 在每个 body 中是否有多个视觉元素？

"Body frames and SceneGraph frames" documentation for MultibodyPlant 似乎有您要找的答案：

Given a GeometryId, SceneGraph cannot report what body it is affixed to. It can only report the SceneGraph alias frame F. But the following idiom can report the body:

const MultibodyPlant<T>& plant = ...;
const SceneGraphInspector<T>& inspector =  ...;
const GeometryId g_id = id_from_some_query;
const FrameId f_id = inspector.GetFrameId(g_id);
const Body<T>* body = plant.GetBodyFromFrameId(f_id);

See documentation of geometry::SceneGraphInspector on where to get an inspector.

Answer 2

具体来说，每个 link 都可以注册碰撞和视觉几何。即使这两个几何指定具有相同参数的形状，这些形状也不会合并为一个几何。因此，对于与 link.

关联的每个碰撞和视觉几何，您将获得一个 GeometryId

在您的 URDF 中声明为可视的那些将具有非 None PerceptionProperties（和 IllutrationProperties）。但是他们的 ProximityProperties 将是 None。相反，那些被声明为碰撞几何的那些将具有非None ProximityProperties 但对于其他两种类型将具有 None。

Drake：为什么 MultibodyPlant 中的同一个 link 有多个 geometryId？

Drake: Why are there multiple geometryId for the same link in a MultibodyPlant?

drake