MultiBodyPlant 上下文中带有 nans 的额外 body 导致模拟时出错

Question

在导入一个 urdf 文件并创建一个 MultiBodyPlant 之后，当我打印上下文时，我看到一个带有 nans 的额外参数组。可以看到完整的打印上下文 here。我不能explain/understand的参数组是这个：

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.

这个额外的 body 似乎在 continuous-time 模拟 (time_step=0.0) 期间导致以下错误：

RuntimeError: Encountered singular articulated body hinge inertia for body node index 1. Please ensure that this body has non-zero inertia along all axes of motion.

我尝试删除前 3 个 pseudo-links 但是上下文中的第一个参数组仍然是 nans。所有其他参数组都正确对应于 urdf 文件中的 bodies/joints。

这个错误会不会是 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)

如果我运行一个 discrete-time 模拟 (time_step=0.001)，那么模拟运行成功，但我在 meshcat 可视化器或使用以下行向 base_roll 关节应用关节扭矩后打印上下文 post-simulation：

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)

因此，continuous-time 和 discrete-time 模拟似乎都失败了（可能）是由于我无法从模型中解释的 nans。

Answer 1

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

您现在遇到的问题是因为您的 base_main link（以及您的整个机器人）是一个自由漂浮的物体。您不允许将一棵零质量 links 的自由漂浮树通过接头连接到另一棵 links 的非零质量自由漂浮树，因为在连接它们的接头处施加了任何扭矩（Joint_base_yaw 在你的情况下）会导致内侧物体无限加速。解决这个问题：

（选项 1）：在您的 URDF 文件中的 base_main 和 world 之间添加固定关节。

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

（方案2）：将base_mainlink的body frame焊接到代码中的world frame。
```
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_main"))
```

MultiBodyPlant 上下文中带有 nans 的额外 body 导致模拟时出错

Extra body with nans in Context of a MultiBodyPlant causing Error While Simulation

drake