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,

geometry/optimization/geodesic_convexity.cc

@@ -19,8 +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) {
+std::pair<double, double> internal::GetMinimumAndMaximumValueAlongDimension(
+    const ConvexSet& region, 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.
@@ -55,7 +55,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 +80,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 +93,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 +108,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 +154,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

@@ -10,30 +10,33 @@
 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. */
+// 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);
+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
 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

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;