Add functionality to create Convex shapes from vertices

geometry/optimization/test/vpolytope_test.cc

@@ -294,6 +294,21 @@ GTEST_TEST(VPolytopeTest, NonconvexMesh) {
   EXPECT_TRUE(V.PointInSet(V.MaybeGetFeasiblePoint().value()));
 }
 
+GTEST_TEST(VPolytopeTest, ToShapeConvex) {
+  Eigen::Matrix<double, 3, 4> vertices;
+  vertices.col(0) << 0, 0, 0;
+  vertices.col(1) << 1, 0, 0;
+  vertices.col(2) << 0, 1, 0;
+  vertices.col(3) << 0, 0, 1;
+
+  VPolytope V(vertices);
+  const Convex convex = V.ToShapeConvex();
+
+  int num_vertices_of_convex = convex.GetConvexHull().num_vertices();
+
+  EXPECT_EQ(vertices.cols(), num_vertices_of_convex);
+}
+
 GTEST_TEST(VPolytopeTest, UnitBox6DTest) {
   VPolytope V = VPolytope::MakeUnitBox(6);
   EXPECT_EQ(V.ambient_dimension(), 6);

geometry/optimization/vpolytope.cc

@@ -492,6 +492,11 @@ void VPolytope::WriteObj(const std::filesystem::path& filename) const {
   file.close();
 }
 
+Convex VPolytope::ToShapeConvex() const {
+  DRAKE_THROW_UNLESS(ambient_dimension() == 3);
+  return Convex(vertices_);
+}
+
 std::unique_ptr<ConvexSet> VPolytope::DoClone() const {
   return std::make_unique<VPolytope>(*this);
 }

geometry/optimization/vpolytope.h

@@ -98,6 +98,10 @@ class VPolytope final : public ConvexSet {
   @pre ambient_dimension() == 3. */
   void WriteObj(const std::filesystem::path& filename) const;
 
+  /** Creates a geometry::Convex shape using the vertices of this VPolytope.
+  @pre ambient_dimension() == 3. */
+  Convex ToShapeConvex() const;
+
   /** Computes the volume of this V-Polytope.
   @note this function calls qhull to compute the volume. */
   using ConvexSet::CalcVolume;

geometry/shape_specification.cc

@@ -18,6 +18,20 @@
 #include "drake/geometry/proximity/polygon_to_triangle_mesh.h"
 #include "drake/geometry/proximity/triangle_surface_mesh.h"
 
+namespace {
+std::string PointsToObjString(const Eigen::Matrix3X<double>& points) {
+  std::string result = "";
+
+  for (int i = 0; i < points.cols(); i++) {
+    result +=
+        fmt::format("v {} {} {}\n", points(0, i), points(1, i), points(2, i));
+  }
+  result += "\n";
+
+  return result;
+}
+}  // namespace
+
 namespace drake {
 namespace geometry {
 namespace {
@@ -132,6 +146,15 @@ Convex::Convex(MeshSource source, double scale)
   ThrowForBadScale(scale, "Convex");
 }
 
+Convex::Convex(const Eigen::Matrix3X<double>& points,
+               const std::string& convex_label, double scale)
+    : Convex(
+          InMemoryMesh{
+              .mesh_file =
+                  MemoryFile(PointsToObjString(points), ".obj", convex_label),
+          },
+          scale) {}
+
 std::string Convex::filename() const {
   if (source_.is_path()) {
     return source_.path().string();

geometry/shape_specification.h

@@ -286,6 +286,23 @@ class Convex final : public Shape {
    @param scale    An optional scale to coordinates. */
   explicit Convex(MeshSource source, double scale = 1.0);
 
+  /** Constructs an in-memory convex shape specification from the given points.
+
+   The convex hull is computed from the points provided. The points are
+   expected to be in the canonical frame of the shape. Optionally uniformly
+   scaled by the given scale factor.
+
+   @param points      The points whose convex hull define the shape.
+   @param convex_label  A label for the object. The label is used for warning
+                        and error messages. Otherwise, the label has no other
+                        functional purpose. It must consist of a single line.
+   @param scale         An optional scale to coordinates.
+
+   @throws std::exception       if convex_label contains newlines.
+   @throws std::exception       if |scale| < 1e-8. */
+  explicit Convex(const Eigen::Matrix3X<double>& points,
+                  const std::string& convex_label = {}, double scale = 1.0);
+
   ~Convex() final;
 
   /** Returns the source for this specification's mesh data. When working with

geometry/test/shape_specification_test.cc

@@ -662,6 +662,43 @@ GTEST_TEST(ShapeTest, ConvexFromMemory) {
   EXPECT_EQ(hull.num_elements(), 6);
 }
 
+GTEST_TEST(ShapeTest, ConvexFromVertices) {
+  // This will get normalized to ".obj".
+  Eigen::Matrix<double, 3, 4> vertices;
+  vertices.col(0) << 0, 0, 0;
+  vertices.col(1) << 1, 0, 0;
+  vertices.col(2) << 0, 1, 0;
+  vertices.col(3) << 0, 0, 1;
+
+  const std::string mesh_name = "a_convex.obj";
+  const Convex convex_from_verts(vertices, mesh_name, 2.0);
+
+  EXPECT_EQ(convex_from_verts.scale(), 2.0);
+  EXPECT_EQ(convex_from_verts.extension(), ".obj");
+  const MeshSource& source = convex_from_verts.source();
+  ASSERT_TRUE(source.is_in_memory());
+  EXPECT_EQ(source.in_memory().mesh_file.filename_hint(), mesh_name);
+
+  const PolygonSurfaceMesh<double>& hull = convex_from_verts.GetConvexHull();
+  EXPECT_EQ(hull.num_vertices(), 4);
+  EXPECT_EQ(hull.num_elements(), 4);
+
+  // make sure the convex hull is automatically taken.
+  Eigen::Matrix<double, 3, 5> points;
+  points.col(0) << 0, 0, 0;
+  points.col(1) << 1, 0, 0;
+  points.col(2) << 0, 1, 0;
+  points.col(3) << 0, 0, 1;
+  points.col(4) << 0.25, 0.25, 0.25;
+
+  const std::string mesh_name2 = "a_convex2.obj";
+  const Convex convex_from_points(points, mesh_name2, 2.0);
+
+  const PolygonSurfaceMesh<double>& hull2 = convex_from_points.GetConvexHull();
+  EXPECT_EQ(hull2.num_vertices(), 4);
+  EXPECT_EQ(hull2.num_elements(), 4);
+}
+
 GTEST_TEST(ShapeTest, MeshFromMemory) {
   // This will get normalized to ".obj".
   const std::string mesh_name = "a_mesh.OBJ";
@@ -760,7 +797,6 @@ TEST_F(OverrideDefaultGeometryTest, UnsupportedGeometry) {
   EXPECT_NO_THROW(this->ImplementGeometry(Sphere(0.5), nullptr));
 }
 
-
 GTEST_TEST(ShapeTest, TypeNameAndToString) {
   // In-memory mesh we'll use on Convex and Mesh.
   const InMemoryMesh in_memory{
@@ -828,7 +864,6 @@ GTEST_TEST(ShapeTest, TypeNameAndToString) {
   EXPECT_EQ(fmt::to_string(base), "Box(width=1.5, depth=2.5, height=3.5)");
 }
 
-
 GTEST_TEST(ShapeTest, Volume) {
   EXPECT_NEAR(CalcVolume(Box(1, 2, 3)), 6.0, 1e-14);
   EXPECT_NEAR(CalcVolume(Capsule(1.23, 2.4)),