geometry:optimization Implements IsBounded and Hyperellipsoid Volumes

Also includes a ConvexSets (plural) container as my proposed solution to memory management for workflows like this:
```
ConvexSets MakeIrisObstacles(
    const QueryObject<double>& query_object,
    std::optional<FrameId> reference_frame = std::nullopt);
HPolyhedron Iris(const ConvexSets& obstacles,
                 const Eigen::Ref<const Eigen::VectorXd>& sample,
                 const HPolyhedron& domain,
                 const IrisOptions& options = IrisOptions());
```

Also includes a number of small fix-ups and style updates:
- HyperEllipsoid => Hyperellipsoid (per discussion with Sean)
- AddPointInSetConstraint => AddPointInSetConstraints
- expressed_in => reference_frame
This are API breaking changes, but we marked the classes as Experimental so that we could do this in the early stages.

This is not chopped up quite as finely as I've been trying to do so far.  But it's the last batch of changes, which had been piling up, before I land the IRIS algorithm in the next PR.  I hope it's ok(better?) this way for my reviewers!

bindings/pydrake/geometry_py.cc

@@ -1318,9 +1318,9 @@ void def_geometry_optimization(py::module m) {
             cls_doc.ambient_dimension.doc)
         .def("PointInSet", &ConvexSet::PointInSet, py::arg("x"),
             py::arg("tol") = 1e-8, cls_doc.PointInSet.doc)
-        .def("AddPointInSetConstraint", &ConvexSet::AddPointInSetConstraint,
+        .def("AddPointInSetConstraints", &ConvexSet::AddPointInSetConstraints,
             py::arg("prog"), py::arg("vars"),
-            cls_doc.AddPointInSetConstraint.doc)
+            cls_doc.AddPointInSetConstraints.doc)
         .def("ToShapeWithPose", &ConvexSet::ToShapeWithPose,
             cls_doc.ToShapeWithPose.doc);
   }
@@ -1334,7 +1334,7 @@ void def_geometry_optimization(py::module m) {
         .def(py::init<const QueryObject<double>&, GeometryId,
                  std::optional<FrameId>>(),
             py::arg("query_object"), py::arg("geometry_id"),
-            py::arg("expressed_in") = std::nullopt, cls_doc.ctor.doc_3args)
+            py::arg("reference_frame") = std::nullopt, cls_doc.ctor.doc_3args)
         .def("A", &HPolyhedron::A, cls_doc.A.doc)
         .def("b", &HPolyhedron::b, cls_doc.b.doc)
         .def("MaximumVolumeInscribedEllipsoid",
@@ -1347,17 +1347,17 @@ void def_geometry_optimization(py::module m) {
   }
 
   {
-    const auto& cls_doc = doc.HyperEllipsoid;
-    py::class_<HyperEllipsoid, ConvexSet>(m, "HyperEllipsoid", cls_doc.doc)
+    const auto& cls_doc = doc.Hyperellipsoid;
+    py::class_<Hyperellipsoid, ConvexSet>(m, "Hyperellipsoid", cls_doc.doc)
         .def(py::init<const Eigen::Ref<const Eigen::MatrixXd>&,
                  const Eigen::Ref<const Eigen::VectorXd>&>(),
             py::arg("A"), py::arg("center"), cls_doc.ctor.doc_2args)
         .def(py::init<const QueryObject<double>&, GeometryId,
                  std::optional<FrameId>>(),
             py::arg("query_object"), py::arg("geometry_id"),
-            py::arg("expressed_in") = std::nullopt, cls_doc.ctor.doc_3args)
-        .def("A", &HyperEllipsoid::A, cls_doc.A.doc)
-        .def("center", &HyperEllipsoid::center, cls_doc.center.doc);
+            py::arg("reference_frame") = std::nullopt, cls_doc.ctor.doc_3args)
+        .def("A", &Hyperellipsoid::A, cls_doc.A.doc)
+        .def("center", &Hyperellipsoid::center, cls_doc.center.doc);
   }
 }
 

bindings/pydrake/test/geometry_test.py

@@ -910,7 +910,7 @@ def test_optimization(self):
         np.testing.assert_array_equal(hpoly.A(), A)
         np.testing.assert_array_equal(hpoly.b(), b)
         self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
-        hpoly.AddPointInSetConstraint(prog, x)
+        hpoly.AddPointInSetConstraints(prog, x)
         with self.assertRaisesRegex(
                 RuntimeError, ".*not implemented yet for HPolyhedron.*"):
             hpoly.ToShapeWithPose()
@@ -922,15 +922,15 @@ def test_optimization(self):
         np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
         self.assertIsInstance(
             h_box.MaximumVolumeInscribedEllipsoid(),
-            mut.optimization.HyperEllipsoid)
+            mut.optimization.Hyperellipsoid)
 
-        # Test HyperEllipsoid.
-        ellipsoid = mut.optimization.HyperEllipsoid(A=A, center=b)
+        # Test Hyperellipsoid.
+        ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
         self.assertEqual(ellipsoid.ambient_dimension(), 3)
         np.testing.assert_array_equal(ellipsoid.A(), A)
         np.testing.assert_array_equal(ellipsoid.center(), b)
         self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
-        ellipsoid.AddPointInSetConstraint(prog, x)
+        ellipsoid.AddPointInSetConstraints(prog, x)
         shape, pose = ellipsoid.ToShapeWithPose()
         self.assertIsInstance(shape, mut.Ellipsoid)
         self.assertIsInstance(pose, RigidTransform)
@@ -957,11 +957,11 @@ def test_optimization(self):
         query_object = scene_graph.get_query_output_port().Eval(context)
         H = mut.optimization.HPolyhedron(
             query_object=query_object, geometry_id=box_geometry_id,
-            expressed_in=scene_graph.world_frame_id())
+            reference_frame=scene_graph.world_frame_id())
         self.assertEqual(H.ambient_dimension(), 3)
-        E = mut.optimization.HyperEllipsoid(
+        E = mut.optimization.Hyperellipsoid(
             query_object=query_object, geometry_id=sphere_geometry_id,
-            expressed_in=scene_graph.world_frame_id())
+            reference_frame=scene_graph.world_frame_id())
         self.assertEqual(E.ambient_dimension(), 3)
 
     def test_deprecated_struct_member(self):

geometry/optimization/BUILD.bazel

@@ -53,6 +53,13 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "convex_set_test",
+    deps = [
+        ":convex_set",
+    ],
+)
+
 drake_cc_googletest(
     name = "hpolyhedron_test",
     deps = [

geometry/optimization/convex_set.cc

@@ -1,5 +1,6 @@
 #include "drake/geometry/optimization/convex_set.h"
 
+#include <algorithm>
 #include <limits>
 #include <memory>
 
@@ -31,6 +32,20 @@ ConvexSet::AddPointInNonnegativeScalingConstraints(
   return constraints;
 }
 
+ConvexSets::ConvexSets() = default;
+
+ConvexSets::~ConvexSets() = default;
+
+const ConvexSet& ConvexSets::emplace_back(const ConvexSet& set) {
+  sets_.emplace_back(set.Clone());
+  return *sets_.back();
+}
+
+const ConvexSet& ConvexSets::emplace_back(std::unique_ptr<ConvexSet> set) {
+  sets_.emplace_back(std::move(set));
+  return *sets_.back();
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/convex_set.h

@@ -75,6 +75,13 @@ class ConvexSet : public ShapeReifier {
   rank(A)`. */
   int ambient_dimension() const { return ambient_dimension_; }
 
+  /** 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 { return DoIsBounded(); }
+
   /** Returns true iff the point x is contained in the set. */
   bool PointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                   double tol = 0) const {
@@ -86,11 +93,11 @@ class ConvexSet : public ShapeReifier {
   // dependent.
   /** Adds a constraint to an existing MathematicalProgram enforcing that the
   point defined by vars is inside the set. */
-  void AddPointInSetConstraint(
+  void AddPointInSetConstraints(
       solvers::MathematicalProgram* prog,
       const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const {
     DRAKE_DEMAND(vars.size() == ambient_dimension());
-    return DoAddPointInSetConstraint(prog, vars);
+    return DoAddPointInSetConstraints(prog, vars);
   }
 
   /** Let S be this convex set.  When S is bounded, this method adds the convex
@@ -146,10 +153,12 @@ class ConvexSet : public ShapeReifier {
             int ambient_dimension);
 
   // Non-virtual interface implementations.
+  virtual bool DoIsBounded() const = 0;
+
   virtual bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                             double tol) const = 0;
 
-  virtual void DoAddPointInSetConstraint(
+  virtual void DoAddPointInSetConstraints(
       solvers::MathematicalProgram* prog,
       const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const = 0;
 
@@ -177,6 +186,34 @@ std::unique_ptr<ConvexSet> ConvexSetCloner(const ConvexSet& other) {
   return std::make_unique<Derived>(typed_other);
 }
 
+// TODO(russt): Consider providing an iterator.
+/** Wraps a collection of owned ConvexSet objects, and provides const access to
+ * them.
+ */
+class ConvexSets {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConvexSets)
+
+  /** Constructs an empty collection. */
+  ConvexSets();
+  virtual ~ConvexSets();
+
+  /** Emplaces a copy of @p set at the end of the collection. */
+  const ConvexSet& emplace_back(const ConvexSet& set);
+
+  /** Takes ownership of @p set and emplaces it at the end of the collection. */
+  const ConvexSet& emplace_back(std::unique_ptr<ConvexSet> set);
+
+  /** Returns the number of elements in the collection. */
+  int size() const { return static_cast<int>(sets_.size()); }
+
+  /** Returns a reference to the set at @p pos. */
+  const ConvexSet& operator[](size_t pos) const { return *sets_[pos]; }
+
+ private:
+  std::vector<std::shared_ptr<ConvexSet>> sets_{};
+};
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/hpolyhedron.cc

@@ -30,18 +30,22 @@ HPolyhedron::HPolyhedron(const Eigen::Ref<const MatrixXd>& A,
                          const Eigen::Ref<const VectorXd>& b)
     : ConvexSet(&ConvexSetCloner<HPolyhedron>, A.cols()), A_{A}, b_{b} {
   DRAKE_DEMAND(A.rows() == b.size());
+  // Note: If necessary, we could support infinite b, either by removing the
+  // corresponding rows of A (since the constraint is vacuous), or checking
+  // this explicitly in all relevant computations (like IsBounded).
+  DRAKE_DEMAND(b.array().isFinite().all());
 }
 
 HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
                          GeometryId geometry_id,
-                         std::optional<FrameId> expressed_in)
+                         std::optional<FrameId> reference_frame)
     : ConvexSet(&ConvexSetCloner<HPolyhedron>, 3) {
   std::pair<MatrixXd, VectorXd> Ab_G;
   query_object.inspector().GetShape(geometry_id).Reify(this, &Ab_G);
 
-  const RigidTransformd X_WE = expressed_in
-                                   ? query_object.GetPoseInWorld(*expressed_in)
-                                   : RigidTransformd::Identity();
+  const RigidTransformd X_WE =
+      reference_frame ? query_object.GetPoseInWorld(*reference_frame)
+                      : RigidTransformd::Identity();
   const RigidTransformd& X_WG = query_object.GetPoseInWorld(geometry_id);
   const RigidTransformd X_GE = X_WG.InvertAndCompose(X_WE);
   // A_G*(p_GE + R_GE*p_EE_var) ≤ b_G
@@ -51,7 +55,7 @@ HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
 
 HPolyhedron::~HPolyhedron() = default;
 
-HyperEllipsoid HPolyhedron::MaximumVolumeInscribedEllipsoid() const {
+Hyperellipsoid HPolyhedron::MaximumVolumeInscribedEllipsoid() const {
   MathematicalProgram prog;
   const int N = this->ambient_dimension();
   MatrixXDecisionVariable C = prog.NewSymmetricContinuousVariables(N, "C");
@@ -78,7 +82,7 @@ HyperEllipsoid HPolyhedron::MaximumVolumeInscribedEllipsoid() const {
         "bounded.",
         result.get_solver_id().name(), result.get_solution_result()));
   }
-  return HyperEllipsoid(result.GetSolution(C).inverse(), result.GetSolution(d));
+  return Hyperellipsoid(result.GetSolution(C).inverse(), result.GetSolution(d));
 }
 
 HPolyhedron HPolyhedron::MakeBox(const Eigen::Ref<const VectorXd>& lb,
@@ -97,13 +101,38 @@ HPolyhedron HPolyhedron::MakeUnitBox(int dim) {
   return MakeBox(VectorXd::Constant(dim, -1.0), VectorXd::Constant(dim, 1.0));
 }
 
+bool HPolyhedron::DoIsBounded() const {
+  if (A_.rows() < A_.cols()) {
+    return false;
+  }
+  Eigen::ColPivHouseholderQR<MatrixXd> qr(A_);
+  if (qr.dimensionOfKernel() > 0) {
+    return false;
+  }
+  // Stiemke's theorem of alternatives says that, given A with ker(A) = {0}, we
+  // either have existence of x ≠ 0 such that Ax ≥ 0 or we have existence of y
+  // > 0 such that y^T A = 0.  Since any y that verifies the second condition
+  // can be arbitrarily scaled, and would still pass the second condition,
+  // instead of asking y > 0, we can equivalently ask y ≥ 1.  So boundedness
+  // corresponds to the following LP being feasible: find y s.t. y ≥ 1, y^T A =
+  // 0.
+  MathematicalProgram prog;
+  auto y = prog.NewContinuousVariables(A_.rows(), "y");
+  prog.AddBoundingBoxConstraint(1.0, std::numeric_limits<double>::infinity(),
+                                y);
+  prog.AddLinearEqualityConstraint(A_.transpose(), VectorXd::Zero(A_.cols()),
+                                   y);
+  auto result = solvers::Solve(prog);
+  return result.is_success();
+}
+
 bool HPolyhedron::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                double tol) const {
   DRAKE_DEMAND(A_.cols() == x.size());
   return ((A_ * x).array() <= b_.array() + tol).all();
 }
 
-void HPolyhedron::DoAddPointInSetConstraint(
+void HPolyhedron::DoAddPointInSetConstraints(
     MathematicalProgram* prog,
     const Eigen::Ref<const VectorXDecisionVariable>& vars) const {
   prog->AddLinearConstraint(

geometry/optimization/hpolyhedron.h

@@ -27,12 +27,12 @@ class HPolyhedron final : public ConvexSet {
               const Eigen::Ref<const Eigen::VectorXd>& b);
 
   /** Constructs a new HPolyhedron from a SceneGraph geometry and pose in the
-  @p expressed_in frame, obtained via the QueryObject.  If @p expressed_in
+  @p reference_frame frame, obtained via the QueryObject.  If @p reference_frame
   frame is std::nullopt, then it will be expressed in the world frame.
 
   @throws std::exception the geometry is not a convex polytope. */
   HPolyhedron(const QueryObject<double>& query_object, GeometryId geometry_id,
-              std::optional<FrameId> expressed_in = std::nullopt);
+              std::optional<FrameId> reference_frame = std::nullopt);
   // TODO(russt): Add a method/constructor that would create the geometry using
   // SceneGraph's AABB or OBB representation (for arbitrary objects) pending
   // #15121.
@@ -45,6 +45,13 @@ class HPolyhedron final : public ConvexSet {
   /** Returns the half-space representation vector b. */
   const Eigen::VectorXd& b() const { return b_; }
 
+  /** Returns true iff the set is bounded, e.g. there exists an element-wise
+  finite lower and upper bound for the set.  For HPolyhedron, while there are
+  some fast checks to confirm a set is unbounded, confirming boundedness
+  requires solving a linear program (based on Stiemke’s theorem of
+  alternatives). */
+  using ConvexSet::IsBounded;
+
   // TODO(russt): Add bool IsBounded() so users can test the precondition.
   /** Solves a semi-definite program to compute the inscribed ellipsoid.
   From Section 8.4.2 in Boyd and Vandenberghe, 2004, we solve
@@ -59,7 +66,7 @@ class HPolyhedron final : public ConvexSet {
   @pre the HPolyhedron is bounded.
   @throws std::exception if the solver fails to solve the problem.
   */
-  HyperEllipsoid MaximumVolumeInscribedEllipsoid() const;
+  Hyperellipsoid MaximumVolumeInscribedEllipsoid() const;
 
   /** Constructs a polyhedron as an axis-aligned box from the lower and upper
    * corners. */
@@ -71,10 +78,12 @@ class HPolyhedron final : public ConvexSet {
   static HPolyhedron MakeUnitBox(int dim);
 
  private:
+  bool DoIsBounded() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 
-  void DoAddPointInSetConstraint(
+  void DoAddPointInSetConstraints(
       solvers::MathematicalProgram* prog,
       const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars)
       const final;