Changes for platform.

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -1081,6 +1081,9 @@ void DefineGeometryOptimization(py::module m) {
   }
 
   using drake::geometry::optimization::ConvexSet;
+  m.def("CheckIfSatisfiesConvexityRadius", &CheckIfSatisfiesConvexityRadius,
+      py::arg("convex_set"), py::arg("continuous_revolute_joints"),
+      doc.CheckIfSatisfiesConvexityRadius.doc);
   m.def(
       "PartitionConvexSet",
       [](const ConvexSet& convex_set,

bindings/pydrake/geometry/test/optimization_test.py

@@ -1172,3 +1172,5 @@ def test_partition_convex_set(self):
         mut.PartitionConvexSet(convex_sets=[big_convex_set],
                                continuous_revolute_joints=[0],
                                epsilon=1e-5)
+        mut.CheckIfSatisfiesConvexityRadius(convex_set=big_convex_set,
+                                            continuous_revolute_joints=[0])

geometry/optimization/geodesic_convexity.cc

@@ -19,11 +19,8 @@ using drake::solvers::Solve;
 using drake::solvers::VectorXDecisionVariable;
 using Eigen::VectorXd;
 
-const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
-    const ConvexSet& region, const int dimension) {
-  // TODO(cohnt) centralize this function in geometry/optimization, by making it
-  // a member of ConvexSet. Also potentially only compute the value once, and
-  // then return the pre-computed value instead of re-computing.
+std::pair<double, double> internal::GetMinimumAndMaximumValueAlongDimension(
+    const ConvexSet& region, int dimension) {
   DRAKE_THROW_UNLESS(dimension >= 0 && dimension < region.ambient_dimension());
   MathematicalProgram prog;
   VectorXDecisionVariable y =
@@ -55,7 +52,7 @@ const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
   return {lower_bound, upper_bound};
 }
 
-void ThrowsForInvalidContinuousJointsList(
+void internal::ThrowsForInvalidContinuousJointsList(
     int num_positions, const std::vector<int>& continuous_revolute_joints) {
   for (int i = 0; i < ssize(continuous_revolute_joints); ++i) {
     // Make sure the unbounded revolute joints point to valid indices.
@@ -80,7 +77,7 @@ bool CheckIfSatisfiesConvexityRadius(
     const std::vector<int>& continuous_revolute_joints) {
   for (const int& j : continuous_revolute_joints) {
     auto [min_value, max_value] =
-        GetMinimumAndMaximumValueAlongDimension(convex_set, j);
+        internal::GetMinimumAndMaximumValueAlongDimension(convex_set, j);
     if (max_value - min_value >= M_PI) {
       return false;
     }
@@ -93,8 +90,8 @@ ConvexSets PartitionConvexSet(
     const std::vector<int>& continuous_revolute_joints, const double epsilon) {
   DRAKE_THROW_UNLESS(epsilon > 0);
   DRAKE_THROW_UNLESS(epsilon < M_PI);
-  ThrowsForInvalidContinuousJointsList(convex_set.ambient_dimension(),
-                                       continuous_revolute_joints);
+  internal::ThrowsForInvalidContinuousJointsList(convex_set.ambient_dimension(),
+                                                 continuous_revolute_joints);
   // Boundedness along dimensions corresponding to continuous revolute joints
   // will be asserted by the GetMinimumAndMaximumValueAlongDimension calls
   // below.
@@ -108,7 +105,7 @@ ConvexSets PartitionConvexSet(
   // We only populate the entries corresponding to continuous revolute joints,
   // since the lower and upper limits of other joints aren't needed.
   for (const int& i : continuous_revolute_joints) {
-    bbox[i] = GetMinimumAndMaximumValueAlongDimension(convex_set, i);
+    bbox[i] = internal::GetMinimumAndMaximumValueAlongDimension(convex_set, i);
   }
   // The overall structure is to partition the set along each dimension
   // corresponding to a continuous revolute joint. The partitioning is done by
@@ -154,8 +151,8 @@ ConvexSets PartitionConvexSet(
     const std::vector<int>& continuous_revolute_joints, const double epsilon) {
   DRAKE_THROW_UNLESS(convex_sets.size() > 0);
   DRAKE_THROW_UNLESS(convex_sets[0] != nullptr);
-  ThrowsForInvalidContinuousJointsList(convex_sets[0]->ambient_dimension(),
-                                       continuous_revolute_joints);
+  internal::ThrowsForInvalidContinuousJointsList(
+      convex_sets[0]->ambient_dimension(), continuous_revolute_joints);
 
   int ambient_dimension = convex_sets[0]->ambient_dimension();
   for (int i = 1; i < ssize(convex_sets); ++i) {

geometry/optimization/geodesic_convexity.h

@@ -1,3 +1,13 @@
+/** @file
+A robot that has revolute joints without any limits has an inherently
+non-Euclidean configuration space, but one can still consider
+"geodesically-convex" sets, akin to convex sets in Euclidean space. In practice,
+this only requires that the width of the set along each dimension corresponding
+to an unbounded revolute joint be strictly less than π. These functions are
+primarily used by GcsTrajectoryOptimization to make motion plans for these types
+of robots.
+*/
+
 #pragma once
 
 #include <utility>
@@ -10,30 +20,25 @@
 namespace drake {
 namespace geometry {
 namespace optimization {
+namespace internal {
 
-/** A robot that has revolute joints without any limits has an inherently
-non-Euclidean configuration space, but one can still consider
-"geodesically-convex" sets, akin to convex sets in Euclidean space. In
-practice, this only requires that the width of the set along each dimension
-corresponding to an unbounded revolute joint be strictly less than π.
-
-These functions are primarily used by GcsTrajectoryOptimization to make motion
-plans for these types of robots. */
-
-/** Computes the minimum and maximum values that can be attained along a certain
-dimension for a point constrained to lie within a convex set. */
-const std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
-    const ConvexSet& region, const int dimension);
+/* Computes the minimum and maximum values that can be attained along a certain
+dimension for a point constrained to lie within a convex set.
+@throws std::exception if dimension is outside the interval
+[0, region.ambient_dimension()). */
+std::pair<double, double> GetMinimumAndMaximumValueAlongDimension(
+    const ConvexSet& region, int dimension);
 
-/** Helper function to assert that a given list of continuous revolute joint
+/* Helper function to assert that a given list of continuous revolute joint
 indices satisfies the requirements for the constructor to
 GcsTrajectoryOptimization, as well as any static functions that may take in
 such a list. */
 void ThrowsForInvalidContinuousJointsList(
     int num_positions, const std::vector<int>& continuous_revolute_joints);
+}  // namespace internal
 
 /** Given a convex set, and a list of indices corresponding to continuous
-revolute joints, check whether or not the set satisfies the convexity radius.
+revolute joints, checks whether or not the set satisfies the convexity radius.
 See §6.5.3 of "A Panoramic View of Riemannian Geometry", Marcel Berger for a
 general definition of convexity radius. When dealing with continuous revolute
 joints, respecting the convexity radius entails that each convex set has a width

geometry/optimization/test/geodesic_convexity_test.cc

@@ -27,10 +27,10 @@ GTEST_TEST(GeodesicConvexityTest, PartitionConvexSetAPI) {
   const double epsilon = 1e-5;
 
   // Test the function overload that takes in multiple convex sets.
-  ConvexSets sets5 = PartitionConvexSet(MakeConvexSets(v, v),
-                                        continuous_revolute_joints, epsilon);
-  EXPECT_EQ(sets5.size(), 8);
-  for (const auto& set : sets5) {
+  ConvexSets sets = PartitionConvexSet(MakeConvexSets(v, v),
+                                       continuous_revolute_joints, epsilon);
+  EXPECT_EQ(sets.size(), 8);
+  for (const auto& set : sets) {
     EXPECT_TRUE(
         CheckIfSatisfiesConvexityRadius(*set, continuous_revolute_joints));
   }
@@ -140,18 +140,17 @@ GTEST_TEST(GeodesicConvexityTest, PartitionConvexSet2) {
   std::vector<int> continuous_revolute_joints{0, 1};
   const double epsilon = 1e-5;
 
-  ConvexSets sets2 =
-      PartitionConvexSet(v2, continuous_revolute_joints, epsilon);
-  EXPECT_EQ(sets2.size(), 2);
-  for (const auto& set : sets2) {
+  ConvexSets sets = PartitionConvexSet(v2, continuous_revolute_joints, epsilon);
+  EXPECT_EQ(sets.size(), 2);
+  for (const auto& set : sets) {
     EXPECT_TRUE(
         CheckIfSatisfiesConvexityRadius(*set, continuous_revolute_joints));
   }
 
   // Check that sets overlap by epsilon.
   const auto maybe_intersection_bbox =
       Hyperrectangle::MaybeCalcAxisAlignedBoundingBox(
-          Intersection(*sets2[0], *sets2[1]));
+          Intersection(*sets[0], *sets[1]));
   ASSERT_TRUE(maybe_intersection_bbox.has_value());
   const Hyperrectangle intersection_bbox = maybe_intersection_bbox.value();
   for (int i = 0; i < intersection_bbox.ambient_dimension(); ++i) {
@@ -176,10 +175,9 @@ GTEST_TEST(GeodesicConvexityTest, PartitionConvexSet3) {
   std::vector<int> continuous_revolute_joints{0, 1};
   const double epsilon = 1e-5;
 
-  ConvexSets sets3 =
-      PartitionConvexSet(v3, continuous_revolute_joints, epsilon);
-  EXPECT_EQ(sets3.size(), 1);
-  for (const auto& set : sets3) {
+  ConvexSets sets = PartitionConvexSet(v3, continuous_revolute_joints, epsilon);
+  EXPECT_EQ(sets.size(), 1);
+  for (const auto& set : sets) {
     EXPECT_TRUE(
         CheckIfSatisfiesConvexityRadius(*set, continuous_revolute_joints));
   }
@@ -197,15 +195,15 @@ GTEST_TEST(GeodesicConvexityTest, PartitionConvexSet4) {
   std::vector<int> continuous_revolute_joints{0, 1};
   std::vector<int> only_one_continuous_revolute_joint{1};
 
-  ConvexSets sets4 = PartitionConvexSet(v, only_one_continuous_revolute_joint);
-  EXPECT_EQ(sets4.size(), 2);
-  for (const auto& set : sets4) {
+  ConvexSets sets = PartitionConvexSet(v, only_one_continuous_revolute_joint);
+  EXPECT_EQ(sets.size(), 2);
+  for (const auto& set : sets) {
     EXPECT_TRUE(CheckIfSatisfiesConvexityRadius(
         *set, only_one_continuous_revolute_joint));
   }
 
   bool all_satisfy = true;
-  for (const auto& set : sets4) {
+  for (const auto& set : sets) {
     all_satisfy = all_satisfy && CheckIfSatisfiesConvexityRadius(
                                      *set, continuous_revolute_joints);
   }

planning/trajectory_optimization/gcs_trajectory_optimization.cc

@@ -36,14 +36,14 @@ using geometry::optimization::CartesianProduct;
 using geometry::optimization::CheckIfSatisfiesConvexityRadius;
 using geometry::optimization::ConvexSet;
 using geometry::optimization::ConvexSets;
-using geometry::optimization::GetMinimumAndMaximumValueAlongDimension;
 using geometry::optimization::GraphOfConvexSets;
 using geometry::optimization::GraphOfConvexSetsOptions;
 using geometry::optimization::HPolyhedron;
 using geometry::optimization::Intersection;
 using geometry::optimization::PartitionConvexSet;
 using geometry::optimization::Point;
-using geometry::optimization::ThrowsForInvalidContinuousJointsList;
+using geometry::optimization::internal::GetMinimumAndMaximumValueAlongDimension;
+using geometry::optimization::internal::ThrowsForInvalidContinuousJointsList;
 using multibody::Joint;
 using multibody::JointIndex;
 using multibody::MultibodyPlant;