From b061cd0f97ebd51cca9e70ae7453dde2cac41816 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Tue, 29 Jun 2021 22:00:18 -0400 Subject: [PATCH] geometry:optimization: Add Clone method to ConvexSet hierarchy. --- geometry/optimization/convex_set.cc | 20 +++++++---- geometry/optimization/convex_set.h | 35 +++++++++++++++++-- geometry/optimization/test/convex_set_test.cc | 20 +++++++++++ 3 files changed, 66 insertions(+), 9 deletions(-) diff --git a/geometry/optimization/convex_set.cc b/geometry/optimization/convex_set.cc index cc473fa1130a..78cb415e9f35 100644 --- a/geometry/optimization/convex_set.cc +++ b/geometry/optimization/convex_set.cc @@ -22,15 +22,18 @@ using solvers::VectorXDecisionVariable; using std::pair; using std::sqrt; +std::unique_ptr ConvexSet::Clone() const { return cloner_(*this); } + HPolyhedron::HPolyhedron(const Eigen::Ref& A, const Eigen::Ref& b) - : ConvexSet(A.cols()), A_{A}, b_{b} { + : ConvexSet(ConvexSetTag(), A.cols()), A_{A}, b_{b} { DRAKE_DEMAND(A.rows() == b.size()); } HPolyhedron::HPolyhedron(const QueryObject& query_object, GeometryId geometry_id, - std::optional expressed_in) : ConvexSet(3) { + std::optional expressed_in) + : ConvexSet(ConvexSetTag(), 3) { std::pair Ab_G; query_object.inspector().GetShape(geometry_id).Reify(this, &Ab_G); @@ -130,14 +133,17 @@ void HPolyhedron::ImplementGeometry(const Box& box, void* data) { } HyperEllipsoid::HyperEllipsoid(const Eigen::Ref& A, - const Eigen::Ref& center) - : ConvexSet(center.size()), A_{A}, center_{center} { + const Eigen::Ref& center) + : ConvexSet(ConvexSetTag(), center.size()), + A_{A}, + center_{center} { DRAKE_DEMAND(A.rows() == center.size()); } -HyperEllipsoid::HyperEllipsoid( - const QueryObject& query_object, GeometryId geometry_id, - std::optional expressed_in) : ConvexSet(3) { +HyperEllipsoid::HyperEllipsoid(const QueryObject& query_object, + GeometryId geometry_id, + std::optional expressed_in) + : ConvexSet(ConvexSetTag(), 3) { Eigen::Matrix3d A_G; query_object.inspector().GetShape(geometry_id).Reify(this, &A_G); // p_GG_varᵀ * A_Gᵀ * A_G * p_GG_var ≤ 1 diff --git a/geometry/optimization/convex_set.h b/geometry/optimization/convex_set.h index 8e37e0d3655e..e55e4151fa30 100644 --- a/geometry/optimization/convex_set.h +++ b/geometry/optimization/convex_set.h @@ -54,6 +54,12 @@ The geometry::optimization tools support: @ingroup solvers */ +/** Simple struct for instantiating the type-specific ConvexSet functionality. + A class derived from the ConvexSet class will invoke the parent's constructor + as ConvexSet(ConvexSetTag()). */ +template +struct ConvexSetTag{}; + /** Abstract base class for defining a convex set. @ingroup geometry_optimization */ @@ -61,6 +67,9 @@ class ConvexSet : public ShapeReifier { public: virtual ~ConvexSet() {} + /** Creates a unique deep copy of this set. */ + std::unique_ptr Clone() const; + /** Returns the dimension of the vector space in which the elements of this set are evaluated. Contrast this with the `affine dimension`: the dimension of the smallest affine subset of the ambient space that contains @@ -100,8 +109,29 @@ class ConvexSet : public ShapeReifier { protected: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConvexSet) - explicit ConvexSet(int ambient_dimension) - : ambient_dimension_{ambient_dimension} {} + /** A derived class should invoke this with e.g.: + ``` + class MyConvexSet final : public ConvexSet { + public: + MyConvexSet() : ConvexSet(ConvexSetTag()) {} + ... + }; + ``` + */ + template + ConvexSet(ConvexSetTag, int ambient_dimension) + : ambient_dimension_{ambient_dimension} { + static_assert(std::is_base_of_v, + "Concrete sets *must* be derived from the ConvexSet class"); + + DRAKE_DEMAND(ambient_dimension >= 0); + + cloner_ = [](const ConvexSet& set) { + DRAKE_DEMAND(typeid(set) == typeid(S)); + const S& derived_set = static_cast(set); + return std::unique_ptr(new S(derived_set)); + }; + } // Non-virtual interface implementations. virtual bool DoPointInSet(const Eigen::Ref& x, @@ -115,6 +145,7 @@ class ConvexSet : public ShapeReifier { DoToShapeWithPose() const = 0; int ambient_dimension_{0}; + std::function(const ConvexSet&)> cloner_; }; // Forward declaration. diff --git a/geometry/optimization/test/convex_set_test.cc b/geometry/optimization/test/convex_set_test.cc index d004e363037d..536780741871 100644 --- a/geometry/optimization/test/convex_set_test.cc +++ b/geometry/optimization/test/convex_set_test.cc @@ -214,6 +214,16 @@ GTEST_TEST(HPolyhedronTest, InscribedEllipsoidTest) { EXPECT_NEAR(polytope_halfspace_residue.minCoeff(), 0, kTol); } +GTEST_TEST(HPolyhedronTest, CloneTest) { + HPolyhedron H = HPolyhedron::MakeBox(Vector3d{-3, -4, -5}, Vector3d{6, 7, 8}); + std::unique_ptr clone = H.Clone(); + EXPECT_EQ(clone->ambient_dimension(), H.ambient_dimension()); + HPolyhedron* ptr = dynamic_cast(clone.get()); + EXPECT_NE(ptr, nullptr); + EXPECT_TRUE(CompareMatrices(H.A(), ptr->A())); + EXPECT_TRUE(CompareMatrices(H.b(), ptr->b())); +} + GTEST_TEST(HyperEllipsoidTest, UnitSphereTest) { // Test constructor. const Eigen::Matrix3d A = Eigen::Matrix3d::Identity(); @@ -388,6 +398,16 @@ GTEST_TEST(HyperEllipsoidTest, UnitSphere6DTest) { EXPECT_FALSE(E.PointInSet(out2)); } +GTEST_TEST(HyperEllipsoidTest, CloneTest) { + HyperEllipsoid E(Eigen::Matrix::Identity(), Vector6d::Zero()); + std::unique_ptr clone = E.Clone(); + EXPECT_EQ(clone->ambient_dimension(), E.ambient_dimension()); + HyperEllipsoid* ptr = dynamic_cast(clone.get()); + EXPECT_NE(ptr, nullptr); + EXPECT_TRUE(CompareMatrices(E.A(), ptr->A())); + EXPECT_TRUE(CompareMatrices(E.center(), ptr->center())); +} + } // namespace optimization } // namespace geometry } // namespace drake