ConvexSet can report when its just a point (#19279)

This allows algorithms to customize themselves for degenerate sets,
for performance.

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -46,6 +46,8 @@ void DefineGeometryOptimization(py::module m) {
         .def("IntersectsWith", &ConvexSet::IntersectsWith, py::arg("other"),
             cls_doc.IntersectsWith.doc)
         .def("IsBounded", &ConvexSet::IsBounded, cls_doc.IsBounded.doc)
+        .def("MaybeGetPoint", &ConvexSet::MaybeGetPoint,
+            cls_doc.MaybeGetPoint.doc)
         .def("PointInSet", &ConvexSet::PointInSet, py::arg("x"),
             py::arg("tol") = 1e-8, cls_doc.PointInSet.doc)
         .def("AddPointInSetConstraints", &ConvexSet::AddPointInSetConstraints,

bindings/pydrake/geometry/test/optimization_test.py

@@ -45,6 +45,7 @@ def test_point_convex_set(self):
         point = mut.Point(p)
         self.assertEqual(point.ambient_dimension(), 3)
         np.testing.assert_array_equal(point.x(), p)
+        np.testing.assert_array_equal(point.MaybeGetPoint(), p)
         point.set_x(x=2*p)
         np.testing.assert_array_equal(point.x(), 2*p)
         point.set_x(x=p)

geometry/optimization/BUILD.bazel

@@ -211,6 +211,7 @@ drake_cc_googletest(
         ":convex_set",
         ":test_utilities",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
     ],
 )
 

geometry/optimization/cartesian_product.cc

@@ -52,13 +52,18 @@ CartesianProduct::CartesianProduct(const ConvexSet& setA, const ConvexSet& setB)
 CartesianProduct::CartesianProduct(const ConvexSets& sets,
                                    const Eigen::Ref<const MatrixXd>& A,
                                    const Eigen::Ref<const VectorXd>& b)
-    : ConvexSet(A.cols()), sets_(sets), A_(A), b_(b) {
+    : ConvexSet(A.cols()),
+      sets_(sets),
+      A_(A),
+      b_(b),
+      A_decomp_(Eigen::ColPivHouseholderQR<Eigen::MatrixXd>(*A_)) {
   const int y_ambient_dimension = SumAmbientDimensions(sets);
   const int x_ambient_dimension = ambient_dimension();
   DRAKE_THROW_UNLESS(A_->rows() == y_ambient_dimension);
   DRAKE_THROW_UNLESS(b_->rows() == y_ambient_dimension);
   DRAKE_THROW_UNLESS(A_->cols() == x_ambient_dimension);
-  DRAKE_THROW_UNLESS(A_->colPivHouseholderQr().rank() == A_->cols());
+  // Ensure that A is injective.
+  DRAKE_THROW_UNLESS(A_decomp_->rank() == A_->cols());
 }
 
 CartesianProduct::CartesianProduct(const QueryObject<double>& query_object,
@@ -84,6 +89,8 @@ CartesianProduct::CartesianProduct(const QueryObject<double>& query_object,
 
   A_ = X_GF.rotation().matrix();
   b_ = X_GF.translation();
+  // N.B. We leave A_decomp_ unset here. It's only used by MaybeGetPoint and
+  // there it's irrelevant because a cylinder is never a point anyway.
 }
 
 CartesianProduct::~CartesianProduct() = default;
@@ -107,6 +114,43 @@ bool CartesianProduct::DoIsBounded() const {
   return true;
 }
 
+std::optional<VectorXd> CartesianProduct::DoMaybeGetPoint() const {
+  // Check if all sets are points.
+  std::vector<VectorXd> points;
+  for (const auto& s : sets_) {
+    if (std::optional<VectorXd> point = s->MaybeGetPoint()) {
+      points.push_back(std::move(*point));
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  // Stack the points.
+  const int y_ambient_dimension = A_ ? A_->rows() : ambient_dimension();
+  int start = 0;
+  VectorXd y(y_ambient_dimension);
+  for (const VectorXd& point : points) {
+    const int point_size = point.size();
+    y.segment(start, point_size) = point;
+    start += point_size;
+  }
+  DRAKE_DEMAND(start == y.size());
+
+  // When A and b are NOT in use, x is just y.
+  if (!A_.has_value()) {
+    return y;
+  }
+
+  // When A_ is in use, either A was passed to the constructor or we denote a
+  // cylinder. In the former case, we have the QR decomp already (set in the
+  // constructor). In the latter case, we can't possibly have cleared the "all
+  // sets are points" checks earlier in this function.
+  DRAKE_DEMAND(A_decomp_.has_value());
+
+  // Solve for x.
+  return A_decomp_->solve(y - *b_);
+}
+
 bool CartesianProduct::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                     double tol) const {
   int index = 0;

geometry/optimization/cartesian_product.h

@@ -37,7 +37,8 @@ class CartesianProduct final : public ConvexSet {
   CartesianProduct(const ConvexSet& setA, const ConvexSet& setB);
 
   /** Constructs the product of convex sets in the transformed coordinates:
-  {x | y = Ax + b, y ∈ Y₁ × Y₂ × ⋯ × Yₙ}. `A` must be full column rank. */
+  {x | y = Ax + b, y ∈ Y₁ × Y₂ × ⋯ × Yₙ}.
+  @throws std::exception when `A` is not full column rank. */
   CartesianProduct(const ConvexSets& sets,
                    const Eigen::Ref<const Eigen::MatrixXd>& A,
                    const Eigen::Ref<const Eigen::VectorXd>& b);
@@ -76,6 +77,8 @@ class CartesianProduct final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 
@@ -113,6 +116,12 @@ class CartesianProduct final : public ConvexSet {
   // in the implementation.
   std::optional<Eigen::MatrixXd> A_{std::nullopt};
   std::optional<Eigen::VectorXd> b_{std::nullopt};
+
+  // When an `A` is passed to the constructor, we'll compute its decomposition
+  // and store it here for later use. Note that even though the constructor for
+  // a scene graph cylinder sets A_, it does not set A_decomp_.
+  std::optional<Eigen::ColPivHouseholderQR<Eigen::MatrixXd>> A_decomp_{
+      std::nullopt};
 };
 
 }  // namespace optimization

geometry/optimization/convex_set.cc

@@ -31,6 +31,10 @@ bool ConvexSet::IntersectsWith(const ConvexSet& other) const {
   return result.is_success();
 }
 
+std::optional<Eigen::VectorXd> ConvexSet::DoMaybeGetPoint() const {
+  return std::nullopt;
+}
+
 void ConvexSet::AddPointInSetConstraints(
     solvers::MathematicalProgram* prog,
     const Eigen::Ref<const solvers::VectorXDecisionVariable>& vars) const {

geometry/optimization/convex_set.h

@@ -90,6 +90,19 @@ class ConvexSet : public ShapeReifier {
     return DoIsBounded();
   }
 
+  /** If this set trivially contains exactly one point, returns the value of
+  that point. Otherwise, returns nullopt. When ambient_dimension is zero,
+  returns nullopt. By "trivially", we mean that representation of the set
+  structurally maps to a single point; if checking for point-ness would require
+  solving an optimization program, returns nullopt. In other words, this is a
+  relatively cheap function to call. */
+  std::optional<Eigen::VectorXd> MaybeGetPoint() const {
+    if (ambient_dimension() == 0) {
+      return std::nullopt;
+    }
+    return DoMaybeGetPoint();
+  }
+
   /** Returns true iff the point x is contained in the set.  When
   ambient_dimension is zero, returns false. */
   bool PointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
@@ -193,6 +206,12 @@ class ConvexSet : public ShapeReifier {
   @pre ambient_dimension() > 0 */
   virtual bool DoIsBounded() const = 0;
 
+  /** Non-virtual interface implementation for MaybeGetPoint(). The default
+  implementation returns nullopt. Sets that can model a single point should
+  override with a custom implementation.
+  @pre ambient_dimension() > 0 */
+  virtual std::optional<Eigen::VectorXd> DoMaybeGetPoint() const;
+
   /** Non-virtual interface implementation for PointInSet().
   @pre x.size() == ambient_dimension()
   @pre ambient_dimension() > 0 */

geometry/optimization/hpolyhedron.h

@@ -212,6 +212,8 @@ class HPolyhedron final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  // N.B. No need to override DoMaybeGetPoint here.
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/hyperellipsoid.h

@@ -98,6 +98,8 @@ class Hyperellipsoid final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  // N.B. No need to override DoMaybeGetPoint here.
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/intersection.cc

@@ -68,6 +68,21 @@ bool Intersection::DoIsBounded() const {
       "elements is not currently supported.");
 }
 
+std::optional<VectorXd> Intersection::DoMaybeGetPoint() const {
+  std::optional<VectorXd> result;
+  for (const auto& s : sets_) {
+    if (std::optional<VectorXd> point = s->MaybeGetPoint()) {
+      if (result.has_value() && !(*point == *result)) {
+        return std::nullopt;
+      }
+      result = std::move(point);
+    } else {
+      return std::nullopt;
+    }
+  }
+  return result;
+}
+
 bool Intersection::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                 double tol) const {
   for (const auto& s : sets_) {

geometry/optimization/intersection.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -42,6 +43,8 @@ class Intersection final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/minkowski_sum.cc

@@ -103,6 +103,22 @@ bool MinkowskiSum::DoIsBounded() const {
   return true;
 }
 
+std::optional<VectorXd> MinkowskiSum::DoMaybeGetPoint() const {
+  std::optional<VectorXd> result;
+  for (const auto& s : sets_) {
+    if (std::optional<VectorXd> point = s->MaybeGetPoint()) {
+      if (result.has_value()) {
+        *result += *point;
+      } else {
+        result = std::move(point);
+      }
+    } else {
+      return std::nullopt;
+    }
+  }
+  return result;
+}
+
 bool MinkowskiSum::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                                 double) const {
   // TODO(russt): Figure out if there is a general way to communicate tol

geometry/optimization/minkowski_sum.h

@@ -64,6 +64,8 @@ class MinkowskiSum final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/point.cc

@@ -57,6 +57,10 @@ std::unique_ptr<ConvexSet> Point::DoClone() const {
   return std::make_unique<Point>(*this);
 }
 
+std::optional<VectorXd> Point::DoMaybeGetPoint() const {
+  return x_;
+}
+
 bool Point::DoPointInSet(const Eigen::Ref<const VectorXd>& x,
                          double tol) const {
   return is_approx_equal_abstol(x, x_, tol);

geometry/optimization/point.h

@@ -51,6 +51,8 @@ class Point final : public ConvexSet {
 
   bool DoIsBounded() const final { return true; }
 
+  std::optional<Eigen::VectorXd> DoMaybeGetPoint() const final;
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/spectrahedron.h

@@ -43,6 +43,8 @@ class Spectrahedron final : public ConvexSet {
 
   bool DoIsBounded() const final;
 
+  // N.B. No need to override DoMaybeGetPoint here.
+
   bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
                     double tol) const final;
 

geometry/optimization/test/cartesian_product_test.cc

@@ -7,6 +7,7 @@
 #include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/eigen_types.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/geometry/geometry_frame.h"
 #include "drake/geometry/optimization/hpolyhedron.h"
 #include "drake/geometry/optimization/point.h"
@@ -20,6 +21,7 @@ namespace drake {
 namespace geometry {
 namespace optimization {
 
+using drake::Vector1d;
 using Eigen::Matrix;
 using Eigen::Vector2d;
 using Eigen::Vector3d;
@@ -42,6 +44,10 @@ GTEST_TEST(CartesianProductTest, BasicTest) {
   EXPECT_TRUE(internal::CheckAddPointInSetConstraints(S, in));
   EXPECT_FALSE(internal::CheckAddPointInSetConstraints(S, out));
 
+  // Test MaybeGetPoint.
+  ASSERT_TRUE(S.MaybeGetPoint().has_value());
+  EXPECT_TRUE(CompareMatrices(S.MaybeGetPoint().value(), in));
+
   // Test IsBounded.
   EXPECT_TRUE(S.IsBounded());
 
@@ -63,6 +69,7 @@ GTEST_TEST(CartesianProductTest, DefaultCtor) {
   EXPECT_EQ(dut.ambient_dimension(), 0);
   EXPECT_FALSE(dut.IntersectsWith(dut));
   EXPECT_TRUE(dut.IsBounded());
+  EXPECT_FALSE(dut.MaybeGetPoint().has_value());
   EXPECT_FALSE(dut.PointInSet(Eigen::VectorXd::Zero(0)));
 }
 
@@ -117,6 +124,7 @@ GTEST_TEST(CartesianProductTest, FromSceneGraph) {
     EXPECT_TRUE(S.PointInSet(in_W.col(i), kTol));
     EXPECT_FALSE(S.PointInSet(out_W.col(i), kTol));
   }
+  EXPECT_FALSE(S.MaybeGetPoint().has_value());
 
   // Test reference_frame frame.
   SourceId source_id = scene_graph->RegisterSource("F");
@@ -144,6 +152,7 @@ GTEST_TEST(CartesianProductTest, TwoBoxes) {
   HPolyhedron H2 = HPolyhedron::MakeBox(Vector2d{-2, 2}, Vector2d{0, 4});
   CartesianProduct S(H1, H2);
   EXPECT_TRUE(S.IsBounded());
+  EXPECT_FALSE(S.MaybeGetPoint().has_value());
   EXPECT_TRUE(S.PointInSet(Vector4d{1.9, 1.9, -.1, 3.9}));
   EXPECT_FALSE(S.PointInSet(Vector4d{1.9, 1.9, -.1, 4.1}));
   EXPECT_FALSE(S.PointInSet(Vector4d{2.1, 1.9, -.1, 3.9}));
@@ -175,6 +184,51 @@ GTEST_TEST(CartesianProductTest, UnboundedSets) {
   EXPECT_FALSE(S4.IsBounded());
 }
 
+GTEST_TEST(CartesianProductTest, ScaledPoints) {
+  const Point P1(Vector1d{1.2});
+  const Point P2(Vector1d{3.4});
+  // clang-format off
+  Eigen::Matrix2d A;
+  A << -2, 0,
+        0, 5;
+  // clang-format on
+  Vector2d b{0, 3};
+  const CartesianProduct S(MakeConvexSets(P1, P2), A, b);
+
+  const double kTol = 1e-15;
+  const Vector2d in{-0.6, 0.08};
+  EXPECT_TRUE(S.PointInSet(in, kTol));
+  ASSERT_TRUE(S.MaybeGetPoint().has_value());
+  EXPECT_TRUE(CompareMatrices(S.MaybeGetPoint().value(), in, kTol));
+}
+
+GTEST_TEST(CartesianProductTest, ScaledPointInjective) {
+  const Point P(Vector3d{42, 42, 0});
+  // clang-format off
+  Eigen::MatrixXd A(3, 2);
+  A << 1, 0,
+       0, 1,
+       0, 0;
+  // clang-format on
+  Vector3d b{22, 22, 0};
+  const CartesianProduct S(MakeConvexSets(P), A, b);
+
+  const double kTol = 1e-15;
+  const Vector2d in{20, 20};
+  EXPECT_TRUE(S.PointInSet(in, kTol));
+  ASSERT_TRUE(S.MaybeGetPoint().has_value());
+  EXPECT_TRUE(CompareMatrices(S.MaybeGetPoint().value(), in, kTol));
+}
+
+GTEST_TEST(CartesianProductTest, ScaledPointNotInjectiveFail) {
+  const Point P(Vector1d{0});
+  Eigen::MatrixXd A(1, 2);
+  A << 1, 0;
+  Vector1d b{0};
+  DRAKE_EXPECT_THROWS_MESSAGE(CartesianProduct(MakeConvexSets(P), A, b),
+                              ".*rank.*");
+}
+
 GTEST_TEST(CartesianProductTest, CloneTest) {
   const Point P1(Vector2d{1.2, 3.4}), P2(Vector2d{5.6, 7.8});
   const CartesianProduct S(P1, P2);