AddMinimumDistanceConstraint with No Proximity Geometries Still Fails Inverse Kinematics

Question

I'm trying to include a self-collision avoidance constraint while solving IK. I'm using NLOPT as the solver, and IK without any MinimumDistanceConstraint works well. However, MinimumDistanceConstraint causes the same IK problem to fail. I've visually verified that the solution is feasible. Even when removing the Proximity role of all geometry bodies, IK still fails.

Does anyone have tips on debugging IK failures? What could be the issue here?

  string sdf_filepath = "my_robot.sdf";
  float timestep = 0.0;
  drake::systems::DiagramBuilder<double> builder;
  drake::multibody::MultibodyPlant<double>* plant{};
  drake::geometry::SceneGraph<double>* scene_graph{};
  std::tie(plant, scene_graph) = drake::multibody::AddMultibodyPlantSceneGraph(&builder, timestep);
  plant->set_name("plant");
  scene_graph->set_name("scene_graph");
  drake::multibody::Parser parser(plant, scene_graph);

  const auto robot_model_index = parser.AddModelFromFile(sdf_filepath, "robot");

  plant->Finalize();

  auto diagram = builder.Build();
  auto diagram_context= diagram->CreateDefaultContext();
  auto plant_context = &(diagram->GetMutableSubsystemContext(*plant,
  diagram_context.get()));

  auto all_geom_ids = inspector.GetAllGeometryIds();
  for (const auto& geom_id : all_geom_ids) {
    const auto source_id = inspector.GetOwningSourceId(geom_id);  // NOTE: custom function making a private function public 
    scene_graph->RemoveRole(source_id, geom_id,
        drake::geometry::Role::kProximity);
  }

  // Verified this is empty
  auto collision_pairs = inspector.GetCollisionCandidates();

  drake::multibody::InverseKinematics ik_solver(*plant, plant_context, with_joint_limits);
  float min_distance = 0.1;  // padding distance
  ik_solver.AddMinimumDistanceConstraint(min_distance);
  
  // Solve IK
  ..... // setting pos/ori constraints for EE, solver parameters
  solver_id = drake::solvers::NloptSolver::id();
  std::optional<drake::solvers::SolverId> solver_id_optional = solver_id;
  auto result = drake::solvers::Solve(ik_solver.prog(), solver_id_optional);  // custom change: force NLOPT to be used

  // solver_results.status is 4, but result.is_success() is False, ProgramAttributesSatisfied(ik_solver.prog) is True
  auto solver_results = result.get_solver_details<drake::solvers::NloptSolver>();  

Answer 1

My colleague Sean Curtis figured out the problem

The plant_context is constructed BEFORE removing the collision geometries. So this plant_context doesn't know that the collision geometries are removed, hence it still computes the distance for all pairs as if the proximity geometries were still there.

So there are two solutions:

1. Put the for loop with RemoveRole BEFORE creating the diagram_context
2. Alternatively, you can call RemoveRole(scene_graph_context,...) as in this function, so the code looks like

auto scene_graph_context = scene_graph.GetMyMutableContextFromRoot(diagram_context);
for (const auto& geom_id : all_geom_ids) {
    const auto source_id = inspector.GetOwningSourceId(geom_id);  // NOTE: custom function making a private function public 
    scene_graph->RemoveRole(scene_graph_context, source_id, geom_id,
        drake::geometry::Role::kProximity);
  }