RobotLocomotion · RussTedrake · Jul 6, 2021 · Jul 3, 2021
diff --git a/bindings/pydrake/geometry_py.cc b/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);
   }
 }
 

diff --git a/bindings/pydrake/test/geometry_test.py b/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):

diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel
@@ -53,6 +53,14 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "convex_set_test",
+    deps = [
+        ":convex_set",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "hpolyhedron_test",
     deps = [

diff --git a/geometry/optimization/convex_set.cc b/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;
+
+ConvexSet& ConvexSets::emplace_back(const ConvexSet& set) {
+  sets_.emplace_back(set.Clone());
+  return *sets_.back();
+}
+
+ConvexSet& ConvexSets::emplace_back(std::unique_ptr<ConvexSet> set) {
+  sets_.emplace_back(std::move(set));
+  return *sets_.back();
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake
diff --git a/geometry/optimization/convex_set.h b/geometry/optimization/convex_set.h
@@ -5,6 +5,7 @@
 #include <utility>
 #include <vector>
 
+#include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/drake_assert.h"
 #include "drake/geometry/geometry_ids.h"
 #include "drake/geometry/query_object.h"
@@ -75,6 +76,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 +94,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 +154,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 +187,38 @@ 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.
+
+Use the move operator/constructor on this collection to transfer ownership
+without forcing a copy of the underlying sets. */
+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. */
+  ConvexSet& emplace_back(const ConvexSet& set);
+
+  /** Takes ownership of @p set and emplaces it at the end of the collection. */
+  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 const reference to the set at @p pos. */
+  const ConvexSet& operator[](size_t pos) const { return *sets_[pos]; }
+
+  /** Returns a (mutable) reference to the set at @p pos. */
+  ConvexSet& operator[](size_t pos) { return *sets_[pos]; }
+
+ private:
+  std::vector<copyable_unique_ptr<ConvexSet>> sets_{};
+};
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake
diff --git a/geometry/optimization/hpolyhedron.cc b/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(

diff --git a/geometry/optimization/hpolyhedron.h b/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;