Parallelize the generic method for checking boundedness of a convex set.

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -108,7 +108,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)

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()

geometry/optimization/BUILD.bazel

@@ -516,6 +516,7 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "intersection_test",
+    num_threads = 2,
     deps = [
         ":convex_set",
         ":test_utilities",
@@ -592,6 +593,7 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "spectrahedron_test",
+    num_threads = 2,
     deps = [
         ":convex_set",
         "//common/test_utilities:eigen_matrix_compare",

geometry/optimization/convex_set.cc

@@ -19,14 +19,14 @@ using solvers::Binding;
 using solvers::Constraint;
 using solvers::LinearCost;
 using solvers::MathematicalProgram;
+using solvers::MathematicalProgramResult;
 using solvers::SolutionResult;
 using solvers::VariableRefList;
 using solvers::VectorXDecisionVariable;
 
 namespace {
 
-bool SolverReturnedWithoutError(
-    const solvers::MathematicalProgramResult& result) {
+bool SolverReturnedWithoutError(const MathematicalProgramResult& result) {
   const SolutionResult status = result.get_solution_result();
   return status == SolutionResult::kSolutionFound ||
          status == SolutionResult::kInfeasibleConstraints ||
@@ -50,57 +50,80 @@ bool ConvexSet::IntersectsWith(const ConvexSet& other) const {
   if (ambient_dimension() == 0) {
     return !other.IsEmpty() && !this->IsEmpty();
   }
-  solvers::MathematicalProgram prog{};
+  MathematicalProgram prog{};
   const auto& x = prog.NewContinuousVariables(this->ambient_dimension(), "x");
   this->AddPointInSetConstraints(&prog, x);
   other.AddPointInSetConstraints(&prog, x);
-  solvers::MathematicalProgramResult result = solvers::Solve(prog);
+  MathematicalProgramResult result = solvers::Solve(prog);
   return result.is_success();
 }
 
-bool ConvexSet::GenericDoIsBounded() const {
+bool ConvexSet::GenericDoIsBounded(Parallelism parallelism) const {
   // The empty set is bounded. We check it first, to ensure that the program
   // is feasible, so SolutionResult::kInfeasibleOrUnbounded or
   // SolutionResult::kDualInfeasible indicates unbounded, as solvers may not
   // always explicitly return that the primal is unbounded.
   if (IsEmpty()) {
     return true;
   }
+
+  // At the end of each batch, we check to see if we've identified an unbounded
+  // direction. If so, we immediately return. Thus, the fastest behavior is to
+  // solve one problem on each thread, check if any are unbounded, and then
+  // iterate until all dimensions have been checked. We concurrently solve 2 *
+  // batch_size programs, since batch_size refers to the dimension, and there
+  // are two programs per dimension.
+  const int batch_size = std::max(1, parallelism.num_threads() / 2);
+
   // Let a variable x be contained in the convex set. Iteratively try to
   // minimize or maximize x[i] for each dimension i. If any solves are
   // unbounded, the set is not bounded.
-  MathematicalProgram prog;
-  VectorXDecisionVariable x =
-      prog.NewContinuousVariables(ambient_dimension(), "x");
-  AddPointInSetConstraints(&prog, x);
-  Binding<LinearCost> objective =
-      prog.AddLinearCost(VectorXd::Zero(ambient_dimension()), x);
-
-  VectorXd objective_vector(ambient_dimension());
-  for (int i = 0; i < ambient_dimension(); ++i) {
-    objective_vector.setZero();
-    objective_vector[i] = 1;
-    objective.evaluator()->UpdateCoefficients(objective_vector);
-    const auto result = solvers::Solve(prog);
-    if (result.get_solution_result() == solvers::SolutionResult::kUnbounded ||
-        result.get_solution_result() ==
-            solvers::SolutionResult::kInfeasibleOrUnbounded ||
-        result.get_solution_result() ==
-            solvers::SolutionResult::kDualInfeasible) {
-      return false;
+  int num_batches = 1 + (ambient_dimension() / batch_size);
+  for (int batch_idx = 0; batch_idx < num_batches; ++batch_idx) {
+    int batch_start = batch_idx * batch_size;
+    int batch_end = std::min((batch_idx + 1) * batch_size, ambient_dimension());
+    int this_batch_size = batch_end - batch_start;
+
+    std::vector<MathematicalProgram> progs(2 * this_batch_size);
+    for (int i = 0; i < this_batch_size; ++i) {
+      int dim = batch_start + i;
+      // progs[i] will try to minimize x[dim]. progs[i+this_batch_size] will try
+      // to maximize x[dim].
+      VectorXDecisionVariable x;
+      VectorXd objective_vector = VectorXd::Zero(ambient_dimension());
+
+      x = progs[i].NewContinuousVariables(ambient_dimension(), "x");
+      AddPointInSetConstraints(&(progs[i]), x);
+      objective_vector[dim] = 1;
+      progs[i].AddLinearCost(objective_vector, x);
+
+      x = progs[i + this_batch_size].NewContinuousVariables(ambient_dimension(),
+                                                            "x");
+      AddPointInSetConstraints(&(progs[i + this_batch_size]), x);
+      objective_vector[dim] = -1;
+      progs[i + this_batch_size].AddLinearCost(objective_vector, x);
     }
 
-    objective_vector[i] = -1;
-    objective.evaluator()->UpdateCoefficients(objective_vector);
-    const auto result2 = solvers::Solve(prog);
-    if (result2.get_solution_result() == solvers::SolutionResult::kUnbounded ||
-        result2.get_solution_result() ==
-            solvers::SolutionResult::kInfeasibleOrUnbounded ||
-        result2.get_solution_result() ==
-            solvers::SolutionResult::kDualInfeasible) {
-      return false;
+    std::vector<const MathematicalProgram*> prog_ptrs(progs.size());
+    for (int i = 0; i < ssize(progs); ++i) {
+      prog_ptrs[i] = &(progs[i]);
+    }
+
+    // All programs are the same size, so we use static scheduling.
+    std::vector<MathematicalProgramResult> results =
+        solvers::SolveInParallel(prog_ptrs, nullptr, nullptr, std::nullopt,
+                                 parallelism, false /* use_dynamic_schedule */);
+    for (const auto& result : results) {
+      if (result.get_solution_result() == solvers::SolutionResult::kUnbounded ||
+          result.get_solution_result() ==
+              solvers::SolutionResult::kInfeasibleOrUnbounded ||
+          result.get_solution_result() ==
+              solvers::SolutionResult::kDualInfeasible) {
+        return false;
+      }
     }
   }
+  // All optimization problems were bounded, so we return true.
   return true;
 }
 
@@ -234,7 +257,7 @@ bool ConvexSet::DoIsEmpty() const {
     // required, to ensure AddPointInSetConstraints is not called for a zero
     // dimensional set -- this would throw an error.
   }
-  solvers::MathematicalProgram prog;
+  MathematicalProgram prog;
   auto point = prog.NewContinuousVariables(ambient_dimension());
   AddPointInSetConstraints(&prog, point);
   auto result = solvers::Solve(prog);
@@ -260,7 +283,7 @@ std::optional<Eigen::VectorXd> ConvexSet::DoMaybeGetPoint() const {
 
 std::optional<Eigen::VectorXd> ConvexSet::DoMaybeGetFeasiblePoint() const {
   DRAKE_DEMAND(ambient_dimension() > 0);
-  solvers::MathematicalProgram prog;
+  MathematicalProgram prog;
   auto point = prog.NewContinuousVariables(ambient_dimension());
   AddPointInSetConstraints(&prog, point);
   auto result = solvers::Solve(prog);
@@ -300,7 +323,7 @@ bool ConvexSet::GenericDoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
 std::pair<VectorX<symbolic::Variable>,
           std::vector<solvers::Binding<solvers::Constraint>>>
 ConvexSet::AddPointInSetConstraints(
-    solvers::MathematicalProgram* prog,
+    MathematicalProgram* prog,
     const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const {
   DRAKE_THROW_UNLESS(vars.size() == ambient_dimension());
   DRAKE_THROW_UNLESS(ambient_dimension() > 0);
@@ -309,7 +332,7 @@ ConvexSet::AddPointInSetConstraints(
 
 std::vector<solvers::Binding<solvers::Constraint>>
 ConvexSet::AddPointInNonnegativeScalingConstraints(
-    solvers::MathematicalProgram* prog,
+    MathematicalProgram* prog,
     const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
     const symbolic::Variable& t) const {
   DRAKE_THROW_UNLESS(ambient_dimension() > 0);
@@ -323,8 +346,7 @@ ConvexSet::AddPointInNonnegativeScalingConstraints(
 
 std::vector<solvers::Binding<solvers::Constraint>>
 ConvexSet::AddPointInNonnegativeScalingConstraints(
-    solvers::MathematicalProgram* prog,
-    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    MathematicalProgram* prog, const Eigen::Ref<const Eigen::MatrixXd>& A,
     const Eigen::Ref<const Eigen::VectorXd>& b,
     const Eigen::Ref<const Eigen::VectorXd>& c, double d,
     const Eigen::Ref<const solvers::VectorXDecisionVariable>& x,
@@ -343,7 +365,7 @@ ConvexSet::AddPointInNonnegativeScalingConstraints(
 
 std::optional<symbolic::Variable>
 ConvexSet::HandleZeroAmbientDimensionConstraints(
-    solvers::MathematicalProgram* prog, const ConvexSet& set,
+    MathematicalProgram* prog, const ConvexSet& set,
     std::vector<solvers::Binding<solvers::Constraint>>* constraints) const {
   if (set.IsEmpty()) {
     drake::log()->warn(

geometry/optimization/convex_set.h

@@ -8,6 +8,7 @@
 
 #include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/drake_assert.h"
+#include "drake/common/parallelism.h"
 #include "drake/common/reset_after_move.h"
 #include "drake/geometry/geometry_ids.h"
 #include "drake/geometry/query_object.h"
@@ -90,18 +91,22 @@ class ConvexSet {
 
   /** Returns true iff the set is bounded, e.g., there exists an element-wise
   finite lower and upper bound for the set.  Note: for some derived classes,
-  this check is trivial, but for others it can require solving an (typically
-  small) optimization problem. Check the derived class documentation for any
-  notes. */
-  bool IsBounded() const {
+  this check is trivial, but for others it can require solving a number of
+  (typically small) optimization problems. Check the derived class documentation
+  for any notes.
+
+  @param parallelism specifies the number of cores to use when solving
+  mathematical programs to check boundedness, if the derived class does not
+  provide a more efficient boundedness check. */
+  bool IsBounded(Parallelism parallelism = Parallelism::None()) const {
     if (ambient_dimension() == 0) {
       return true;
     }
     const auto shortcut_result = DoIsBoundedShortcut();
     if (shortcut_result.has_value()) {
       return shortcut_result.value();
     }
-    return GenericDoIsBounded();
+    return GenericDoIsBounded(parallelism);
   }
 
   /** Returns true iff the set is empty. Note: for some derived classes, this
@@ -472,7 +477,7 @@ class ConvexSet {
  private:
   /** Generic implementation for IsBounded() -- applicable for all convex sets.
   @pre ambient_dimension() >= 0 */
-  bool GenericDoIsBounded() const;
+  bool GenericDoIsBounded(Parallelism parallelism) const;
 
   /** Generic implementation for PointInSet() -- applicable for all convex sets.
   @pre ambient_dimension() >= 0 */

geometry/optimization/test/intersection_test.cc

@@ -56,8 +56,12 @@ GTEST_TEST(IntersectionTest, BasicTest) {
     EXPECT_GE(new_constraints.size(), 2);
   }
 
+  // Cross-check that our BUILD file allows parallelism.
+  DRAKE_DEMAND(Parallelism::Max().num_threads() > 1);
+
   // Test IsBounded.
-  EXPECT_TRUE(S.IsBounded());
+  EXPECT_TRUE(S.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(S.IsBounded(Parallelism::Max()));
 
   // Test IsEmpty
   EXPECT_FALSE(S.IsEmpty());
@@ -95,7 +99,8 @@ GTEST_TEST(IntersectionTest, DefaultCtor) {
   EXPECT_NO_THROW(dut.Clone());
   EXPECT_EQ(dut.ambient_dimension(), 0);
   EXPECT_TRUE(dut.IntersectsWith(dut));
-  EXPECT_TRUE(dut.IsBounded());
+  EXPECT_TRUE(dut.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(dut.IsBounded(Parallelism::Max()));
   EXPECT_FALSE(dut.IsEmpty());
   EXPECT_TRUE(dut.MaybeGetPoint().has_value());
   EXPECT_TRUE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
@@ -141,10 +146,12 @@ GTEST_TEST(IntersectionTest, BoundedTest) {
 
   EXPECT_FALSE(H1.IsBounded());
   EXPECT_FALSE(H2.IsBounded());
-  EXPECT_TRUE(S1.IsBounded());
+  EXPECT_TRUE(S1.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(S1.IsBounded(Parallelism::Max()));
 
   Intersection S2(H1, H1);
-  EXPECT_FALSE(S2.IsBounded());
+  EXPECT_FALSE(S2.IsBounded(Parallelism::None()));
+  EXPECT_FALSE(S2.IsBounded(Parallelism::Max()));
 }
 
 GTEST_TEST(IntersectionTest, CloneTest) {

geometry/optimization/test/spectrahedron_test.cc

@@ -64,9 +64,14 @@ GTEST_TEST(SpectrahedronTest, TrivialSdp1) {
 
   Spectrahedron spect(prog);
   EXPECT_EQ(spect.ambient_dimension(), 3 * (3 + 1) / 2);
-  EXPECT_TRUE(spect.IsBounded());
   EXPECT_FALSE(spect.IsEmpty());
 
+  // Cross-check that our BUILD file allows parallelism.
+  DRAKE_DEMAND(Parallelism::Max().num_threads() > 1);
+
+  EXPECT_TRUE(spect.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(spect.IsBounded(Parallelism::Max()));
+
   const double kTol{1e-6};
   EXPECT_TRUE(spect.PointInSet(x_star, kTol));
   EXPECT_FALSE(spect.PointInSet(x_bad, kTol));
@@ -327,31 +332,36 @@ GTEST_TEST(SpectrahedronTest, UnboundedTest) {
   auto X1 = prog.NewSymmetricContinuousVariables<2>();
   prog.AddPositiveSemidefiniteConstraint(X1);
   Spectrahedron spect(prog);
-  EXPECT_FALSE(spect.IsBounded());
+  EXPECT_FALSE(spect.IsBounded(Parallelism::None()));
+  EXPECT_FALSE(spect.IsBounded(Parallelism::Max()));
 
   // Construct an unbounded but constrained SDP, and check that IsBounded
   // notices.
   prog.AddLinearConstraint(X1(0, 0) >= 0);
   Spectrahedron spect2(prog);
-  EXPECT_FALSE(spect2.IsBounded());
+  EXPECT_FALSE(spect2.IsBounded(Parallelism::None()));
+  EXPECT_FALSE(spect2.IsBounded(Parallelism::Max()));
 
   // Add more constraints that keep the program unbounded
   prog.AddLinearConstraint(X1(0, 0) == X1(1, 1));
   prog.AddLinearConstraint(X1(0, 1) >= 1);
   Spectrahedron spect3(prog);
-  EXPECT_FALSE(spect3.IsBounded());
+  EXPECT_FALSE(spect3.IsBounded(Parallelism::None()));
+  EXPECT_FALSE(spect3.IsBounded(Parallelism::Max()));
 
   // Add a constraint that makes the program bounded
   prog.AddLinearConstraint(X1(0, 0) + X1(0, 1) + X1(1, 1) <= 43);
   Spectrahedron spect4(prog);
-  EXPECT_TRUE(spect4.IsBounded());
+  EXPECT_TRUE(spect4.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(spect4.IsBounded(Parallelism::Max()));
 
   // Add a constraint that makes the program infeasible (the empty
   // set is bounded)
   prog.AddLinearConstraint(X1(0, 0) >= 50);
   Spectrahedron spect5(prog);
   EXPECT_TRUE(spect5.IsEmpty());
-  EXPECT_TRUE(spect5.IsBounded());
+  EXPECT_TRUE(spect5.IsBounded(Parallelism::None()));
+  EXPECT_TRUE(spect5.IsBounded(Parallelism::Max()));
 }
 
 }  // namespace