direct搭配不尊重MinimumDistanceConstraint

Question

我使用这里给出的AddShape()函数的轻微变化为MultibodyPlant添加了Box障碍。我用WeldFrames()把障碍物焊到工厂上;当我想象障碍时，它也会出现在世界上正确的地方。

然而，当我添加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()

下面是direct搭配的相关代码片段。

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模型没有为其组件定义碰撞几何形状。似乎AutoDiff最适合球体碰撞，所以我用球体做了一个粗略的近似模型(例如:构建Box推车作为两个并排放置的球体，Cylinder杆作为许多微小的球体放在一条线上)，并将这些碰撞几何形状写在我的URDF中。

如果有兴趣的话，我使用一个小字符串生成脚本生成了这行微小的Spheres。

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中。

将球体碰撞几何图形添加到我的URDF中解决了我的问题，direct搭配开始尊重minimumdistanceconstrconstraint。