DirectCollocation 不遵守 MinimumDistanceConstraint

Question

我使用给定 here. I've welded the obstacle to the plant using WeldFrames() 的 AddShape() 函数的细微变化为我的 MultibodyPlant 添加了一个 Box 障碍；当我想象它时，障碍物也会出现在世界上正确的位置。

然而，当我添加 MinimumDistanceConstraint 时，DirectCollocation 不尊重障碍物，并产生与障碍物相撞的轨迹。

以下是我创建 plant、scene_graph 和 diagram 的方法。

builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(plant).AddModelFromFile(urdf_path)

# add obstacles
# this function internally calls RegisterCollisionGeometry and 
# RegisterVisualGeometry on the plant
plant = add_obstacles_plant(plant, obstacles)

plant.Finalize()
diagram = builder.Build()

这是 DirectCollocation 的相关代码片段。

input_port_index = builder.ExportInput(plant.get_actuation_input_port(), "input")

dircol = DirectCollocation(
    diagram, diagram.CreateDefaultContext(),
    num_time_samples=num_time_samples,
    minimum_timestep=0.1,
    maximum_timestep=0.4,
    input_port_index=input_port_index
)

# collision avoidance constraints
collision_constraint = MinimumDistanceConstraint(plant, 0.1, 
        diagram.GetMutableSubsystemContext(plant, diagram_context))
dircol.AddConstraintToAllKnotPoints(constraint=collision_constraint, vars=dircol.state()[:2])

我是否将错误的上下文传递给 MinimumDistanceConstraint/误解了 MinimumDistanceConstraint 的作用？

Answer 1

原来我使用的 cartpole URDF model 没有为其组件定义碰撞几何。 AutoDiff 似乎最适合 Sphere 碰撞，所以我用 Spheres 对我的模型进行了粗略近似（例如：将 Box 购物车构建为并排放置的两个 Spheres，Cylinder 杆与许多小 Spheres排成一行），并将这些碰撞几何体写到我的 URDF 中。

如果有兴趣，我使用小型字符串生成脚本生成了一行微小的球体。

sphere_str = '<collision> <origin xyz="0 0 {s_o:.2f}"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>'
for i in range(50):
    print(col_str.format(s_o=-0.01 + i*-0.02))

打印出来：

<collision> <origin xyz="0 0 -0.01"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
<collision> <origin xyz="0 0 -0.03"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
<collision> <origin xyz="0 0 -0.05"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>
...
<collision> <origin xyz="0 0 -0.99"/> <geometry> <sphere radius="0.01"/> </geometry> </collision>

然后我将其粘贴到我的 URDF 中。

将 Sphere 碰撞几何体添加到我的 URDF 中解决了我的问题，DirectCollocation 开始遵守 MinimumDistanceConstraint。

DirectCollocation 不遵守 MinimumDistanceConstraint

DirectCollocation not respecting MinimumDistanceConstraint

drake