drake - MultiBodyPlant 上下文中带有 nans 的额外主体在模拟时导致错误
问题描述
导入 urdf 文件并创建 MultiBodyPlant 后,我在打印上下文时看到一个带有nans的额外参数组。完整的印刷上下文可以在这里看到。我无法解释/理解的参数组是这个:
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 )。拓扑图像可以在这里看到。
在连续时间模拟(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 个伪链接,但 Context 中的第一个参数组仍然是 nans。所有其他参数组正确对应于 urdf 文件中的实体/关节。
这个错误可能出在 urdf 文件的编写方式上吗?
可以在此处找到 urdf 文件。用于打印上下文的代码:
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 可视化器或打印的上下文仿真后没有变化:
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)
因此,由于我无法从模型中解释的 nan,连续时间和离散时间模拟似乎都失败了(可能)。
解决方案
您在参数组中看到的NaN对应于世界主体,尽管它们看起来很可疑,但我不认为它们是您问题的根源。世界体没有一组有效的惯性参数(因此它们被设置为 NaN),并在代码中作为特殊情况处理。正如在当前实现中所写的那样,body 参数 API 对于每个 body 都无处不在。每个主体在上下文中都有一组参数,无论它们是否有效。这可能是特殊机构(“世界”)的争论点,所以我提出了一个问题来讨论。
您现在遇到的问题是因为您的base_main
链接(以及您的整个机器人)是一个自由浮动体。您不允许将零质量链接的自由浮动树与一个关节连接到另一个非零质量链接的自由浮动树,因为在连接它们的关节处施加的任何扭矩(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"))
推荐阅读
- python - 任何人都可以从 django 中找到为什么表单字段没有在模型中显示的错误
- typescript - 可选的泛型参数类型缩小
- reactjs - ReactJS:使用连接从两个单独的文件中分派动作
- javascript - 如何在haml的javascript代码中使用ruby代码?
- javascript - 为什么不应用内联样式?
- python - 使用 unicode 替换特殊字符
- arrays - 为什么每当我单独使用“if”语句以及在“if”中使用“if”时值会发生变化?
- c# - 关于在 Protobuf-net 中实现 VARIANT
- html - 嗨,我在 flexbox 中定位有问题
- reactjs - (React & Mobx & Leaflet)等待然后没有按我预期的那样工作