Iris from clique cover allows the use of iris variants

bindings/pydrake/planning/planning_py.cc

@@ -30,10 +30,11 @@ and/or trajectories of dynamical systems.
   internal::DefinePlanningGraphAlgorithms(m);
   internal::DefinePlanningTrajectoryOptimization(m);
   internal::DefinePlanningVisibilityGraph(m);
-  internal::DefinePlanningIrisFromCliqueCover(m);
   internal::DefinePlanningIrisCommon(m);
   internal::DefinePlanningIrisNp2(m);
   internal::DefinePlanningIrisZo(m);
+  internal::DefinePlanningIrisFromCliqueCover(
+      m); /* IrisZo and IrisNp2 must be declared before these bindings. */
   internal::DefinePlanningZmpPlanner(m);
 }
 

bindings/pydrake/planning/test/iris_from_clique_cover_test.py

@@ -6,8 +6,11 @@
     RobotDiagramBuilder,
     SceneGraphCollisionChecker,
     CollisionCheckerParams,
+    IrisNp2Options,
+    IrisZoOptions,
 )
 from pydrake.solvers import MosekSolver, GurobiSolver, SnoptSolver
+from pydrake.geometry.optimization import IrisOptions
 
 import textwrap
 import numpy as np
@@ -17,12 +20,12 @@
 
 def _snopt_and_mip_solver_available():
     mip_solver_available = (
-       MosekSolver().available() and MosekSolver().enabled() or (
-        GurobiSolver().available() and GurobiSolver().enabled()
-        )
+        MosekSolver().available()
+        and MosekSolver().enabled()
+        or (GurobiSolver().available() and GurobiSolver().enabled())
     )
     snopt_solver_available = (
-            SnoptSolver().available() and SnoptSolver().enabled()
+        SnoptSolver().available() and SnoptSolver().enabled()
     )
     return mip_solver_available and snopt_solver_available
 
@@ -63,13 +66,13 @@ def _make_scene_graph_collision_checker(self, use_provider, use_function):
         )
 
         if use_provider:
-            checker_kwargs[
-                "distance_and_interpolation_provider"
-            ] = mut.LinearDistanceAndInterpolationProvider(plant)
+            checker_kwargs["distance_and_interpolation_provider"] = (
+                mut.LinearDistanceAndInterpolationProvider(plant)
+            )
         if use_function:
-            checker_kwargs[
-                "configuration_distance_function"
-            ] = self._configuration_distance
+            checker_kwargs["configuration_distance_function"] = (
+                self._configuration_distance
+            )
 
         return mut.SceneGraphCollisionChecker(**checker_kwargs)
 
@@ -96,6 +99,22 @@ def test_iris_in_configuration_space_from_clique_cover_options(self):
         options.point_in_set_tol = 1e-5
         self.assertEqual(options.point_in_set_tol, 1e-5)
 
+        self.assertIsInstance(options.iris_options, IrisOptions)
+
+        options.iris_options = IrisZoOptions()
+        self.assertIsInstance(options.iris_options, IrisZoOptions)
+        options.iris_options.sampled_iris_options.max_iterations = 2
+        self.assertEqual(
+            options.iris_options.sampled_iris_options.max_iterations, 2
+        )
+
+        options.iris_options = IrisNp2Options()
+        self.assertIsInstance(options.iris_options, IrisNp2Options)
+        options.iris_options.sampled_iris_options.max_iterations = 1
+        self.assertEqual(
+            options.iris_options.sampled_iris_options.max_iterations, 1
+        )
+
     # IPOPT performs poorly on this test. We also need a MIP solver to
     # be available.  Hence only run this test if both SNOPT and a MIP solver
     # are available.

planning/iris/BUILD.bazel

@@ -32,6 +32,8 @@ drake_cc_library(
         "//planning:visibility_graph",
         "//planning/graph_algorithms:max_clique_solver_base",
         "//planning/graph_algorithms:max_clique_solver_via_greedy",
+        "//planning/iris:iris_np2",
+        "//planning/iris:iris_zo",
         "//solvers:gurobi_solver",
         "//solvers:mosek_solver",
     ],

planning/iris/iris_from_clique_cover.cc

@@ -183,15 +183,28 @@ void ComputeGreedyTruncatedCliqueCover(
 std::queue<HPolyhedron> IrisWorker(
     const CollisionChecker& checker,
     const Eigen::Ref<const Eigen::MatrixXd>& points, const int builder_id,
-    const IrisFromCliqueCoverOptions& options,
+    const IrisFromCliqueCoverOptions& options, const HPolyhedron& domain,
     AsyncQueue<VectorX<bool>>* computed_cliques, bool disable_meshcat = true) {
   // Copy the IrisOptions as we will change the value of the starting ellipse
   // in this worker.
-  IrisOptions iris_options = options.iris_options;
+  std::variant<geometry::optimization::IrisOptions, IrisNp2Options,
+               IrisZoOptions>
+      iris_options = options.iris_options;
+
   // Disable the IRIS meshcat option in this worker since we cannot write to
   // meshcat from a different thread.
   if (disable_meshcat) {
-    iris_options.meshcat = nullptr;
+    std::visit(
+        [](auto&& arg) {
+          using T = std::decay_t<decltype(arg)>;
+          if constexpr (std::is_same_v<T,
+                                       geometry::optimization::IrisOptions>) {
+            arg.meshcat = nullptr;
+          } else {
+            arg.sampled_iris_options.meshcat = nullptr;
+          }
+        },
+        iris_options);
   }
 
   std::queue<HPolyhedron> ret{};
@@ -218,29 +231,40 @@ std::queue<HPolyhedron> IrisWorker(
       current_clique = computed_cliques->pop();
       continue;
     }
-
-    if (checker.CheckConfigCollisionFree(clique_ellipse.center(), builder_id)) {
-      iris_options.starting_ellipse = clique_ellipse;
-    } else {
-      // Find the nearest clique member to the center that is not in collision.
+    if (!checker.CheckConfigCollisionFree(clique_ellipse.center(),
+                                          builder_id)) {
       Eigen::Index nearest_point_col;
       (clique_points.colwise() - clique_ellipse.center())
           .colwise()
           .norm()
           .minCoeff(&nearest_point_col);
       Eigen::VectorXd center = clique_points.col(nearest_point_col);
-      iris_options.starting_ellipse =
-          Hyperellipsoid(clique_ellipse.A(), center);
+      clique_ellipse = Hyperellipsoid(clique_ellipse.A(), center);
     }
-    checker.UpdatePositions(iris_options.starting_ellipse->center(),
-                            builder_id);
+    checker.UpdatePositions(clique_ellipse.center(), builder_id);
     log()->debug("Iris builder thread {} is constructing a set.", builder_id);
-    ret.emplace(IrisInConfigurationSpace(
-        checker.plant(), checker.plant_context(builder_id), iris_options));
+    std::visit(
+        [&](auto&& arg) {
+          using T = std::decay_t<decltype(arg)>;
+          if constexpr (std::is_same_v<T,
+                                       geometry::optimization::IrisOptions>) {
+            arg.starting_ellipse = clique_ellipse;
+            ret.emplace(IrisInConfigurationSpace(
+                checker.plant(), checker.plant_context(builder_id), arg));
+          } else if constexpr (std::is_same_v<T, IrisNp2Options>) {
+            ret.emplace(IrisNp2(
+                *static_cast<const SceneGraphCollisionChecker*>(&checker),
+                clique_ellipse, domain, arg));
+          } else if constexpr (std::is_same_v<T, IrisZoOptions>) {
+            ret.emplace(IrisZo(checker, clique_ellipse, domain, arg));
+          }
+        },
+        iris_options);
     log()->debug("Iris builder thread {} has constructed a set.", builder_id);
 
     current_clique = computed_cliques->pop();
   }
+
   log()->debug("Iris builder thread {} has completed.", builder_id);
   return ret;
 }
@@ -331,6 +355,11 @@ void IrisInConfigurationSpaceFromCliqueCover(
         max_clique_solver_ptr) {
   DRAKE_THROW_UNLESS(options.coverage_termination_threshold > 0);
   DRAKE_THROW_UNLESS(options.iteration_limit > 0);
+  if (std::holds_alternative<IrisNp2Options>(options.iris_options)) {
+    // IrisNp2Options only supports SceneGraphCollisionChecker.
+    DRAKE_THROW_UNLESS(
+        dynamic_cast<const SceneGraphCollisionChecker*>(&checker) != nullptr);
+  }
 
   // Note: Even though the iris_options.bounding_region may be provided,
   // IrisInConfigurationSpace (currently) requires finite joint limits.
@@ -339,9 +368,16 @@ void IrisInConfigurationSpaceFromCliqueCover(
   DRAKE_THROW_UNLESS(
       checker.plant().GetPositionUpperLimits().array().isFinite().all());
 
-  const HPolyhedron domain = options.iris_options.bounding_region.value_or(
+  const HPolyhedron default_domain =
       HPolyhedron::MakeBox(checker.plant().GetPositionLowerLimits(),
-                           checker.plant().GetPositionUpperLimits()));
+                           checker.plant().GetPositionUpperLimits());
+  const HPolyhedron domain =
+      std::holds_alternative<geometry::optimization::IrisOptions>(
+          options.iris_options)
+          ? std::get<geometry::optimization::IrisOptions>(options.iris_options)
+                .bounding_region.value_or(default_domain)
+          : default_domain;
+
   DRAKE_THROW_UNLESS(domain.ambient_dimension() ==
                      checker.plant().num_positions());
   Eigen::VectorXd last_polytope_sample = domain.UniformSample(generator);
@@ -402,16 +438,26 @@ void IrisInConfigurationSpaceFromCliqueCover(
     }
 
     // Show the samples used in build cliques. Debugging visualization.
-    if (options.iris_options.meshcat && domain.ambient_dimension() <= 3) {
+    auto meshcat_ptr = std::visit(
+        [](auto&& arg) -> std::shared_ptr<geometry::Meshcat> {
+          using T = std::decay_t<decltype(arg)>;
+          if constexpr (std::is_same_v<T,
+                                       geometry::optimization::IrisOptions>) {
+            return arg.meshcat;
+          } else {
+            return arg.sampled_iris_options.meshcat;
+          }
+        },
+        options.iris_options);
+    if (meshcat_ptr && domain.ambient_dimension() <= 3) {
       Eigen::Vector3d point_to_draw = Eigen::Vector3d::Zero();
       for (int pt_to_draw = 0; pt_to_draw < points.cols(); ++pt_to_draw) {
         std::string path = fmt::format("iteration{:02}/sample_{:03}",
                                        num_iterations, pt_to_draw);
-        options.iris_options.meshcat->SetObject(
-            path, Sphere(0.01), geometry::Rgba(1, 0.1, 0.1, 1.0));
+        meshcat_ptr->SetObject(path, Sphere(0.01),
+                               geometry::Rgba(1, 0.1, 0.1, 1.0));
         point_to_draw.head(domain.ambient_dimension()) = points.col(pt_to_draw);
-        options.iris_options.meshcat->SetTransform(
-            path, RigidTransform<double>(point_to_draw));
+        meshcat_ptr->SetTransform(path, RigidTransform<double>(point_to_draw));
       }
     }
 
@@ -434,7 +480,7 @@ void IrisInConfigurationSpaceFromCliqueCover(
       ComputeGreedyTruncatedCliqueCover(minimum_clique_size, *max_clique_solver,
                                         &visibility_graph, &computed_cliques);
       std::queue<HPolyhedron> new_set_queue =
-          IrisWorker(checker, points, 0, options, &computed_cliques,
+          IrisWorker(checker, points, 0, options, domain, &computed_cliques,
                      false /* No need to disable meshcat */);
       while (!new_set_queue.empty()) {
         sets->push_back(std::move(new_set_queue.front()));
@@ -458,7 +504,7 @@ void IrisInConfigurationSpaceFromCliqueCover(
       for (int i = 0; i < num_builder_threads; ++i) {
         build_sets_future.emplace_back(
             std::async(std::launch::async, IrisWorker, std::ref(checker),
-                       points, i, std::ref(options), &computed_cliques,
+                       points, i, std::ref(options), domain, &computed_cliques,
                        // NOLINTNEXTLINE
                        true /* Disable meshcat since IRIS runs outside the main thread */));
       }

planning/iris/iris_from_clique_cover.h

@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <memory>
 #include <optional>
+#include <variant>
 #include <vector>
 
 #include "drake/common/parallelism.h"
@@ -12,16 +13,23 @@
 #include "drake/geometry/optimization/iris.h"
 #include "drake/planning/graph_algorithms/max_clique_solver_base.h"
 #include "drake/planning/graph_algorithms/max_clique_solver_via_greedy.h"
+#include "drake/planning/iris/iris_np2.h"
+#include "drake/planning/iris/iris_zo.h"
 #include "drake/planning/scene_graph_collision_checker.h"
 
 namespace drake {
 namespace planning {
 
 struct IrisFromCliqueCoverOptions {
+  IrisFromCliqueCoverOptions()
+      : iris_options(
+            geometry::optimization::IrisOptions{/*iteration_limit=*/1}) {}
+
   /**
-   * The options used on internal calls to Iris.  Currently, it is recommended
-   * to only run Iris for one iteration when building from a clique so as to
-   * avoid discarding the information gained from the clique.
+   * The options used on internal calls to Iris. The type of this option
+   * determines which variant of Iris is called. Currently, it is recommended to
+   * only run Iris for one iteration when building from a clique so as to avoid
+   * discarding the information gained from the clique.
    *
    * Note that `IrisOptions` can optionally include a meshcat instance to
    * provide debugging visualization. If this is provided `IrisFromCliqueCover`
@@ -30,8 +38,13 @@ struct IrisFromCliqueCoverOptions {
    * allow more than 1 thread, then the debug visualizations of internal Iris
    * calls will be disabled. This is due to a limitation of drawing to meshcat
    * from outside the main thread.
+   *
+   * @note If IrisNp2Options is used, then the collision checker must be a
+   * SceneGraphCollisionChecker.
    */
-  geometry::optimization::IrisOptions iris_options{.iteration_limit = 1};
+  std::variant<geometry::optimization::IrisOptions, IrisNp2Options,
+               IrisZoOptions>
+      iris_options;
 
   /**
    * The fraction of the domain that must be covered before we terminate the