Common Iris Entrypoint Via Abstract Classes

planning/iris/BUILD.bazel

@@ -37,6 +37,130 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "iris_interface_options",
+    srcs = ["iris_interface_options.cc"],
+    hdrs = ["iris_interface_options.h"],
+    deps = [
+        "//geometry/optimization:convex_set",
+        "//solvers:solver_options",
+        "//geometry:meshcat",
+        "//common:random"
+    ],
+    implementation_deps = [
+    ],
+)
+
+
+drake_cc_library(
+    name = "iris_via_collisions_and_ellipsoid_interface_options",
+    srcs = ["iris_via_collisions_and_ellipsoid_interface_options.cc"],
+    hdrs = ["iris_via_collisions_and_ellipsoid_interface_options.h"],
+    deps = [
+        "//planning/iris:iris_interface",
+        "//planning/iris:iris_interface_options"
+    ],
+)
+
+drake_cc_library(
+    name = "iris_via_collisions_and_ellipsoid_interface",
+    srcs = ["iris_via_collisions_and_ellipsoid_interface.cc"],
+    hdrs = ["iris_via_collisions_and_ellipsoid_interface.h"],
+    deps = [
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options",
+        "//planning/iris:iris_interface",
+        "//planning:collision_checker",
+        "//planning/iris:iris_interface_options"
+    ],
+)
+
+drake_cc_library(
+    name = "iris_interface",
+    srcs = ["iris_interface.cc"],
+    hdrs = ["iris_interface.h"],
+    deps = [
+        "//planning:collision_checker",
+        ":iris_interface_options",
+        "//geometry/optimization:convex_set",
+    ],
+    implementation_deps = [
+    ],
+)
+
+drake_cc_library(
+    name = "iris_np",
+    srcs = ["iris_np.cc"],
+    hdrs = ["iris_np.h"],
+    deps = [
+        "//planning:scene_graph_collision_checker",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options",
+        "//geometry/optimization:convex_set",
+    ],
+    implementation_deps = [
+    ],
+)
+
+drake_cc_library(
+    name = "iris_np2",
+    srcs = ["iris_np2.cc"],
+    hdrs = ["iris_np2.h"],
+    deps = [
+        "//planning:scene_graph_collision_checker",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options",
+        "//geometry/optimization:convex_set",
+    ],
+    implementation_deps = [
+    ],
+)
+
+drake_cc_library(
+    name = "iris_zo",
+    srcs = ["iris_zo.cc"],
+    hdrs = ["iris_zo.h"],
+    deps = [
+        "//planning:collision_checker",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options",
+        "//geometry/optimization:convex_set",
+    ],
+    implementation_deps = [
+    ],
+)
+
+
+drake_cc_library(
+    name = "c_iris",
+    srcs = ["c_iris.cc"],
+    hdrs = ["c_iris.h"],
+    deps = [
+        "//planning:scene_graph_collision_checker",
+        "//planning/iris:iris_interface",
+        "//planning/iris:iris_interface_options",
+        "//geometry/optimization:convex_set",
+        "//geometry/optimization:cspace_free_polytope",
+        "//geometry/optimization:cspace_free_polytope_base",
+        "//geometry/optimization:cspace_free_structs",
+    ],
+    implementation_deps = [
+    ],
+)
+
+drake_cc_library(
+    name = "vanilla_iris",
+    srcs = ["vanilla_iris.cc"],
+    hdrs = ["vanilla_iris.h"],
+    deps = [
+        "//planning:collision_checker",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface",
+        "//planning/iris:iris_via_collisions_and_ellipsoid_interface_options",
+        "//geometry/optimization:convex_set",
+    ],
+    implementation_deps = [
+    ],
+)
+
 drake_cc_googletest(
     name = "iris_from_clique_cover_test",
     timeout = "moderate",

planning/iris/c_iris.cc

@@ -0,0 +1,43 @@
+#include "drake/planning/iris/c_iris.h"
+
+namespace drake {
+namespace planning {
+using geometry::optimization::CspaceFreePolytope;
+
+C_Iris::C_Iris(
+    const SceneGraphCollisionChecker& checker,
+    geometry::optimization::SeparatingPlaneOrder plane_order,
+    const Eigen::Ref<const Eigen::VectorXd>& q_star,
+    const geometry::optimization::CspaceFreePolytopeBase::Options& options)
+    : IrisInterface<C_IrisOptions>(checker),
+      set_builder_{&(checker_->plant()), &(checker_->model().scene_graph()),
+                   plane_order, q_star, options} {};
+
+void C_Iris::DoSetup(const C_IrisOptions& options,
+                     geometry::optimization::HPolyhedron* set) {
+  *set = options.seed_region;
+  std::optional<CspaceFreePolytope::SearchResult> search_result =
+      set_builder_.BinarySearch(options.ignored_collision_pairs, set->A(),
+                                set->b(), set->ChebyshevCenter(),
+                                options.binary_search_options);
+  throw std::logic_error("unimplemented");
+}
+
+void C_Iris::DoImproveRegionHyperplanes(
+    const C_IrisOptions& options, geometry::optimization::HPolyhedron* set) {
+  // This should implement the step which pushes the hyperplanes away from the
+  // max volume inscribed ellipse.
+  unused(options, set);
+  throw std::logic_error("unimplemented");
+}
+
+void C_Iris::DoUpdateMetric(const C_IrisOptions& options,
+                            const geometry::optimization::HPolyhedron& set) {
+  // This should compute the new inscribed ellipse and the new certificate after
+  // the hyperplanes have been pushed back.
+  unused(options, set);
+  throw std::logic_error("unimplemented");
+}
+
+}  // namespace planning
+}  // namespace drake

planning/iris/c_iris.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "drake/geometry/optimization/cspace_free_polytope.h"
+#include "drake/geometry/optimization/cspace_free_polytope_base.h"
+#include "drake/geometry/optimization/cspace_free_structs.h"
+#include "drake/geometry/optimization/hpolyhedron.h"
+#include "drake/planning/iris/iris_interface.h"
+#include "drake/planning/iris/iris_interface_options.h"
+#include "drake/planning/scene_graph_collision_checker.h"
+
+namespace drake {
+namespace planning {
+
+struct C_IrisOptions : IrisInterfaceOptions {
+  // TODO add all the unique options.
+
+  // Options for running binary search during DoSetup which expands the seed
+  // polytope.
+  geometry::optimization::CspaceFreePolytope::BinarySearchOptions
+      binary_search_options;
+
+  // Options for running binary search during DoSetup which expands the seed
+  // polytope.
+  geometry::optimization::CspaceFreePolytope::BilinearAlternationOptions
+      bilinearSearchOptions;
+
+  geometry::optimization::HPolyhedron seed_region;
+
+  geometry::optimization::CspaceFreePolytopeBase::IgnoredCollisionPairs
+      ignored_collision_pairs;
+};
+
+class C_Iris final : public IrisInterface<C_IrisOptions> {
+ public:
+  C_Iris(
+      const SceneGraphCollisionChecker& checker,
+      geometry::optimization::SeparatingPlaneOrder plane_order,
+      const Eigen::Ref<const Eigen::VectorXd>& q_star,
+      const geometry::optimization::CspaceFreePolytopeBase::Options& options =
+          geometry::optimization::CspaceFreePolytopeBase::Options{});
+
+ private:
+  // Do not hide IrisInterface's pure virtual functions.
+  using IrisInterface<C_IrisOptions>::DoImproveRegionHyperplanes;
+  using IrisInterface<C_IrisOptions>::DoUpdateMetric;
+  using IrisInterface<C_IrisOptions>::DoSetup;
+
+  /** Runs Binary Search on the seed polytope. */
+  void DoSetup(const C_IrisOptions& options,
+               geometry::optimization::HPolyhedron* set);
+
+  /** Runs the step which pushes the hyperplanes away from the max volume
+   * inscribed ellipse. */
+  void DoImproveRegionHyperplanes(const C_IrisOptions& options,
+                                  geometry::optimization::HPolyhedron* set);
+
+  /** Runs the step which computes the max volume inscribed ellipse and a new
+   * certificate. */
+  void DoUpdateMetric(const C_IrisOptions& options,
+                      const geometry::optimization::HPolyhedron& set);
+
+  // Currently we use the set_builder_ to implement the above methods, but a
+  // better rewrite will condense this into one file.
+  geometry::optimization::CspaceFreePolytope set_builder_;
+};
+
+}  // namespace planning
+}  // namespace drake

planning/iris/iris_interface.cc

@@ -0,0 +1 @@
+#include "drake/planning/iris/iris_interface.h"

planning/iris/iris_interface.h

@@ -0,0 +1,100 @@
+#pragma once
+
+#include <memory>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/geometry/optimization/hpolyhedron.h"
+#include "drake/planning/collision_checker.h"
+#include "drake/planning/iris/iris_interface_options.h"
+
+namespace drake {
+namespace planning {
+
+// SFINAE that the options subclass must derive from the IrisInterfaceOptions.
+template <typename IrisInterfaceOptionsSubclass,
+          typename = std::enable_if_t<std::is_base_of<
+              IrisInterfaceOptions, IrisInterfaceOptionsSubclass>::value>>
+class IrisInterface {
+  /**
+   * A class for implementing various Iris-type algorithms. Note that this
+   * interface is NOT thread-safe in the sense that one IrisInterface object
+   * cannot be used to construct multiple regions in parallel.
+   */
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IrisInterface);
+  virtual ~IrisInterface();
+
+  geometry::optimization::HPolyhedron BuildSet(
+      const IrisInterfaceOptionsSubclass& options) {
+    geometry::optimization::HPolyhedron set =
+        options.domain.value_or(geometry::optimization::HPolyhedron::MakeBox(
+            checker_->plant().GetPositionLowerLimits(),
+            checker_->plant().GetPositionUpperLimits()));
+    DRAKE_THROW_UNLESS(set.IsBounded());
+
+    Setup(options, &set);
+
+    int iteration = 0;
+    while (iteration < options.iteration_limit && !CheckTermination(set)) {
+      ImproveRegionHyperplanes(options, &set);
+      UpdateMetric(options, set);
+      ++iteration;
+    }
+    return set;
+  };
+
+  void Setup(const IrisInterfaceOptionsSubclass& options,
+             geometry::optimization::HPolyhedron* set) {
+    DRAKE_THROW_UNLESS(set != nullptr);
+    DoSetup(options, set);
+  }
+
+  void ImproveRegionHyperplanes(const IrisInterfaceOptionsSubclass& options,
+                                geometry::optimization::HPolyhedron* set) {
+    DRAKE_THROW_UNLESS(set != nullptr);
+    DoImproveRegionHyperplanes(options, set);
+  }
+
+  void UpdateMetric(const IrisInterfaceOptionsSubclass& options,
+                    const geometry::optimization::HPolyhedron& set) {
+    DoUpdateMetric(options, set);
+  }
+
+  bool CheckTermination(const IrisInterfaceOptionsSubclass& options,
+                        const geometry::optimization::HPolyhedron& set) {
+    bool ret{false};
+    if (options.termination_func) {
+      ret = options.termination_func(set);
+    }
+    return ret || DoCheckTermination(options, set);
+  }
+
+ protected:
+  IrisInterface(const CollisionChecker& checker)
+      : checker_{std::move(checker.Clone())} {};
+
+  /** Runs any additional set up code before the set building loop starts. */
+  virtual void DoSetup(const IrisInterfaceOptionsSubclass& options,
+                       geometry::optimization::HPolyhedron* set) = 0;
+
+  /** Given a proposed region, modify it to a better region, e.g. by adding
+   * hyperplanes so that less of the set is in collision. */
+  virtual void DoImproveRegionHyperplanes(
+      const IrisInterfaceOptionsSubclass& options,
+      geometry::optimization::HPolyhedron* set) = 0;
+
+  /** Updates the metric used to find an improvement of the set */
+  virtual void DoUpdateMetric(
+      const IrisInterfaceOptionsSubclass& options,
+      const geometry::optimization::HPolyhedron& set) = 0;
+
+  /** Returns true if the set construct loop should terminate. */
+  virtual bool DoCheckTermination(
+      const IrisInterfaceOptionsSubclass& options,
+      const geometry::optimization::HPolyhedron& set) = 0;
+
+  std::unique_ptr<CollisionChecker> checker_;
+};
+
+}  // namespace planning
+}  // namespace drake

planning/iris/iris_interface_options.cc

@@ -0,0 +1 @@
+#include "drake/planning/iris/iris_interface_options.h"

planning/iris/iris_interface_options.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "drake/common/random.h"
+#include "drake/geometry/meshcat.h"
+#include "drake/geometry/optimization/hpolyhedron.h"
+
+namespace drake {
+namespace planning {
+
+struct IrisInterfaceOptions {
+  /** Maximum number of iterations. */
+  int iteration_limit{100};
+
+  /** A function which returns true when a set is done being constructed.*/
+  std::function<bool(const geometry::optimization::HPolyhedron&)>
+      termination_func;
+
+  /** Solver options used by any internal solvers used in the loop. */
+  std::optional<solvers::SolverOptions> solver_options;
+
+  /** The domain in which the set is constructed. If unset, the plant's joint
+   * limits are used. */
+  std::optional<geometry::optimization::HPolyhedron> domain;
+
+  /** A default randomness generator for source of randomness which may occur in
+   * the algorithm. */
+  RandomGenerator random_generator{0};
+
+  /** Passing a meshcat instance may enable debugging visualizations. */
+  std::shared_ptr<geometry::Meshcat> meshcat{};
+};
+
+}  // namespace planning
+}  // namespace drake