Allow the user to specify the solver to be used by IrisNp2 for the counterexample search program. (#23101)

Also warn the user if the solver is failing frequently, and suggest using IpoptSolver in this case.

Also change the convex configuration space test to use IPOPT instead of SNOPT for speed reasons.

Author: cohnt

bindings/pydrake/planning/planning_py_iris_np2.cc

@@ -31,6 +31,9 @@ void DefinePlanningIrisNp2(py::module m) {
             .format(self.sampled_iris_options);
       });
 
+  DefReadWriteKeepAlive(
+      &iris_np2_options, "solver", &IrisNp2Options::solver, cls_doc.solver.doc);
+
   // The `options` contains a `Parallelism`; we must release the GIL.
   m.def("IrisNp2", &IrisNp2, py::arg("checker"), py::arg("starting_ellipsoid"),
       py::arg("domain"), py::arg("options") = IrisNp2Options(),

bindings/pydrake/planning/test/iris_test.py

@@ -11,6 +11,7 @@
     CollisionCheckerParams,
     IrisParameterizationFunction,
 )
+from pydrake.solvers import IpoptSolver
 from pydrake.symbolic import Variable
 
 import numpy as np
@@ -181,6 +182,9 @@ def test_iris_np2(self):
         # Feature still TODO.
         options.sampled_iris_options.containment_points = None
 
+        # For speed reasons -- IPOPT seems to be faster than SNOPT here.
+        options.solver = IpoptSolver()
+
         domain = HPolyhedron.MakeBox(plant.GetPositionLowerLimits(),
                                      plant.GetPositionUpperLimits())
         starting_ellipsoid = Hyperellipsoid.MakeHypersphere(0.01,

planning/iris/iris_np2.cc

@@ -35,6 +35,7 @@ using geometry::optimization::internal::IrisConvexSetMaker;
 using geometry::optimization::internal::SamePointConstraint;
 using math::RigidTransform;
 using multibody::MultibodyPlant;
+using solvers::SolverInterface;
 using systems::Context;
 
 namespace {
@@ -232,9 +233,15 @@ HPolyhedron IrisNp2(const SceneGraphCollisionChecker& checker,
   VectorXd closest(nq);
   RandomGenerator generator(options.sampled_iris_options.random_seed);
 
-  // TODO(cohnt): Allow the user to specify a solver.
-  auto solver = solvers::MakeFirstAvailableSolver(
-      {solvers::SnoptSolver::id(), solvers::IpoptSolver::id()});
+  const SolverInterface* solver;
+  std::unique_ptr<SolverInterface> default_solver =
+      solvers::MakeFirstAvailableSolver(
+          {solvers::SnoptSolver::id(), solvers::IpoptSolver::id()});
+  if (options.solver) {
+    solver = options.solver;
+  } else {
+    solver = default_solver.get();
+  }
 
   // For debugging visualization.
   Vector3d point_to_draw = Vector3d::Zero();
@@ -363,6 +370,9 @@ HPolyhedron IrisNp2(const SceneGraphCollisionChecker& checker,
       }
 
       int num_hyperplanes_added = 0;
+      int num_prog_failures = 0;
+      int num_prog_successes = 0;
+      constexpr double kSolverFailRateWarning = 0.1;
 
       for (const auto& particle : particles_in_collision) {
         if (num_hyperplanes_added >
@@ -409,7 +419,9 @@ HPolyhedron IrisNp2(const SceneGraphCollisionChecker& checker,
         }
 
         // TODO(cohnt): Allow the user to specify the solver options used here.
-        if (prog.Solve(*solver, particle, {}, &closest)) {
+        bool solve_succeeded = prog.Solve(*solver, particle, {}, &closest);
+        if (solve_succeeded) {
+          ++num_prog_successes;
           if (do_debugging_visualization) {
             point_to_draw.head(nq) = closest;
             std::string path = fmt::format("iteration{:02}/{:03}/found",
@@ -436,6 +448,7 @@ HPolyhedron IrisNp2(const SceneGraphCollisionChecker& checker,
           prog.UpdatePolytope(A.topRows(num_constraints),
                               b.head(num_constraints));
         } else {
+          ++num_prog_failures;
           if (do_debugging_visualization) {
             point_to_draw.head(nq) = closest;
             std::string path = fmt::format("iteration{:02}/{:03}/closest",
@@ -448,6 +461,18 @@ HPolyhedron IrisNp2(const SceneGraphCollisionChecker& checker,
         }
       }
 
+      const double failure_rate =
+          static_cast<double>(num_prog_failures) /
+          static_cast<double>(num_prog_failures + num_prog_successes);
+      if (failure_rate >= kSolverFailRateWarning) {
+        log()->warn(fmt::format(
+            "IrisNp2 WARNING, only {} out of {} closest collision "
+            "programs solved successfully ({}% failure rate). If you are using "
+            "SnoptSolver or NloptSolver, consider using IpoptSolver instead.",
+            num_prog_successes, num_prog_successes + num_prog_failures,
+            failure_rate));
+      }
+
       ++num_iterations_separating_planes;
     }
 

planning/iris/iris_np2.h

@@ -5,6 +5,7 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/planning/iris/iris_common.h"
 #include "drake/planning/scene_graph_collision_checker.h"
+#include "drake/solvers/solve.h"
 
 namespace drake {
 namespace planning {
@@ -28,6 +29,11 @@ class IrisNp2Options {
 
   IrisNp2Options() = default;
 
+  /** The user can specify a solver to use for the counterexample search
+   * program. If nullptr (the default value) is given, then
+   * solvers::MakeFirstAvailableSolver will be used to pick the solver. */
+  const solvers::SolverInterface* solver{nullptr};
+
   /** Options common to IRIS-type algorithms. */
   CommonSampledIrisOptions sampled_iris_options{};
 

planning/iris/test/iris_np2_test.cc

@@ -6,6 +6,9 @@
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/planning/iris/iris_common.h"
 #include "drake/planning/iris/test/iris_test_utilities.h"
+#include "drake/solvers/equality_constrained_qp_solver.h"
+#include "drake/solvers/ipopt_solver.h"
+#include "drake/solvers/nlopt_solver.h"
 
 namespace drake {
 namespace planning {
@@ -126,6 +129,22 @@ TEST_F(DoublePendulum, FilterCollisions) {
   PlotEnvironmentAndRegion(region);
 }
 
+// Verify we can specify the solver for the counterexample search by
+// deliberately specifying a solver that can't solve problems of that type, and
+// catch the error message.
+TEST_F(DoublePendulum, SpecifySolver) {
+  IrisNp2Options options;
+  auto sgcc_ptr = dynamic_cast<SceneGraphCollisionChecker*>(checker_.get());
+  ASSERT_TRUE(sgcc_ptr != nullptr);
+
+  solvers::EqualityConstrainedQPSolver invalid_solver;
+  options.solver = &invalid_solver;
+
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      IrisNp2(*sgcc_ptr, starting_ellipsoid_, domain_, options),
+      ".*EqualityConstrainedQPSolver is unable to solve.*");
+}
+
 TEST_F(BlockOnGround, IrisNp2Test) {
   IrisNp2Options options;
   auto sgcc_ptr = dynamic_cast<SceneGraphCollisionChecker*>(checker_.get());
@@ -156,6 +175,12 @@ TEST_F(ConvexConfigurationSpace, IrisNp2Test) {
   meshcat_->Delete();
   options.sampled_iris_options.meshcat = meshcat_;
   options.sampled_iris_options.verbose = true;
+
+  // We use IPOPT for this test since SNOPT has a large number of solve failures
+  // in this environment.
+  solvers::IpoptSolver solver;
+  options.solver = &solver;
+
   HPolyhedron region =
       IrisNp2(*sgcc_ptr, starting_ellipsoid_, domain_, options);
   CheckRegion(region);