Implement IRIS-NP2

geometry/optimization/iris.cc

@@ -36,6 +36,7 @@ using Eigen::MatrixXd;
 using Eigen::Ref;
 using Eigen::Vector3d;
 using Eigen::VectorXd;
+using internal::IrisConvexSetMaker;
 using math::RigidTransform;
 using multibody::Frame;
 using multibody::JacobianWrtVariable;
@@ -155,86 +156,6 @@ HPolyhedron Iris(const ConvexSets& obstacles, const Ref<const VectorXd>& sample,
   return P;
 }
 
-namespace {
-// Constructs a ConvexSet for each supported Shape and adds it to the set.
-class IrisConvexSetMaker final : public ShapeReifier {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IrisConvexSetMaker);
-
-  IrisConvexSetMaker(const QueryObject<double>& query,
-                     std::optional<FrameId> reference_frame)
-      : query_{query}, reference_frame_{reference_frame} {};
-
-  void set_reference_frame(const FrameId& reference_frame) {
-    DRAKE_DEMAND(reference_frame.is_valid());
-    *reference_frame_ = reference_frame;
-  }
-
-  void set_geometry_id(const GeometryId& geom_id) { geom_id_ = geom_id; }
-
-  using ShapeReifier::ImplementGeometry;
-
-  void ImplementGeometry(const Box&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    // Note: We choose HPolyhedron over VPolytope here, but the IRIS paper
-    // discusses a significant performance improvement using a "least-distance
-    // programming" instance from CVXGEN that exploited the VPolytope
-    // representation.  So we may wish to revisit this.
-    set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Capsule&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<MinkowskiSum>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Cylinder&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set =
-        std::make_unique<CartesianProduct>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Ellipsoid&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const HalfSpace&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Sphere&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Convex&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
-  }
-
-  void ImplementGeometry(const Mesh&, void* data) {
-    DRAKE_DEMAND(geom_id_.is_valid());
-    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
-    set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
-  }
-
- private:
-  const QueryObject<double>& query_{};
-  std::optional<FrameId> reference_frame_{};
-  GeometryId geom_id_{};
-};
-
-}  // namespace
-
 ConvexSets MakeIrisObstacles(const QueryObject<double>& query_object,
                              std::optional<FrameId> reference_frame) {
   const SceneGraphInspector<double>& inspector = query_object.inspector();
@@ -449,19 +370,6 @@ void MakeGuessFeasible(const HPolyhedron& P, Eigen::VectorXd* guess) {
   }
 }
 
-struct GeometryPairWithDistance {
-  GeometryId geomA;
-  GeometryId geomB;
-  double distance;
-
-  GeometryPairWithDistance(GeometryId gA, GeometryId gB, double dist)
-      : geomA(gA), geomB(gB), distance(dist) {}
-
-  bool operator<(const GeometryPairWithDistance& other) const {
-    return distance < other.distance;
-  }
-};
-
 bool CheckTerminate(const IrisOptions& options, const HPolyhedron& P,
                     const std::string& error_msg, const std::string& info_msg,
                     const bool is_initial_region) {
@@ -650,7 +558,7 @@ HPolyhedron IrisInConfigurationSpace(const MultibodyPlant<double>& plant,
   // distance between each collision pair from the seed point configuration.
   // This could improve computation times and produce regions with fewer
   // faces.
-  std::vector<GeometryPairWithDistance> sorted_pairs;
+  std::vector<internal::GeometryPairWithDistance> sorted_pairs;
   for (const auto& [geomA, geomB] : pairs) {
     const double distance =
         query_object.ComputeSignedDistancePairClosestPoints(geomA, geomB)

geometry/optimization/iris_internal.cc

@@ -167,6 +167,59 @@ bool ClosestCollisionProgram::Solve(
   }
   return false;
 }
+
+void IrisConvexSetMaker::ImplementGeometry(const Box&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  // Note: We choose HPolyhedron over VPolytope here, but the IRIS paper
+  // discusses a significant performance improvement using a "least-distance
+  // programming" instance from CVXGEN that exploited the VPolytope
+  // representation.  So we may wish to revisit this.
+  set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Capsule&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<MinkowskiSum>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Cylinder&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<CartesianProduct>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Ellipsoid&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const HalfSpace&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<HPolyhedron>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Sphere&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<Hyperellipsoid>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Convex&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
+}
+
+void IrisConvexSetMaker::ImplementGeometry(const Mesh&, void* data) {
+  DRAKE_DEMAND(geom_id_.is_valid());
+  auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+  set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
+}
+
 }  // namespace internal
 }  // namespace optimization
 }  // namespace geometry

geometry/optimization/iris_internal.h

@@ -7,8 +7,13 @@
 #include <memory>
 #include <optional>
 
+#include "drake/geometry/optimization/affine_ball.h"
+#include "drake/geometry/optimization/cartesian_product.h"
 #include "drake/geometry/optimization/convex_set.h"
 #include "drake/geometry/optimization/hyperellipsoid.h"
+#include "drake/geometry/optimization/iris_internal.h"
+#include "drake/geometry/optimization/minkowski_sum.h"
+#include "drake/geometry/optimization/vpolytope.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/solvers/mathematical_program.h"
 #include "drake/solvers/solver_interface.h"
@@ -91,6 +96,60 @@ class ClosestCollisionProgram {
   solvers::VectorXDecisionVariable q_;
   std::optional<solvers::Binding<solvers::LinearConstraint>> P_constraint_{};
 };
+
+// Constructs a ConvexSet for each supported Shape and adds it to the set.
+class IrisConvexSetMaker final : public ShapeReifier {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IrisConvexSetMaker);
+
+  IrisConvexSetMaker(const QueryObject<double>& query,
+                     std::optional<FrameId> reference_frame)
+      : query_{query}, reference_frame_{reference_frame} {};
+
+  void set_reference_frame(const FrameId& reference_frame) {
+    DRAKE_DEMAND(reference_frame.is_valid());
+    *reference_frame_ = reference_frame;
+  }
+
+  void set_geometry_id(const GeometryId& geom_id) { geom_id_ = geom_id; }
+
+  using ShapeReifier::ImplementGeometry;
+
+  void ImplementGeometry(const Box&, void* data);
+
+  void ImplementGeometry(const Capsule&, void* data);
+
+  void ImplementGeometry(const Cylinder&, void* data);
+
+  void ImplementGeometry(const Ellipsoid&, void* data);
+
+  void ImplementGeometry(const HalfSpace&, void* data);
+
+  void ImplementGeometry(const Sphere&, void* data);
+
+  void ImplementGeometry(const Convex&, void* data);
+
+  void ImplementGeometry(const Mesh&, void* data);
+
+ private:
+  const QueryObject<double>& query_{};
+  std::optional<FrameId> reference_frame_{};
+  GeometryId geom_id_{};
+};
+
+struct GeometryPairWithDistance {
+  GeometryId geomA;
+  GeometryId geomB;
+  double distance;
+
+  GeometryPairWithDistance(GeometryId gA, GeometryId gB, double dist)
+      : geomA(gA), geomB(gB), distance(dist) {}
+
+  bool operator<(const GeometryPairWithDistance& other) const {
+    return distance < other.distance;
+  }
+};
+
 }  // namespace internal
 }  // namespace optimization
 }  // namespace geometry

planning/iris/BUILD.bazel

@@ -15,6 +15,7 @@ drake_cc_package_library(
     deps = [
         ":iris_common",
         ":iris_from_clique_cover",
+        ":iris_np2",
         ":iris_zo",
     ],
 )
@@ -46,6 +47,7 @@ drake_cc_library(
     deps = [
         "//common:name_value",
         "//geometry:meshcat",
+        "//geometry/optimization:convex_set",
         "//multibody/rational:rational_forward_kinematics",
         "//solvers:mathematical_program",
     ],
@@ -72,6 +74,19 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "iris_np2",
+    srcs = ["iris_np2.cc"],
+    hdrs = ["iris_np2.h"],
+    deps = [
+        ":iris_common",
+        "//geometry/optimization:convex_set",
+        "//geometry/optimization:iris_internal",
+        "//planning:scene_graph_collision_checker",
+        "//solvers",
+    ],
+)
+
 drake_cc_googletest(
     name = "iris_from_clique_cover_test",
     timeout = "moderate",
@@ -130,6 +145,25 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "iris_np2_test",
+    timeout = "moderate",
+    data = ["//multibody/parsing:test_models"],
+    num_threads = 2,
+    shard_count = 4,
+    tags = [
+        "no_asan",
+        "no_lsan",
+    ],
+    deps = [
+        ":iris_np2",
+        ":iris_test_utilities",
+        "//common/symbolic:expression",
+        "//common/test_utilities:expect_throws_message",
+        "//solvers",
+    ],
+)
+
 drake_cc_library(
     name = "iris_test_utilities",
     testonly = 1,

planning/iris/iris_common.cc

@@ -1,5 +1,7 @@
 #include "drake/planning/iris/iris_common.h"
 
+#include "drake/geometry/optimization/hyperellipsoid.h"
+
 namespace drake {
 namespace planning {
 
@@ -59,5 +61,43 @@ IrisParameterizationFunction::IrisParameterizationFunction(
   parameterization_dimension_ = dimension;
 }
 
+namespace internal {
+int unadaptive_test_samples(double epsilon, double delta, double tau) {
+  return static_cast<int>(-2 * std::log(delta) / (tau * tau * epsilon) + 0.5);
+}
+float calc_delta_min(double delta, int max_iterations) {
+  return delta * 6 / (M_PI * M_PI * max_iterations * max_iterations);
+}
+
+// Add the tangent to the (scaled) ellipsoid at @p point as a
+// constraint.
+void AddTangentToPolytope(
+    const geometry::optimization::Hyperellipsoid& E,
+    const Eigen::Ref<const Eigen::VectorXd>& point,
+    double configuration_space_margin,
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>* A,
+    Eigen::VectorXd* b, int* num_constraints) {
+  while (*num_constraints >= A->rows()) {
+    // Increase pre-allocated polytope size.
+    A->conservativeResize(A->rows() * 2, A->cols());
+    b->conservativeResize(b->rows() * 2);
+  }
+
+  A->row(*num_constraints) =
+      (E.A().transpose() * E.A() * (point - E.center())).normalized();
+  (*b)[*num_constraints] =
+      A->row(*num_constraints) * point - configuration_space_margin;
+  if (A->row(*num_constraints) * E.center() > (*b)[*num_constraints]) {
+    throw std::logic_error(
+        "The current center of the IRIS region is within "
+        "options.sampled_iris_options.configuration_space_margin of being "
+        "infeasible.  Check your sample point and/or any additional "
+        "constraints you've passed in via the options. The configuration space "
+        "surrounding the sample point must have an interior.");
+  }
+  *num_constraints += 1;
+}
+
+}  // namespace internal
 }  // namespace planning
 }  // namespace drake