时间戳

首页 > 解决方案 > Drake：为什么 MultibodyPlant 中的同一个链接有多个 geometryId？

drake - Drake：为什么 MultibodyPlant 中的同一个链接有多个 geometryId？

问题描述

我的 URDF 中有 6 个真实链接（命名为左远端、左近端、右远端、右近端、手掌和球）+ 2 个虚拟链接（因此球可以在 xy 平面上自由移动）。我想知道哪个 geometryId 对应于哪个链接，以便我可以检查联系。但是，注册的geometryId 比系统中的链接多得多。于是我尝试通过geometryIds打印出body名称，发现多个geometryIds的body名称相同。然后从 Drake 的 render_multibody_plant.ipynb 教程中我看到了这条线geometry_label = inspector.GetPerceptionProperties(geometry_id).GetProperty("label", "id")，所以我为我的 geometryIds 打印了相同的内容。但是，其中一些返回None. 当我打印出int(geometry_label)那些不是None，我得到了 6 个数字（但某些链接的数字是相同数字的倍数！）。我不明白那些额外的 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

标签： drake

解决方案

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

MultibodyPlant的“Body frames and SceneGraph frames”文档似乎有您正在寻找的答案：

给定一个 GeometryId，SceneGraph 无法报告它附加到哪个主体。只能报SceneGraph别名帧F。但是下面的成语可以报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);

请参阅geometry::SceneGraphInspector 的文档，了解从何处获得检查器。

推荐阅读