CODEWITHSUNDEEP
drake
radhika
radhika

Reputation: 85

DirectCollocation not respecting MinimumDistanceConstraint

I've added a Box obstacle to my MultibodyPlant using a slight variation of the AddShape() function given here. I've welded the obstacle to the plant using WeldFrames(); the obstacle also shows up in the correct place in the world when I visualize it.

However, DirectCollocation doesn't respect the obstacle when I add a MinimumDistanceConstraint, and produces a trajectory which collides with the obstacle.

Here's how I create my plant, scene_graph, and 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()

Here's the relevant code snippet for 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])

Am I passing in the wrong context to MinimumDistanceConstraint / misunderstood what MinimumDistanceConstraint does?

Upvotes: 0

Views: 89

Answers (1)

radhika
radhika

Reputation: 85

So turns out the cartpole URDF model I was using didn't have collision geometries defined for its components. It seems AutoDiff works best with Sphere collisions, so I made a rough approximation of my model with Spheres (eg: constructed the Box cart as two Spheres placed side by side, the Cylinder pole as many tiny Spheres placed in a line), and wrote those collision geometries out to my URDF.

If interested, I generated the line of tiny Spheres using a small string generation script.

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))

This prints out:

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

Which I then pasted into my URDF.

Adding the Sphere collision geometries into my URDF solved my problem and DirectCollocation started respecting the MinimumDistanceConstraint.

Upvotes: 2

Related Questions