diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc index 569190b3735c..1589bb1634a1 100644 --- a/bindings/pydrake/geometry/geometry_py_optimization.cc +++ b/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, diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 7370ff492710..8492175d6e63 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/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) diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index fdd93affb8d2..08524d88c341 100644 --- a/geometry/optimization/BUILD.bazel +++ b/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", ], ) diff --git a/geometry/optimization/cartesian_product.cc b/geometry/optimization/cartesian_product.cc index 0836d6fd131d..a4f3b5f1d183 100644 --- a/geometry/optimization/cartesian_product.cc +++ b/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& A, const Eigen::Ref& b) - : ConvexSet(A.cols()), sets_(sets), A_(A), b_(b) { + : ConvexSet(A.cols()), + sets_(sets), + A_(A), + b_(b), + A_decomp_(Eigen::ColPivHouseholderQR(*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& query_object, @@ -84,6 +89,8 @@ CartesianProduct::CartesianProduct(const QueryObject& 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 CartesianProduct::DoMaybeGetPoint() const { + // Check if all sets are points. + std::vector points; + for (const auto& s : sets_) { + if (std::optional 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& x, double tol) const { int index = 0; diff --git a/geometry/optimization/cartesian_product.h b/geometry/optimization/cartesian_product.h index 6a2c9de97c8a..2c11f614a9d2 100644 --- a/geometry/optimization/cartesian_product.h +++ b/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& A, const Eigen::Ref& b); @@ -76,6 +77,8 @@ class CartesianProduct final : public ConvexSet { bool DoIsBounded() const final; + std::optional DoMaybeGetPoint() const final; + bool DoPointInSet(const Eigen::Ref& x, double tol) const final; @@ -113,6 +116,12 @@ class CartesianProduct final : public ConvexSet { // in the implementation. std::optional A_{std::nullopt}; std::optional 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> A_decomp_{ + std::nullopt}; }; } // namespace optimization diff --git a/geometry/optimization/convex_set.cc b/geometry/optimization/convex_set.cc index 6414ee221f0e..177de6132b31 100644 --- a/geometry/optimization/convex_set.cc +++ b/geometry/optimization/convex_set.cc @@ -31,6 +31,10 @@ bool ConvexSet::IntersectsWith(const ConvexSet& other) const { return result.is_success(); } +std::optional ConvexSet::DoMaybeGetPoint() const { + return std::nullopt; +} + void ConvexSet::AddPointInSetConstraints( solvers::MathematicalProgram* prog, const Eigen::Ref& vars) const { diff --git a/geometry/optimization/convex_set.h b/geometry/optimization/convex_set.h index 541765b5a8b9..bb2f406e2381 100644 --- a/geometry/optimization/convex_set.h +++ b/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 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& 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 DoMaybeGetPoint() const; + /** Non-virtual interface implementation for PointInSet(). @pre x.size() == ambient_dimension() @pre ambient_dimension() > 0 */ diff --git a/geometry/optimization/hpolyhedron.h b/geometry/optimization/hpolyhedron.h index f1f9098142dd..a5d6c9321e54 100644 --- a/geometry/optimization/hpolyhedron.h +++ b/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& x, double tol) const final; diff --git a/geometry/optimization/hyperellipsoid.h b/geometry/optimization/hyperellipsoid.h index f26f635d3a68..2e6a2da65469 100644 --- a/geometry/optimization/hyperellipsoid.h +++ b/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& x, double tol) const final; diff --git a/geometry/optimization/intersection.cc b/geometry/optimization/intersection.cc index c413de693991..7887f989dc75 100644 --- a/geometry/optimization/intersection.cc +++ b/geometry/optimization/intersection.cc @@ -68,6 +68,21 @@ bool Intersection::DoIsBounded() const { "elements is not currently supported."); } +std::optional Intersection::DoMaybeGetPoint() const { + std::optional result; + for (const auto& s : sets_) { + if (std::optional 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& x, double tol) const { for (const auto& s : sets_) { diff --git a/geometry/optimization/intersection.h b/geometry/optimization/intersection.h index cc8514eab5b5..e7b85460f082 100644 --- a/geometry/optimization/intersection.h +++ b/geometry/optimization/intersection.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -42,6 +43,8 @@ class Intersection final : public ConvexSet { bool DoIsBounded() const final; + std::optional DoMaybeGetPoint() const final; + bool DoPointInSet(const Eigen::Ref& x, double tol) const final; diff --git a/geometry/optimization/minkowski_sum.cc b/geometry/optimization/minkowski_sum.cc index 38ce6755c097..96a0f939fbbd 100644 --- a/geometry/optimization/minkowski_sum.cc +++ b/geometry/optimization/minkowski_sum.cc @@ -103,6 +103,22 @@ bool MinkowskiSum::DoIsBounded() const { return true; } +std::optional MinkowskiSum::DoMaybeGetPoint() const { + std::optional result; + for (const auto& s : sets_) { + if (std::optional 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& x, double) const { // TODO(russt): Figure out if there is a general way to communicate tol diff --git a/geometry/optimization/minkowski_sum.h b/geometry/optimization/minkowski_sum.h index db22831936e0..7dfc863b1a32 100644 --- a/geometry/optimization/minkowski_sum.h +++ b/geometry/optimization/minkowski_sum.h @@ -64,6 +64,8 @@ class MinkowskiSum final : public ConvexSet { bool DoIsBounded() const final; + std::optional DoMaybeGetPoint() const final; + bool DoPointInSet(const Eigen::Ref& x, double tol) const final; diff --git a/geometry/optimization/point.cc b/geometry/optimization/point.cc index 69b71bd02654..96f66c172f93 100644 --- a/geometry/optimization/point.cc +++ b/geometry/optimization/point.cc @@ -57,6 +57,10 @@ std::unique_ptr Point::DoClone() const { return std::make_unique(*this); } +std::optional Point::DoMaybeGetPoint() const { + return x_; +} + bool Point::DoPointInSet(const Eigen::Ref& x, double tol) const { return is_approx_equal_abstol(x, x_, tol); diff --git a/geometry/optimization/point.h b/geometry/optimization/point.h index 895bb973a223..11d928364535 100644 --- a/geometry/optimization/point.h +++ b/geometry/optimization/point.h @@ -51,6 +51,8 @@ class Point final : public ConvexSet { bool DoIsBounded() const final { return true; } + std::optional DoMaybeGetPoint() const final; + bool DoPointInSet(const Eigen::Ref& x, double tol) const final; diff --git a/geometry/optimization/spectrahedron.h b/geometry/optimization/spectrahedron.h index 3b5c176a3160..93187a78605f 100644 --- a/geometry/optimization/spectrahedron.h +++ b/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& x, double tol) const final; diff --git a/geometry/optimization/test/cartesian_product_test.cc b/geometry/optimization/test/cartesian_product_test.cc index af287b760d64..d9059b3fcd2d 100644 --- a/geometry/optimization/test/cartesian_product_test.cc +++ b/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); diff --git a/geometry/optimization/test/hpolyhedron_test.cc b/geometry/optimization/test/hpolyhedron_test.cc index 9f25864f2995..209a74a1d9ea 100644 --- a/geometry/optimization/test/hpolyhedron_test.cc +++ b/geometry/optimization/test/hpolyhedron_test.cc @@ -67,6 +67,9 @@ GTEST_TEST(HPolyhedronTest, UnitBoxTest) { EXPECT_TRUE(CompareMatrices(A, Hbox.A())); EXPECT_TRUE(CompareMatrices(b, Hbox.b())); + // Test MaybeGetPoint. + EXPECT_FALSE(H.MaybeGetPoint().has_value()); + // Test PointInSet. EXPECT_TRUE(H.PointInSet(Vector3d(.8, .3, -.9))); EXPECT_TRUE(H.PointInSet(Vector3d(-1.0, 1.0, 1.0))); diff --git a/geometry/optimization/test/hyperellipsoid_test.cc b/geometry/optimization/test/hyperellipsoid_test.cc index 19f637c30d02..e33c44bcf716 100644 --- a/geometry/optimization/test/hyperellipsoid_test.cc +++ b/geometry/optimization/test/hyperellipsoid_test.cc @@ -53,6 +53,9 @@ GTEST_TEST(HyperellipsoidTest, UnitSphereTest) { EXPECT_TRUE(CompareMatrices(A, E_scene_graph.A())); EXPECT_TRUE(CompareMatrices(center, E_scene_graph.center())); + // Test MaybeGetPoint. + EXPECT_FALSE(E.MaybeGetPoint().has_value()); + // Test PointInSet. const Vector3d in1_W{.99, 0, 0}, in2_W{.5, .5, .5}, out1_W{1.01, 0, 0}, out2_W{1.0, 1.0, 1.0}; diff --git a/geometry/optimization/test/intersection_test.cc b/geometry/optimization/test/intersection_test.cc index 280923a55a1f..47a325cd5865 100644 --- a/geometry/optimization/test/intersection_test.cc +++ b/geometry/optimization/test/intersection_test.cc @@ -34,6 +34,11 @@ GTEST_TEST(IntersectionTest, BasicTest) { EXPECT_EQ(S.num_elements(), 2); EXPECT_EQ(S.ambient_dimension(), 2); + // Test MaybeGetPoint. Even though the logical intersection *does* represent a + // point, our implementation doesn't yet have any implementation tactic that + // would identify that condition quickly. + EXPECT_FALSE(S.MaybeGetPoint().has_value()); + // Test PointInSet. Vector2d in, out; in << P1.x(); @@ -58,6 +63,15 @@ GTEST_TEST(IntersectionTest, BasicTest) { EXPECT_FALSE(S2.PointInSet(out)); } +GTEST_TEST(IntersectionTest, TwoIdenticalPoints) { + const Point P1(Vector2d{0.1, 1.2}); + Intersection S(P1, P1); + EXPECT_EQ(S.ambient_dimension(), 2); + ASSERT_TRUE(S.MaybeGetPoint().has_value()); + EXPECT_TRUE(CompareMatrices(S.MaybeGetPoint().value(), P1.x())); + EXPECT_TRUE(S.PointInSet(P1.x())); +} + GTEST_TEST(IntersectionTest, DefaultCtor) { const Intersection dut; EXPECT_EQ(dut.num_elements(), 0); @@ -65,6 +79,7 @@ GTEST_TEST(IntersectionTest, 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))); } @@ -93,6 +108,7 @@ GTEST_TEST(IntersectionTest, TwoBoxes) { EXPECT_TRUE(S.PointInSet(Vector2d{1.9, 0.9})); EXPECT_FALSE(S.PointInSet(Vector2d{1.9, 1.1})); EXPECT_FALSE(S.PointInSet(Vector2d{2.1, 0.9})); + EXPECT_FALSE(S.MaybeGetPoint().has_value()); } GTEST_TEST(IntersectionTest, BoundedTest) { diff --git a/geometry/optimization/test/minkowski_sum_test.cc b/geometry/optimization/test/minkowski_sum_test.cc index 6ca07f22f86e..40f06b61fb9d 100644 --- a/geometry/optimization/test/minkowski_sum_test.cc +++ b/geometry/optimization/test/minkowski_sum_test.cc @@ -31,6 +31,10 @@ GTEST_TEST(MinkowskiSumTest, BasicTest) { EXPECT_EQ(S.num_terms(), 2); EXPECT_EQ(S.ambient_dimension(), 2); + // Test MaybeGetPoint. + ASSERT_TRUE(S.MaybeGetPoint().has_value()); + EXPECT_TRUE(CompareMatrices(S.MaybeGetPoint().value(), P1.x() + P2.x())); + // Test PointInSet. Vector2d in{P1.x() + P2.x()}, out{P1.x() + P2.x() + Vector2d::Constant(0.01)}; EXPECT_TRUE(S.PointInSet(in)); @@ -60,6 +64,7 @@ GTEST_TEST(MinkowskiSumTest, 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))); } @@ -143,6 +148,7 @@ GTEST_TEST(MinkowskiSumTest, TwoBoxes) { EXPECT_TRUE(S.PointInSet(Vector2d{-1, 3})); EXPECT_FALSE(S.PointInSet(Vector2d{-1, 2.9})); EXPECT_FALSE(S.PointInSet(Vector2d{-1.01, 3})); + EXPECT_FALSE(S.MaybeGetPoint().has_value()); } GTEST_TEST(MinkowskiSumTest, CloneTest) { diff --git a/geometry/optimization/test/point_test.cc b/geometry/optimization/test/point_test.cc index 598384006929..d0464ccf43e3 100644 --- a/geometry/optimization/test/point_test.cc +++ b/geometry/optimization/test/point_test.cc @@ -33,6 +33,10 @@ GTEST_TEST(PointTest, BasicTest) { EXPECT_EQ(P.ambient_dimension(), 3); EXPECT_TRUE(CompareMatrices(p_W, P.x())); + // Test MaybeGetPoint. + ASSERT_TRUE(P.MaybeGetPoint().has_value()); + EXPECT_TRUE(CompareMatrices(P.MaybeGetPoint().value(), p_W)); + // Test PointInSet. EXPECT_TRUE(P.PointInSet(p_W)); EXPECT_FALSE(P.PointInSet(p_W + Vector3d::Constant(0.01))); @@ -56,6 +60,7 @@ GTEST_TEST(PointTest, BasicTest) { const Vector3d p2_W{6.2, -.23, -8.2}; P.set_x(p2_W); EXPECT_TRUE(CompareMatrices(p2_W, P.x())); + EXPECT_TRUE(CompareMatrices(P.MaybeGetPoint().value(), p2_W)); } GTEST_TEST(PointTest, DefaultCtor) { @@ -65,6 +70,7 @@ GTEST_TEST(PointTest, 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))); Point P; diff --git a/geometry/optimization/test/spectrahedron_test.cc b/geometry/optimization/test/spectrahedron_test.cc index 8de9f90a1fcb..9dd0c420b6d1 100644 --- a/geometry/optimization/test/spectrahedron_test.cc +++ b/geometry/optimization/test/spectrahedron_test.cc @@ -67,6 +67,7 @@ GTEST_TEST(SpectrahedronTest, TrivialSdp1) { const double kTol{1e-6}; EXPECT_TRUE(spect.PointInSet(x_star, kTol)); EXPECT_FALSE(spect.PointInSet(x_bad, kTol)); + EXPECT_FALSE(spect.MaybeGetPoint().has_value()); MathematicalProgram prog2; auto x2 = prog2.NewContinuousVariables<6>("x"); diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 205c16f51c8a..bf7893d3c7d7 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -43,6 +43,7 @@ GTEST_TEST(VPolytopeTest, TriangleTest) { VPolytope V(triangle); EXPECT_EQ(V.ambient_dimension(), 2); EXPECT_TRUE(CompareMatrices(V.vertices(), triangle)); + EXPECT_FALSE(V.MaybeGetPoint().has_value()); // Check IsBounded (which is trivially true for V Polytopes). EXPECT_TRUE(V.IsBounded()); @@ -66,6 +67,13 @@ GTEST_TEST(VPolytopeTest, TriangleTest) { } } +GTEST_TEST(VPolytopeTest, SinglePoint) { + Vector2d point(2, -1); + VPolytope V(point); + ASSERT_TRUE(V.MaybeGetPoint().has_value()); + EXPECT_TRUE(CompareMatrices(V.MaybeGetPoint().value(), point)); +} + GTEST_TEST(VPolytopeTest, DefaultCtor) { const VPolytope dut; EXPECT_NO_THROW(dut.GetMinimalRepresentation()); @@ -75,6 +83,7 @@ GTEST_TEST(VPolytopeTest, 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))); } diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index b2187f4ea9ca..5522e4de7883 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -299,6 +299,13 @@ std::unique_ptr VPolytope::DoClone() const { return std::make_unique(*this); } +std::optional VPolytope::DoMaybeGetPoint() const { + if (vertices_.cols() == 1) { + return vertices_.col(0); + } + return std::nullopt; +} + bool VPolytope::DoPointInSet(const Eigen::Ref& x, double tol) const { const int n = ambient_dimension(); diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h index 86cf7751624f..65c3f4691280 100644 --- a/geometry/optimization/vpolytope.h +++ b/geometry/optimization/vpolytope.h @@ -89,6 +89,8 @@ class VPolytope final : public ConvexSet { bool DoIsBounded() const final { return true; } + std::optional DoMaybeGetPoint() const final; + bool DoPointInSet(const Eigen::Ref& x, double tol) const final; diff --git a/planning/trajectory_optimization/gcs_trajectory_optimization.cc b/planning/trajectory_optimization/gcs_trajectory_optimization.cc index c4e867d38ab1..74452d448db0 100644 --- a/planning/trajectory_optimization/gcs_trajectory_optimization.cc +++ b/planning/trajectory_optimization/gcs_trajectory_optimization.cc @@ -436,11 +436,9 @@ bool EdgesBetweenSubgraphs::RegionsConnectThroughSubspace( DRAKE_THROW_UNLESS(A.ambient_dimension() > 0); DRAKE_THROW_UNLESS(A.ambient_dimension() == B.ambient_dimension()); DRAKE_THROW_UNLESS(A.ambient_dimension() == subspace.ambient_dimension()); - // TODO(wrangelvid) Replace dynamic cast with a function that checks if the - // convex set degenerates to a point. - if (const Point* pt = dynamic_cast(&subspace)) { + if (std::optional subspace_point = subspace.MaybeGetPoint()) { // If the subspace is a point, then the point must be in both A and B. - return A.PointInSet(pt->x()) && B.PointInSet(pt->x()); + return A.PointInSet(*subspace_point) && B.PointInSet(*subspace_point); } else { // Otherwise, we can formulate a problem to check if a point is contained in // A, B and the subspace.