drake - MultiBodyPlant 环境中带有 nan 的额外主体在仿真时导致错误

Question

导入 urdf 文件并创建 MultiBodyPlant 后，当我打印上下文时，我看到一个带有 nans 的额外参数组。完整的打印上下文可以看到 here 。我无法解释/理解的参数组是这个:

18 numeric parameter groups with
   10 parameters
     nan nan nan nan nan nan nan nan nan nan

如果我使用以下方法获取工厂拓扑: pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].write_svg("robot_topology.svg")

在制作 urdf 时，我看到了植物的拓扑结构(在 WorldBodyInstance 内有一个额外的 WorldBody)。拓扑图可见here .

这个额外的主体似乎在连续时间模拟 (time_step=0.0) 期间导致以下错误:

运行时错误:主体节点索引 1 遇到奇异的铰接主体铰链惯性。请确保该主体沿所有运动轴具有非零惯性。

我尝试删除前 3 个伪链接，但上下文中的第一个参数组仍然是 nan。所有其他参数组都正确对应于 urdf 文件中的主体/关节。

这个错误可能是 urdf 文件的写入方式造成的吗？

可以找到urdf文件here 。 用于打印上下文的代码:

builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(SCpath)
# Remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()

# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
print(plant_context)

如果我运行离散时间仿真 (time_step=0.001)，则仿真会成功运行，但在对 base_roll 施加关节扭矩后，我发现 meshcat 可视化工具或打印的上下文后仿真没有任何变化/em> 使用以下行联合:

jointAcutation =  np.zeros((plant.num_actuators(),1))
jointAcutation[0] = 10
plant.get_actuation_input_port().FixValue(plant_context, jointAcutation)

simulator = Simulator(diagram, context)

meshcat.load()

meshcat.start_recording()
simulator.AdvanceTo(30.0)
meshcat.stop_recording()
meshcat.publish_recording()
print(plant_context)

因此，连续时间和离散时间模拟似乎(可能)由于我无法从模型中解释的纳米值而失败。

Answer 1

您在参数组中看到的 NaN 对应于世界体，尽管它们看起来很可疑，但我不认为它们是问题的根源。世界体没有一组有效的惯性参数(因此它们被设置为 NaN)，并在代码中作为特殊情况进行处理。正如当前实现中所写的那样，主体参数 API 对于每个主体来说都是普遍存在的。每个主体在上下文中都有一组参数，无论它们是否有效。这可能是特殊机构(“世界”)的争论点，所以我打开了 an issue进行讨论。

您现在遇到的问题是因为您的 base_main 链接(以及您的整个机器人)是一个自由 float 的主体。不允许将零质量链接的自由 float 树通过关节连接到另一个具有非零质量的自由 float 链接树，因为在连接它们的关节处施加了任何扭矩(Joint_base_yaw你的情况)会导致内侧物体无限加速。要解决这个问题:

• (选项 1):在 URDF 文件中的 base_main 和 world 之间添加固定关节。

  <joint name="base_main" type="fixed">
    <parent link="world"/>
    <child link="base_main"/>
  </joint>
  

• (选项 2):将 base_main 链接的主体框架焊接到代码中的世界框架。

  plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_main"))