drake - 如何实现离散多体仿真的关节限制

Question

我正在使用 pydrake 实现双连杆杂技机器人模拟，并希望强制执行关节限制以防止下连杆与上连杆碰撞。我已将关节限制添加到 URDF 并解析此 URDF 以生成 acrobot multibodyplant 对象。我已经使用函数成功验证了关节限制是否被解析(position_lower_limits()、position_upper_limits())，但我的模拟没有响应这些限制。我真的很感激一些帮助来确定为什么会这样。

我正在创建和模拟 acrobot multibodyplant 如下:

plt_is_interactive = SetupMatplotlibBackend()
builder = DiagramBuilder()

# instantiate the pendulum and the scene graph
double_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
    builder,
    time_step=0.01 # discrete update period 
)

# parse the urdf 
urdf_path = 'double_pendulum_no_shoulder.urdf'
Parser(double_pendulum).AddModelFromFile(urdf_path)
double_pendulum.Finalize()

#Simulation Set-Up

# Set the constant torque 
torque_system = builder.AddSystem(ConstantVectorSource([[0.]]))
builder.Connect(torque_system.get_output_port(0), double_pendulum.get_input_port(3))
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, xlim=[-10., 10.], ylim=[-10., 10.], show=plt_is_interactive))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))

diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(double_pendulum, context)
double_pendulum.SetPositionsAndVelocities(plant_context, [2, 0, 0, 0])

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

AdvanceToAndVisualize(simulator, visualizer, duration)

我的 URDF 在关联的关节中包含以下行以尝试强制执行关节限制。

<limit lower="-2.35" upper="2.35"/>

Answer 1

我刚刚尝试重现，但也很惊讶它似乎不起作用。
这是一个可运行的复制品，它违反了肘部的关节下限，该值设置为 0.0。

它还打印出关节的限制，确认解析有效。 https://www.dropbox.com/s/2m12ws0g88t5uei/joint_limits_test.py?dl=0 https://www.dropbox.com/s/fdjb4uv4m2i17i8/double_pendulum.urdf?dl=0