Function to partition convex sets into pieces that satisfy the convex…

…ity radius property.

bindings/pydrake/geometry/optimization_pybind.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <vector>
 
 #include "drake/bindings/pydrake/pydrake_pybind.h"
@@ -26,5 +27,23 @@ geometry::optimization::ConvexSets CloneConvexSets(
   return sets;
 }
 
+/** Deep-copies the ConvexSet copyable_unique_ptrs in the given list into a
+Python-compatible list of ConvexSet unique_ptrs. This is useful to return
+Python-natural values out of the C++ API, which would otherwise return the more
+complex type. */
+std::vector<std::unique_ptr<geometry::optimization::ConvexSet>> CloneConvexSets(
+    geometry::optimization::ConvexSets sets_in) {
+  std::vector<std::unique_ptr<geometry::optimization::ConvexSet>> sets_out;
+  sets_out.reserve(sets_in.size());
+  for (const auto& set : sets_in) {
+    if (set == nullptr) {
+      sets_out.push_back(nullptr);
+    } else {
+      sets_out.push_back(set->Clone());
+    }
+  }
+  return sets_out;
+}
+
 }  // namespace pydrake
 }  // namespace drake

bindings/pydrake/planning/planning_py_trajectory_optimization.cc

@@ -2,6 +2,7 @@
 #include "drake/bindings/pydrake/geometry/optimization_pybind.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
 #include "drake/bindings/pydrake/symbolic_types_pybind.h"
+#include "drake/geometry/optimization/convex_set.h"
 #include "drake/planning/trajectory_optimization/direct_collocation.h"
 #include "drake/planning/trajectory_optimization/direct_transcription.h"
 #include "drake/planning/trajectory_optimization/gcs_trajectory_optimization.h"
@@ -460,6 +461,32 @@ void DefinePlanningTrajectoryOptimization(py::module m) {
         .def_static("NormalizeSegmentTimes", &Class::NormalizeSegmentTimes,
             py::arg("trajectory"), cls_doc.NormalizeSegmentTimes.doc);
   }
+
+  m.def(
+      "PartitionConvexSet",
+      [](const drake::geometry::optimization::ConvexSet& convex_set,
+          const std::vector<int>& continuous_revolute_joints,
+          const double epsilon)
+          -> std::vector<std::unique_ptr<geometry::optimization::ConvexSet>> {
+        return CloneConvexSets(PartitionConvexSet(
+            convex_set, continuous_revolute_joints, epsilon));
+      },
+      py::arg("convex_set"), py::arg("continuous_revolute_joints"),
+      py::arg("epsilon") = 1e-5, py_rvp::take_ownership,
+      doc.PartitionConvexSet.doc);
+  m.def(
+      "PartitionConvexSet",
+      [](const std::vector<drake::geometry::optimization::ConvexSet*>&
+              convex_sets,
+          const std::vector<int>& continuous_revolute_joints,
+          const double epsilon = 1e-5)
+          -> std::vector<std::unique_ptr<geometry::optimization::ConvexSet>> {
+        return CloneConvexSets(PartitionConvexSet(
+            CloneConvexSets(convex_sets), continuous_revolute_joints, epsilon));
+      },
+      py::arg("convex_sets"), py::arg("continuous_revolute_joints"),
+      py::arg("epsilon") = 1e-5, py_rvp::take_ownership,
+      doc.PartitionConvexSet.doc);
 }
 
 }  // namespace internal

bindings/pydrake/planning/test/trajectory_optimization_test.py

@@ -13,6 +13,7 @@
     DirectTranscription,
     GcsTrajectoryOptimization,
     KinematicTrajectoryOptimization,
+    PartitionConvexSet
 )
 from pydrake.geometry.optimization import (
     GraphOfConvexSetsOptions,
@@ -545,3 +546,14 @@ def test_gcs_trajectory_optimization_wraparound(self):
                                   order=1,
                                   edges_between_regions=[[0, 1]],
                                   edge_offsets=[[2*np.pi]])
+        big_convex_set = VPolytope(np.array([[0, 4]]))
+        PartitionConvexSet(big_convex_set, [0])
+        PartitionConvexSet(big_convex_set, [0], 1e-5)
+        PartitionConvexSet(convex_set=big_convex_set,
+                           continuous_revolute_joints=[0],
+                           epsilon=1e-5)
+        PartitionConvexSet([big_convex_set], [0])
+        PartitionConvexSet([big_convex_set], [0], 1e-5)
+        PartitionConvexSet(convex_sets=[big_convex_set],
+                           continuous_revolute_joints=[0],
+                           epsilon=1e-5)

planning/trajectory_optimization/gcs_trajectory_optimization.cc

@@ -10,6 +10,7 @@
 #include "drake/common/symbolic/decompose.h"
 #include "drake/geometry/optimization/cartesian_product.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
+#include "drake/geometry/optimization/hyperrectangle.h"
 #include "drake/geometry/optimization/intersection.h"
 #include "drake/geometry/optimization/point.h"
 #include "drake/math/matrix_util.h"
@@ -34,6 +35,7 @@ using geometry::optimization::ConvexSets;
 using geometry::optimization::GraphOfConvexSets;
 using geometry::optimization::GraphOfConvexSetsOptions;
 using geometry::optimization::HPolyhedron;
+using geometry::optimization::Hyperrectangle;
 using geometry::optimization::Intersection;
 using geometry::optimization::Point;
 using solvers::Binding;
@@ -188,7 +190,8 @@ void Subgraph::ThrowsForInvalidConvexityRadius() const {
             "along dimension {}, so it doesn't respect the convexity radius! "
             "To add this set, separate it into smaller pieces so that along "
             "dimensions corresponding to continuous revolute joints, its width "
-            "is strictly smaller than π.",
+            "is strictly smaller than π. This can be done manually, or with "
+            "the helper function PartitionConvexSet.",
             i, j));
       }
     }
@@ -1056,6 +1059,72 @@ GcsTrajectoryOptimization::NormalizeSegmentTimes(
   return CompositeTrajectory<double>(normalized_bezier_curves);
 }
 
+ConvexSets PartitionConvexSet(
+    const ConvexSet& convex_set,
+    const std::vector<int>& continuous_revolute_joints, const double epsilon) {
+  DRAKE_THROW_UNLESS(epsilon > 0);
+  // Unboundedness will be asserted by
+  // Hyperrectangle::MaybeCalcAxisAlignedBoundingBox.
+
+  ConvexSets sets = MakeConvexSets(convex_set);
+  const double convexity_radius_step = M_PI - 2 * epsilon;
+  const int dim = convex_set.ambient_dimension();
+  const auto bbox_maybe =
+      Hyperrectangle::MaybeCalcAxisAlignedBoundingBox(convex_set);
+  DRAKE_THROW_UNLESS(bbox_maybe.has_value());
+  const Hyperrectangle bbox = bbox_maybe.value();
+  // The overall structure is to partition the set along each dimension
+  // corresponding to a continuous revolute joint. The partitioning is done by
+  // constructing axis-aligned bounding boxes, and intersecting them with the
+  // original set.
+  for (const int& i : continuous_revolute_joints) {
+    const double min_value = bbox.lb()[i];
+    const double max_value = bbox.ub()[i];
+    if (max_value - min_value >= M_PI) {
+      const int j_max = sets.size();
+      for (int j = 0; j < j_max; ++j) {
+        for (double k = min_value + convexity_radius_step;
+             k < max_value + convexity_radius_step;
+             k += convexity_radius_step) {
+          Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2, dim);
+          Eigen::VectorXd b = Eigen::VectorXd::Zero(2);
+          A(0, i) = 1;
+          b[0] = k + (epsilon / 2.0);
+          A(1, i) = -1;
+          b[1] = -(k - convexity_radius_step - (epsilon / 2.0));
+          HPolyhedron chunk_bbox(A, b);
+          sets.push_back(copyable_unique_ptr<ConvexSet>(
+              Intersection(*sets[j], chunk_bbox)));
+        }
+      }
+      sets.erase(sets.begin(), sets.begin() + j_max);
+    }
+  }
+  return sets;
+}
+
+ConvexSets PartitionConvexSet(
+    ConvexSets convex_sets, const std::vector<int>& continuous_revolute_joints,
+    const double epsilon) {
+  DRAKE_THROW_UNLESS(convex_sets.size() > 0);
+  DRAKE_THROW_UNLESS(convex_sets[0] != nullptr);
+
+  int ambient_dimension = convex_sets[0]->ambient_dimension();
+  for (int i = 1; i < ssize(convex_sets); ++i) {
+    DRAKE_THROW_UNLESS(convex_sets[i] != nullptr);
+    DRAKE_THROW_UNLESS(convex_sets[i]->ambient_dimension() ==
+                       ambient_dimension);
+  }
+
+  ConvexSets sets;
+  for (int i = 0; i < ssize(convex_sets); ++i) {
+    ConvexSets new_sets = PartitionConvexSet(
+        *convex_sets[i], continuous_revolute_joints, epsilon);
+    sets.insert(sets.end(), new_sets.begin(), new_sets.end());
+  }
+  return sets;
+}
+
 }  // namespace trajectory_optimization
 }  // namespace planning
 }  // namespace drake

planning/trajectory_optimization/gcs_trajectory_optimization.h

@@ -551,6 +551,32 @@ class GcsTrajectoryOptimization final {
   std::vector<int> global_continuity_constraints_{};
 };
 
+/* Given a convex set, and a list of indices corresponding to continuous
+revolute joints, return a list of convex sets that respect the convexity
+radius, and whose union is the given convex set. Respecting the convexity radius
+entails that each convex set has a width of stricty less than π along each
+dimension corresponding to a continuous revolute joint. In practice, this is
+implemented as having a width less than or equal to π-ϵ.
+@param epsilon is the ϵ value used for the convexity radius inequality. The
+partitioned sets are made to overlap by ϵ/2 along each dimension, for numerical
+purposes.
+@throws std::exception if ϵ <= 0.
+@throws std::exception if the input convex set is unbounded.
+*/
+geometry::optimization::ConvexSets PartitionConvexSet(
+    const geometry::optimization::ConvexSet& convex_set,
+    const std::vector<int>& continuous_revolute_joints,
+    const double epsilon = 1e-5);
+
+/* Function overload to take in a list of convex sets, and partition all so as
+to respect the convexity radius. Every set must be bounded and have the same
+ambient dimension.
+@throws std::exception if ϵ <= 0. */
+geometry::optimization::ConvexSets PartitionConvexSet(
+    geometry::optimization::ConvexSets convex_sets,
+    const std::vector<int>& continuous_revolute_joints,
+    const double epsilon = 1e-5);
+
 }  // namespace trajectory_optimization
 }  // namespace planning
 }  // namespace drake

planning/trajectory_optimization/test/gcs_trajectory_optimization_test.cc

@@ -24,10 +24,12 @@ using Eigen::MatrixXd;
 using Eigen::Vector2d;
 using Eigen::Vector3d;
 using Eigen::VectorXd;
+using geometry::optimization::ConvexSet;
 using geometry::optimization::ConvexSets;
 using geometry::optimization::GraphOfConvexSets;
 using geometry::optimization::GraphOfConvexSetsOptions;
 using geometry::optimization::HPolyhedron;
+using geometry::optimization::Hyperellipsoid;
 using geometry::optimization::MakeConvexSets;
 using geometry::optimization::Point;
 using geometry::optimization::VPolytope;
@@ -1322,6 +1324,85 @@ GTEST_TEST(GcsTrajectoryOptimizationTest, ContinuousJointsApi) {
       "interval.*");
 }
 
+bool PointInASet(ConvexSets sets, Eigen::VectorXd point, double kTol) {
+  for (int i = 0; i < ssize(sets); ++i) {
+    if (sets[i]->PointInSet(point, kTol)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+GTEST_TEST(GcsTrajectoryOptimizationTest, PartitionConvexSet) {
+  Eigen::Matrix<double, 2, 4> vertices;
+  // clang-format off
+  vertices << 0.0, 4.0, 0.0, 4.0,
+              0.0, 0.0, 4.0, 4.0;
+  // clang-format on
+  VPolytope v(vertices);
+  std::vector<int> continuous_revolute_joints{0, 1};
+  const double epsilon = 1e-5;
+
+  ConvexSets sets = PartitionConvexSet(v, continuous_revolute_joints, epsilon);
+  EXPECT_EQ(sets.size(), 4);
+
+  GcsTrajectoryOptimization gcs(2, continuous_revolute_joints);
+  DRAKE_EXPECT_THROWS_MESSAGE(gcs.AddRegions(MakeConvexSets(v), 1),
+                              ".*doesn't respect the convexity radius.*");
+
+  // Check that sets satisfy the convexity radius by trying to use them in GCS.
+  DRAKE_EXPECT_NO_THROW(gcs.AddRegions(sets, 1));
+  const double kTol = 1e-11;
+  EXPECT_TRUE(PointInASet(sets, Eigen::Vector2d(0.0, 0.0), kTol));
+  EXPECT_TRUE(PointInASet(sets, Eigen::Vector2d(4.0, 0.0), kTol));
+  EXPECT_TRUE(PointInASet(sets, Eigen::Vector2d(0.0, 4.0), kTol));
+  EXPECT_TRUE(PointInASet(sets, Eigen::Vector2d(4.0, 4.0), kTol));
+  for (int i = 0; i < ssize(sets); ++i) {
+    EXPECT_TRUE(sets[i]->PointInSet(
+        Eigen::Vector2d(M_PI - 2.0 * epsilon, M_PI - 2.0 * epsilon), kTol));
+  }
+
+  ConvexSets sets2 = PartitionConvexSet(MakeConvexSets(v, v),
+                                        continuous_revolute_joints, epsilon);
+  EXPECT_EQ(sets2.size(), 8);
+  DRAKE_EXPECT_NO_THROW(gcs.AddRegions(sets, 1));
+
+  // List of convex sets must have at least one entry.
+  EXPECT_THROW(PartitionConvexSet(MakeConvexSets(), continuous_revolute_joints),
+               std::exception);
+
+  // List of convex sets must not have null pointers.
+  ConvexSets contains_nullptr;
+  contains_nullptr.push_back(copyable_unique_ptr<ConvexSet>(nullptr));
+  EXPECT_THROW(PartitionConvexSet(contains_nullptr, continuous_revolute_joints),
+               std::exception);
+
+  // List of convex sets must not have unbounded sets.
+  Hyperellipsoid unbounded_hyperellipsoid(Eigen::MatrixXd::Zero(2, 2),
+                                          Eigen::VectorXd::Zero(2));
+  Eigen::Matrix<double, 1, 2> A;
+  A << 1, 0;
+  Eigen::VectorXd b(1);
+  b << 0;
+  HPolyhedron unbounded_hpolyhedron(A, b);
+  EXPECT_THROW(
+      PartitionConvexSet(unbounded_hpolyhedron, continuous_revolute_joints),
+      std::exception);
+  EXPECT_THROW(PartitionConvexSet(MakeConvexSets(unbounded_hpolyhedron),
+                                  continuous_revolute_joints),
+               std::exception);
+
+  // List of convex sets must have matching ambient dimension.
+  VPolytope v2(Eigen::MatrixXd::Zero(3, 1));
+  EXPECT_THROW(
+      PartitionConvexSet(MakeConvexSets(v, v2), continuous_revolute_joints),
+      std::exception);
+
+  // Epsilon must be strictly positive.
+  EXPECT_THROW(PartitionConvexSet(v, continuous_revolute_joints, 0.0),
+               std::exception);
+}
+
 }  // namespace
 }  // namespace trajectory_optimization
 }  // namespace planning