The API for the SceneGraphCollisionChecker seems to indicate that it can make permanent changes/additions to the collision filter of the scene graph. Is this assumption correct?
I have a case where I manually add a collision body to the plant that collides with another body in a non-adjacent link. This collision is not added to the filter by MultibodyPlant::Finalize because the colliding bodies are not in adjacent links. I can use the SceneGraphCollisionChecker to identify and filter any "default" collisions. However, subsequent calls to a MathematicalProgram that uses the MinimumDistanceConstraint fail due to infeasible constraints (drake::multibody::MinimumDistanceConstraint[0]: -inf <= 3.000000 <= 1.000000), presumably because the scene graph is not actually filtering the collision pair. If I manually add the filter declaration to the scene graph, the constraint now becomes feasible.
Am I doing something incorrectly here, or does the SceneGraphCollisionChecker not actually permanently filter collisions in the scene graph? For reference, I'm working off of version 1.19.0.
resource_file = 'drake/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf'
rdb = RobotDiagramBuilder(time_step=0)
resource = FindResourceOrThrow(resource_file)
model = rdb.parser().AddModelFromFile(resource)
n_pos = rdb.plant().num_positions()
# Add a body to the plant with collision geometry that collides by default
...
# Finalize the plant
plant.Finalize()
# Build the diagram
diagram = rdb.Build()
# Check for possible collisions and add those collisions to the filter
sgcc = SceneGraphCollisionChecker(model=diagram, robot_model_instances=[model],
configuration_distance_function=l2_dist, edge_step_size=0.01,
env_collision_padding=0.0, self_collision_padding=0.0)
nominal_state = np.zeros((n_pos,))
# Check collisions
collision_free = sgcc.CheckConfigCollisionFree(nominal_state) # -> reports False
clearance = sgcc.CalcRobotClearance(nominal_state, 0.0)
for idx in np.argwhere(clearance.distances() < 0.0).flatten():
body_1 = sgcc.plant().get_body(clearance.robot_indices()[idx])
body_2 = sgcc.plant().get_body(clearance.other_indices()[idx])
dist = clearance.distances()[idx]
print(f'Collision between \'{body_1.name()}\' and \'{body_2.name()}\' with distance {dist}')
sgcc.SetCollisionFilteredBetween(body_1, body_2, True)
# Check collisions
collision_free = sgcc.CheckConfigCollisionFree(nominal_state) # -> reports True
# Solve IK optimization with min distance constraint -> fails due to an infeasible minimum distance constraint
...
g1 = diagram.plant().CollectRegisteredGeometries([diagram.plant().GetBodyByName('body_a')])
g2 = diagram.plant().CollectRegisteredGeometries([diagram.plant().GetBodyByName('body_b')])
decl = CollisionFilterDeclaration()
decl.ExcludeBetween(g1, g2)
diagram.scene_graph().collision_filter_manager().Apply(decl)
# Solve IK optimization with min distance constraint -> no longer infeasible and succeeds
Expanding on the code I am using to solve IK. My assumption is that the scene graph object itself is holding the new collision filters
# Solve IK optimization with min distance constraint
plant_context = diagram.plant().GetMyContextFromRoot(diagram.CreateDefaultContext())
ik = InverseKinematics(diagram.plant(), plant_context)
# Add constraints
ik.AddMinimumDistanceConstraint(0.010, 0.1)
...
result = Solve(ik.prog(), initial_guess=...) # -> fails due to an infeasible minimum distance constraint
... MathematicalProgram that uses the MinimumDistanceConstraint fail ...
I don't see the sample code for this?
I have a guess at the problem, though.
The CollisionChecker filtering API only configures its owned contexts; it does not configure the SceneGraph's model value filtering.
To use MinimumDistanceConstraint with a CollisionChecker, you need to be sure to pass it plant_context that has been given to you by the CollisionChecker. You can't make a new context and expect to match the checker.