Leverage parallelization where possible when checking boundedness of …

…a ConvexSet. (#22084) This parallelizes the generic method for checking boundedness of a convex set, which is sometimes used by various derived classes of ConvexSet. Also fixes a bug in the boundedness check of MinkowskiSum.
RobotLocomotion · Nov 5, 2024 · bde2f2d · bde2f2d
1 parent bf2408a
commit bde2f2d
Show file tree

Hide file tree

Showing 28 changed files with 414 additions and 84 deletions.
diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc
@@ -109,7 +109,9 @@ void DefineConvexSetBaseClassAndSubclasses(py::module m) {
             cls_doc.ambient_dimension.doc)
         .def("IntersectsWith", &ConvexSet::IntersectsWith, py::arg("other"),
             cls_doc.IntersectsWith.doc)
-        .def("IsBounded", &ConvexSet::IsBounded, cls_doc.IsBounded.doc)
+        .def("IsBounded", &ConvexSet::IsBounded,
+            py::arg("parallelism") = Parallelism::None(),
+            py::call_guard<py::gil_scoped_release>(), cls_doc.IsBounded.doc)
         .def("IsEmpty", &ConvexSet::IsEmpty, cls_doc.IsEmpty.doc)
         .def("MaybeGetPoint", &ConvexSet::MaybeGetPoint,
             cls_doc.MaybeGetPoint.doc)

diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py
@@ -6,7 +6,7 @@
 
 import numpy as np
 
-from pydrake.common import RandomGenerator, temp_directory
+from pydrake.common import RandomGenerator, temp_directory, Parallelism
 from pydrake.common.test_utilities.deprecation import catch_drake_warnings
 from pydrake.common.test_utilities.pickle_compare import assert_pickle
 from pydrake.geometry import (
@@ -456,6 +456,7 @@ def test_spectrahedron(self):
         self.assertFalse(s.IsEmpty())
         self.assertFalse(s.MaybeGetFeasiblePoint() is None)
         self.assertTrue(s.PointInSet(s.MaybeGetFeasiblePoint()))
+        self.assertTrue(s.IsBounded(Parallelism.Max()))
 
     def test_v_polytope(self):
         mut.VPolytope()

diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel
@@ -95,6 +95,7 @@ drake_cc_library(
         "//solvers:mosek_solver",
         "//solvers:scs_solver",
         "//solvers:solve",
+        "@common_robotics_utilities",
         "@qhull_internal//:qhull",
     ],
 )
@@ -536,6 +537,7 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "intersection_test",
+    num_threads = 2,
     deps = [
         ":convex_set",
         ":test_utilities",
@@ -612,6 +614,7 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "spectrahedron_test",
+    num_threads = 2,
     deps = [
         ":convex_set",
         "//common/test_utilities:eigen_matrix_compare",

diff --git a/geometry/optimization/affine_ball.cc b/geometry/optimization/affine_ball.cc
@@ -94,6 +94,10 @@ double AffineBall::DoCalcVolume() const {
          std::abs(B_.determinant());
 }
 
+std::optional<bool> AffineBall::DoIsBoundedShortcut() const {
+  return true;
+}
+
 AffineBall AffineBall::MakeAxisAligned(
     const Eigen::Ref<const VectorXd>& radius,
     const Eigen::Ref<const VectorXd>& center) {

diff --git a/geometry/optimization/affine_ball.h b/geometry/optimization/affine_ball.h
@@ -118,11 +118,17 @@ class AffineBall final : public ConvexSet {
     CheckInvariants();
   }
 
+  /** Every AffineBall is bounded by construction.
+  @param parallelism Ignored -- no parallelization is used.
+  @note See @ref ConvexSet::IsBounded "parent class's documentation" for more
+  details. */
+  using ConvexSet::IsBounded;
+
  private:
   std::unique_ptr<ConvexSet> DoClone() const final;
 
   /* AffineBall can only represent bounded sets. */
-  std::optional<bool> DoIsBoundedShortcut() const final { return true; };
+  std::optional<bool> DoIsBoundedShortcut() const final;
 
   /* AffineBall can only represent nonempty sets. */
   bool DoIsEmpty() const final { return false; };

diff --git a/geometry/optimization/affine_subspace.h b/geometry/optimization/affine_subspace.h
@@ -144,6 +144,13 @@ class AffineSubspace final : public ConvexSet {
   to this AffineSubspace.*/
   Eigen::MatrixXd OrthogonalComplementBasis() const;
 
+  /** An AffineSubspace is bounded if and only if it is zero-dimensional (i.e.,
+  a point).
+  @param parallelism Ignored -- no parallelization is used.
+  @note See @ref ConvexSet::IsBounded "parent class's documentation" for more
+  details. */
+  using ConvexSet::IsBounded;
+
  private:
   std::unique_ptr<ConvexSet> DoClone() const final;
 

diff --git a/geometry/optimization/cartesian_product.cc b/geometry/optimization/cartesian_product.cc
@@ -132,10 +132,11 @@ std::unique_ptr<ConvexSet> CartesianProduct::DoClone() const {
   return std::make_unique<CartesianProduct>(*this);
 }
 
-std::optional<bool> CartesianProduct::DoIsBoundedShortcut() const {
+std::optional<bool> CartesianProduct::DoIsBoundedShortcutParallel(
+    Parallelism parallelism) const {
   // Note: The constructor enforces that A_ is full column rank.
   for (const auto& s : sets_) {
-    if (!s->IsBounded()) {
+    if (!s->IsBounded(parallelism)) {
       return false;
     }
   }

diff --git a/geometry/optimization/cartesian_product.h b/geometry/optimization/cartesian_product.h
@@ -90,10 +90,19 @@ class CartesianProduct final : public ConvexSet {
   product. */
   using ConvexSet::CalcVolume;
 
+  /** A CartesianProduct is bounded if and only if each constituent set is
+  bounded. This class honors requests for parallelism only so far as its
+  constituent sets do.
+  @param parallelism The maximum number of threads to use.
+  @note See @ref ConvexSet::IsBounded "parent class's documentation" for more
+  details. */
+  using ConvexSet::IsBounded;
+
  private:
   std::unique_ptr<ConvexSet> DoClone() const final;
 
-  std::optional<bool> DoIsBoundedShortcut() const final;
+  std::optional<bool> DoIsBoundedShortcutParallel(
+      Parallelism parallelism) const final;
 
   bool DoIsEmpty() const final;
 

diff --git a/geometry/optimization/convex_hull.cc b/geometry/optimization/convex_hull.cc
@@ -82,8 +82,14 @@ std::unique_ptr<ConvexSet> ConvexHull::DoClone() const {
   return std::make_unique<ConvexHull>(*this);
 }
 
-std::optional<bool> ConvexHull::DoIsBoundedShortcut() const {
-  return std::nullopt;
+std::optional<bool> ConvexHull::DoIsBoundedShortcutParallel(
+    Parallelism parallelism) const {
+  for (const auto& s : sets_) {
+    if (!s->IsBounded(parallelism)) {
+      return false;
+    }
+  }
+  return true;
 }
 
 bool ConvexHull::DoIsEmpty() const {

diff --git a/geometry/optimization/convex_hull.h b/geometry/optimization/convex_hull.h
@@ -64,10 +64,19 @@ class ConvexHull final : public ConvexSet, private ShapeReifier {
   /** @throws  Not implemented. */
   using ConvexSet::CalcVolume;
 
+  /** A ConvexHull is bounded if and only if each constituent set is bounded.
+  This class honors requests for parallelism only so far as its constituent sets
+  do.
+  @param parallelism The maximum number of threads to use.
+  @note See @ref ConvexSet::IsBounded "parent class's documentation" for more
+  details. */
+  using ConvexSet::IsBounded;
+
  private:
   std::unique_ptr<ConvexSet> DoClone() const final;
 
-  std::optional<bool> DoIsBoundedShortcut() const final;
+  std::optional<bool> DoIsBoundedShortcutParallel(
+      Parallelism parallelism) const final;
 
   bool DoIsEmpty() const final;