geometry:optimization: Add Clone method to ConvexSet hierarchy.

geometry/optimization/convex_set.cc

@@ -22,15 +22,18 @@ using solvers::VectorXDecisionVariable;
 using std::pair;
 using std::sqrt;
 
+std::unique_ptr<ConvexSet> ConvexSet::Clone() const { return cloner_(*this); }
+
 HPolyhedron::HPolyhedron(const Eigen::Ref<const Eigen::MatrixXd>& A,
                          const Eigen::Ref<const Eigen::VectorXd>& b)
-    : ConvexSet(A.cols()), A_{A}, b_{b} {
+    : ConvexSet(ConvexSetTag<HPolyhedron>(), A.cols()), A_{A}, b_{b} {
   DRAKE_DEMAND(A.rows() == b.size());
 }
 
 HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
                          GeometryId geometry_id,
-                         std::optional<FrameId> expressed_in) : ConvexSet(3) {
+                         std::optional<FrameId> expressed_in)
+    : ConvexSet(ConvexSetTag<HPolyhedron>(), 3) {
   std::pair<Eigen::MatrixXd, Eigen::VectorXd> 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<const Eigen::MatrixXd>& A,
-                     const Eigen::Ref<const Eigen::VectorXd>& center)
-    : ConvexSet(center.size()), A_{A}, center_{center} {
+                               const Eigen::Ref<const Eigen::VectorXd>& center)
+    : ConvexSet(ConvexSetTag<HyperEllipsoid>(), center.size()),
+      A_{A},
+      center_{center} {
   DRAKE_DEMAND(A.rows() == center.size());
 }
 
-HyperEllipsoid::HyperEllipsoid(
-    const QueryObject<double>& query_object, GeometryId geometry_id,
-    std::optional<FrameId> expressed_in) : ConvexSet(3) {
+HyperEllipsoid::HyperEllipsoid(const QueryObject<double>& query_object,
+                               GeometryId geometry_id,
+                               std::optional<FrameId> expressed_in)
+    : ConvexSet(ConvexSetTag<HyperEllipsoid>(), 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

geometry/optimization/convex_set.h

@@ -54,13 +54,22 @@ 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<DerivedSet>()). */
+template <typename S>
+struct ConvexSetTag{};
+
 /** Abstract base class for defining a convex set.
 @ingroup geometry_optimization
 */
 class ConvexSet : public ShapeReifier {
  public:
   virtual ~ConvexSet() {}
 
+  /** Creates a unique deep copy of this set. */
+  std::unique_ptr<ConvexSet> 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<MyConvexSet>()) {}
+     ...
+   };
+   ```
+   */
+  template <typename S>
+  ConvexSet(ConvexSetTag<S>, int ambient_dimension)
+      : ambient_dimension_{ambient_dimension} {
+    static_assert(std::is_base_of_v<ConvexSet, S>,
+                  "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<const S&>(set);
+      return std::unique_ptr<ConvexSet>(new S(derived_set));
+    };
+  }
 
   // Non-virtual interface implementations.
   virtual bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
@@ -115,6 +145,7 @@ class ConvexSet : public ShapeReifier {
   DoToShapeWithPose() const = 0;
 
   int ambient_dimension_{0};
+  std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner_;
 };
 
 // Forward declaration.

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<ConvexSet> clone = H.Clone();
+  EXPECT_EQ(clone->ambient_dimension(), H.ambient_dimension());
+  HPolyhedron* ptr = dynamic_cast<HPolyhedron*>(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<double, 6, 6>::Identity(), Vector6d::Zero());
+  std::unique_ptr<ConvexSet> clone = E.Clone();
+  EXPECT_EQ(clone->ambient_dimension(), E.ambient_dimension());
+  HyperEllipsoid* ptr = dynamic_cast<HyperEllipsoid*>(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