Implements IRIS ZO

bindings/pydrake/planning/BUILD.bazel

@@ -41,6 +41,7 @@ drake_pybind_library(
         "planning_py_trajectory_optimization.cc",
         "planning_py_visibility_graph.cc",
         "planning_py_iris_from_clique_cover.cc",
+        "planning_py_iris_zo.cc",
         "planning_py_zmp_planner.cc",
     ],
     package_info = PACKAGE_INFO,
@@ -130,6 +131,14 @@ drake_py_unittest(
     ],
 )
 
+# drake_py_unittest(
+#     name = "iris_zo_test",
+#     num_threads = 2,
+#     deps = [
+#         ":planning",
+#     ],
+# )
+
 drake_py_unittest(
     name = "zmp_planner_test",
     deps = [

bindings/pydrake/planning/planning_py.cc

@@ -28,6 +28,7 @@ and/or trajectories of dynamical systems.
   internal::DefinePlanningTrajectoryOptimization(m);
   internal::DefinePlanningVisibilityGraph(m);
   internal::DefinePlanningIrisFromCliqueCover(m);
+  internal::DefinePlanningIrisZo(m);
   internal::DefinePlanningZmpPlanner(m);
 }
 

bindings/pydrake/planning/planning_py.h

@@ -26,6 +26,9 @@ void DefinePlanningGraphAlgorithms(py::module m);
 /* Defines bindings per planning_py_iris_from_clique_cover.cc. */
 void DefinePlanningIrisFromCliqueCover(py::module m);
 
+/* Defines bindings per planning_py_iris_zo.cc. */
+void DefinePlanningIrisZo(py::module m);
+
 /* Defines bindings per planning_py_robot_diagram.cc. */
 void DefinePlanningRobotDiagram(py::module m);
 

bindings/pydrake/planning/planning_py_iris_zo.cc

@@ -0,0 +1,92 @@
+#include "drake/bindings/pydrake/documentation_pybind.h"
+#include "drake/bindings/pydrake/planning/planning_py.h"
+#include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/planning/iris/iris_zo.h"
+
+namespace drake {
+namespace pydrake {
+namespace internal {
+
+void DefinePlanningIrisZo(py::module m) {
+  // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
+  using namespace drake::planning;
+  constexpr auto& doc = pydrake_doc.drake.planning;
+
+  // IrisZoOptions
+  const auto& cls_doc = doc.IrisZoOptions;
+  py::class_<IrisZoOptions>(m, "IrisZoOptions", cls_doc.doc)
+      .def(py::init<>())
+      .def_readwrite("num_particles", &IrisZoOptions::num_particles,
+          cls_doc.num_particles.doc)
+      .def_readwrite("tau", &IrisZoOptions::tau, cls_doc.tau.doc)
+      .def_readwrite("delta", &IrisZoOptions::delta, cls_doc.delta.doc)
+      .def_readwrite("epsilon", &IrisZoOptions::epsilon, cls_doc.epsilon.doc)
+      .def_readwrite("containment_points", &IrisZoOptions::containment_points,
+          cls_doc.containment_points.doc)
+      .def_readwrite("max_iterations", &IrisZoOptions::max_iterations,
+          cls_doc.max_iterations.doc)
+      .def_readwrite("max_iterations_separating_planes",
+          &IrisZoOptions::max_iterations_separating_planes,
+          cls_doc.max_iterations_separating_planes.doc)
+      .def_readwrite("max_separating_planes_per_iteration",
+          &IrisZoOptions::max_separating_planes_per_iteration,
+          cls_doc.max_separating_planes_per_iteration.doc)
+      .def_readwrite("bisection_steps", &IrisZoOptions::bisection_steps,
+          cls_doc.bisection_steps.doc)
+      .def_readwrite(
+          "parallelism", &IrisZoOptions::parallelism, cls_doc.parallelism.doc)
+      .def_readwrite("verbose", &IrisZoOptions::verbose, cls_doc.verbose.doc)
+      .def_readwrite("require_sample_point_is_contained",
+          &IrisZoOptions::require_sample_point_is_contained,
+          cls_doc.require_sample_point_is_contained.doc)
+      .def_readwrite("configuration_space_margin",
+          &IrisZoOptions::configuration_space_margin,
+          cls_doc.configuration_space_margin.doc)
+      .def_readwrite("termination_threshold",
+          &IrisZoOptions::termination_threshold,
+          cls_doc.termination_threshold.doc)
+      .def_readwrite("relative_termination_threshold",
+          &IrisZoOptions::relative_termination_threshold,
+          cls_doc.relative_termination_threshold.doc)
+      .def_readwrite(
+          "random_seed", &IrisZoOptions::random_seed, cls_doc.random_seed.doc)
+      .def_readwrite("mixing_steps", &IrisZoOptions::mixing_steps,
+          cls_doc.mixing_steps.doc)
+      .def("__repr__", [](const IrisZoOptions& self) {
+        return py::str(
+            "IrisZoOptions("
+            "num_particles={}, "
+            "tau={}, "
+            "delta={}, "
+            "epsilon={}, "
+            "max_iterations={}, "
+            "max_iterations_separating_planes={}, "
+            "max_separating_planes_per_iteration={}, "
+            "bisection_steps={}, "
+            "parallelism={}, "
+            "verbose={}, "
+            "require_sample_point_is_contained={}, "
+            "configuration_space_margin={}, "
+            "termination_threshold={}, "
+            "relative_termination_threshold={}, "
+            "random_seed={}, "
+            "mixing_steps={}, "
+            ")")
+            .format(self.num_particles, self.tau, self.delta, self.epsilon,
+                self.max_iterations, self.max_iterations_separating_planes,
+                self.max_separating_planes_per_iteration, self.bisection_steps,
+                self.parallelism, self.verbose,
+                self.require_sample_point_is_contained,
+                self.configuration_space_margin, self.termination_threshold,
+                self.relative_termination_threshold, self.random_seed,
+                self.mixing_steps);
+      });
+
+  m.def("IrisZo", &IrisZo, py::arg("checker"), py::arg("starting_ellipsoid"),
+      py::arg("domain"), py::arg("options") = IrisZoOptions(),
+      py::call_guard<py::gil_scoped_release>(), doc.IrisZo.doc);
+}
+
+}  // namespace internal
+}  // namespace pydrake
+}  // namespace drake

bindings/pydrake/planning/test/iris_zo_test.py

@@ -0,0 +1,77 @@
+import unittest
+
+import pydrake.planning as mut
+from pydrake.common import RandomGenerator, Parallelism
+from pydrake.geometry.optimization import Hyperellipsoid, HPolyhedron
+from pydrake.planning import (
+    RobotDiagramBuilder,
+    SceneGraphCollisionChecker,
+    CollisionCheckerParams,
+)
+
+import numpy as np
+
+
+class TestIrisZo(unittest.TestCase):
+    def test_iris_zo(self):
+        # Taken from iris_from_clique_cover_test.py
+        cross_cspace_urdf = """
+        <robot name="boxes">
+          <link name="fixed">
+            <collision name="top_left">
+              <origin rpy="0 0 0" xyz="-1 1 0"/>
+              <geometry><box size="1 1 1"/></geometry>
+            </collision>
+            <collision name="top_right">
+              <origin rpy="0 0 0" xyz="1 1 0"/>
+              <geometry><box size="1 1 1"/></geometry>
+            </collision>
+            <collision name="bottom_left">
+              <origin rpy="0 0 0" xyz="-1 -1 0"/>
+              <geometry><box size="1 1 1"/></geometry>
+            </collision>
+            <collision name="bottom_right">
+              <origin rpy="0 0 0" xyz="1 -1 0"/>
+              <geometry><box size="1 1 1"/></geometry>
+            </collision>
+          </link>
+          <joint name="fixed_link_weld" type="fixed">
+            <parent link="world"/>
+            <child link="fixed"/>
+          </joint>
+          <link name="movable">
+            <collision name="sphere">
+              <geometry><sphere radius="0.1"/></geometry>
+            </collision>
+          </link>
+          <link name="for_joint"/>
+          <joint name="x" type="prismatic">
+            <axis xyz="1 0 0"/>
+            <limit lower="-2" upper="2"/>
+            <parent link="world"/>
+            <child link="for_joint"/>
+          </joint>
+          <joint name="y" type="prismatic">
+            <axis xyz="0 1 0"/>
+            <limit lower="-2" upper="2"/>
+            <parent link="for_joint"/>
+            <child link="movable"/>
+          </joint>
+        </robot>
+"""
+        params = dict(edge_step_size=0.1)
+        builder = RobotDiagramBuilder()
+        params["robot_model_instances"] = builder.parser().AddModelsFromString(
+            cross_cspace_urdf, "urdf"
+        )
+        params["model"] = builder.Build()
+        plant = params["model"].plant()
+        checker = SceneGraphCollisionChecker(**params)
+        options = mut.IrisZoOptions()
+        seed_point = np.zeros((2,))
+        starting_ellipsoid = Hyperellipsoid.MakeHypersphere(0.01, seed_point)
+        domain = HPolyhedron.MakeBox(plant.GetPositionLowerLimits(),
+                                     plant.GetPositionUpperLimits())
+        region = mut.IrisZo(checker, starting_ellipsoid, domain, options)
+        test_point = np.array([0.0, 0.5])
+        self.assertTrue(region.PointInSet(test_point))

planning/iris/BUILD.bazel

@@ -14,6 +14,7 @@ drake_cc_package_library(
     visibility = ["//visibility:public"],
     deps = [
         ":iris_from_clique_cover",
+        ":iris_zo",
     ],
 )
 
@@ -37,6 +38,26 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "iris_zo",
+    srcs = ["iris_zo.cc"],
+    hdrs = ["iris_zo.h"],
+    deps = [
+        "//common:parallelism",
+        "//geometry:meshcat",
+        "//geometry/optimization:convex_set",
+        "//planning:collision_checker",
+        "//solvers:choose_best_solver",
+        "//solvers:clarabel_solver",
+        "//solvers:mathematical_program",
+        "//solvers:mosek_solver",
+        "//solvers:solve",
+    ],
+    implementation_deps = [
+        "@common_robotics_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "iris_from_clique_cover_test",
     timeout = "moderate",
@@ -69,4 +90,28 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "iris_zo_test",
+    timeout = "moderate",
+    data = ["//multibody/parsing:test_models"],
+    num_threads = 1,
+    shard_count = 1,
+    # Most of these tests take an exceptionally long time under
+    # instrumentation, resulting in timeouts, and so are excluded.
+    tags = [
+        "no_asan",
+        "no_memcheck",
+    ],
+    deps = [
+        ":iris_zo",
+        "//common/test_utilities:expect_throws_message",
+        "//common/test_utilities:maybe_pause_for_user",
+        "//geometry:meshcat",
+        "//geometry/test_utilities:meshcat_environment",
+        "//multibody/inverse_kinematics",
+        "//planning:robot_diagram_builder",
+        "//planning:scene_graph_collision_checker",
+    ],
+)
+
 add_lint_tests()