[geometry] Add MeshcatCone isa Shape (#15936)

* [geometry] Add MeshcatCone isa Shape

This supports visualizing cone geometries in meshcat (which is
immediately useful for drawing the arrowheads of contact force
vectors).

Per a discussion with @SeanCurtis-TRI, we've decided to add the type
to the main Shape class hierarchy, but with a class name
(`MeshcatCone`) that makes it obvious that the current support for
this shape is limited to Meshcat.

bindings/pydrake/geometry_py_common.cc

@@ -471,6 +471,21 @@ void DoScalarIndependentDefinitions(py::module m) {
         .def("radius", &Sphere::radius, doc.Sphere.radius.doc)
         .def(py::pickle([](const Sphere& self) { return self.radius(); },
             [](const double radius) { return Sphere(radius); }));
+
+    py::class_<MeshcatCone, Shape>(m, "MeshcatCone", doc.MeshcatCone.doc)
+        .def(py::init<double, double, double>(), py::arg("height"),
+            py::arg("a") = 1.0, py::arg("b") = 1.0, doc.MeshcatCone.ctor.doc)
+        .def("height", &MeshcatCone::height, doc.MeshcatCone.height.doc)
+        .def("a", &MeshcatCone::a, doc.MeshcatCone.a.doc)
+        .def("b", &MeshcatCone::b, doc.MeshcatCone.b.doc)
+        .def(py::pickle(
+            [](const MeshcatCone& self) {
+              return std::make_tuple(self.height(), self.a(), self.b());
+            },
+            [](std::tuple<double, double, double> params) {
+              return MeshcatCone(std::get<0>(params), std::get<1>(params),
+                  std::get<2>(params));
+            }));
   }
 
   m.def("MakePhongIllustrationProperties", &MakePhongIllustrationProperties,

bindings/pydrake/test/geometry_common_test.py

@@ -331,7 +331,8 @@ def test_shape_constructors(self):
             mut.Ellipsoid(a=1.0, b=2.0, c=3.0),
             mut.HalfSpace(),
             mut.Mesh(absolute_filename="arbitrary/path", scale=1.0),
-            mut.Convex(absolute_filename="arbitrary/path", scale=1.0)
+            mut.Convex(absolute_filename="arbitrary/path", scale=1.0),
+            mut.MeshcatCone(height=1.23, a=3.45, b=6.78)
         ]
         for shape in shapes:
             self.assertIsInstance(shape, mut.Shape)
@@ -406,3 +407,11 @@ def assert_shape_api(shape):
         assert_shape_api(sphere)
         self.assertEqual(sphere.radius(), 1.0)
         assert_pickle(self, sphere, mut.Sphere.radius)
+
+        cone = mut.MeshcatCone(height=1.2, a=3.4, b=5.6)
+        assert_shape_api(cone)
+        self.assertEqual(cone.height(), 1.2)
+        self.assertEqual(cone.a(), 3.4)
+        self.assertEqual(cone.b(), 5.6)
+        assert_pickle(self, cone, lambda shape: [
+                      shape.height(), shape.a(), shape.b()])

geometry/meshcat.cc

@@ -232,6 +232,7 @@ class MeshcatShapeReifier : public ShapeReifier {
     geometry->height = cylinder.length();
     lumped.geometry = std::move(geometry);
 
+    // Meshcat cylinders have their long axis in y.
     Eigen::Map<Eigen::Matrix4d>(mesh.matrix) =
         RigidTransformd(RotationMatrixd::MakeXRotation(M_PI / 2.0))
             .GetAsMatrix4();
@@ -407,6 +408,28 @@ class MeshcatShapeReifier : public ShapeReifier {
     ImplementMesh(mesh, data);
   }
 
+  void ImplementGeometry(const MeshcatCone& cone, void* data) override {
+    DRAKE_DEMAND(data != nullptr);
+    auto& lumped = *static_cast<internal::LumpedObjectData*>(data);
+    auto& mesh = lumped.object.emplace<internal::MeshData>();
+
+    auto geometry = std::make_unique<internal::CylinderGeometryData>();
+    geometry->uuid = uuids::to_string((*uuid_generator_)());
+    geometry->radiusBottom = 0;
+    geometry->radiusTop = 1.0;
+    geometry->height = cone.height();
+    lumped.geometry = std::move(geometry);
+
+    // Meshcat cylinders have their long axis in y and are centered at the
+    // origin.  A cone is just a cylinder with radiusBottom=0.  So we transform
+    // here, in addition to scaling to support non-uniform principle axes.
+    Eigen::Map<Eigen::Matrix4d>(mesh.matrix) =
+        Eigen::Vector4d{cone.a(), cone.b(), 1.0, 1.0}.asDiagonal() *
+        RigidTransformd(RotationMatrixd::MakeXRotation(M_PI / 2.0),
+                        Eigen::Vector3d{0, 0, cone.height() / 2.0})
+            .GetAsMatrix4();
+  }
+
  private:
   uuids::uuid_random_generator* const uuid_generator_{};
 };

geometry/proximity/hydroelastic_internal.h

@@ -324,6 +324,8 @@ class Geometries final : public ShapeReifier {
     const ProximityProperties& properties;
   };
 
+  using ShapeReifier::ImplementGeometry;
+
   void ImplementGeometry(const Sphere& sphere, void* user_data) override;
   void ImplementGeometry(const Cylinder& cylinder, void* user_data) override;
   void ImplementGeometry(const HalfSpace&, void* user_data) override;

geometry/shape_specification.cc

@@ -118,6 +118,16 @@ Convex::Convex(const std::string& absolute_filename, double scale)
   }
 }
 
+MeshcatCone::MeshcatCone(double height, double a, double b)
+    : Shape(ShapeTag<MeshcatCone>()), height_(height), a_(a), b_(b) {
+  if (height <= 0 || a <= 0 || b <= 0) {
+    throw std::logic_error(fmt::format(
+        "MeshcatCone parameters height, a, and b should all be > 0 (they were "
+        "{}, {}, and {}, respectively).",
+        height, a, b));
+  }
+}
+
 void ShapeReifier::ImplementGeometry(const Sphere&, void*) {
   ThrowUnsupportedGeometry("Sphere");
 }
@@ -149,6 +159,10 @@ void ShapeReifier::ImplementGeometry(const Convex&, void*) {
   ThrowUnsupportedGeometry("Convex");
 }
 
+void ShapeReifier::ImplementGeometry(const MeshcatCone&, void*) {
+  ThrowUnsupportedGeometry("MeshcatCone");
+}
+
 void ShapeReifier::ThrowUnsupportedGeometry(const std::string& shape_name) {
   throw std::runtime_error(fmt::format("This class ({}) does not support {}.",
                                        NiceTypeName::Get(*this), shape_name));

geometry/shape_specification.h

@@ -318,6 +318,39 @@ class Convex final : public Shape {
   double scale_;
 };
 
+// TODO(russt): Rename this to `Cone` if/when it is supported by more of the
+// geometry engine.
+/** Definition of a cone. Its point is at the origin, its height extends in the
+ direction of the frame's +z axis. Or, more formally: a finite section of a
+ Lorentz cone (aka "second-order cone"), defined by
+
+      sqrt(x²/a² + y²/b²) ≤ z;  z ∈ [0, height],
+
+ where `a` and `b` are the lengths of the principle semi-axes of the horizontal
+ section at `z=1`.
+
+ This shape is currently only supported by Meshcat. It will not appear in any
+ renderings, proximity queries, or other visualizers.
+*/
+class MeshcatCone final : public Shape {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MeshcatCone)
+
+  /** Constructs the parameterized cone.
+   @throws std::exception if `height`, `a`, or `b` are not strictly positive.
+   */
+  explicit MeshcatCone(double height, double a = 1.0, double b = 1.0);
+
+  double height() const { return height_; }
+  double a() const { return a_; }
+  double b() const { return b_; }
+
+ private:
+  double height_;
+  double a_;
+  double b_;
+};
+
 /** The interface for converting shape descriptions to real shapes. Any entity
  that consumes shape descriptions _must_ implement this interface.
 
@@ -379,6 +412,7 @@ class ShapeReifier {
   virtual void ImplementGeometry(const Ellipsoid& ellipsoid, void* user_data);
   virtual void ImplementGeometry(const Mesh& mesh, void* user_data);
   virtual void ImplementGeometry(const Convex& convex, void* user_data);
+  virtual void ImplementGeometry(const MeshcatCone& cone, void* user_data);
 
  protected:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ShapeReifier)
@@ -450,6 +484,9 @@ class ShapeName final : public ShapeReifier {
   void ImplementGeometry(const Convex&, void*) final {
     string_ = "Convex";
   }
+  void ImplementGeometry(const MeshcatCone&, void*) final {
+    string_ = "MeshcatCone";
+  }
 
   //@}
 

geometry/test/shape_specification_test.cc

@@ -52,6 +52,10 @@ class ReifierTest : public ShapeReifier, public ::testing::Test {
     received_user_data_ = data;
     convex_made_ = true;
   }
+  void ImplementGeometry(const MeshcatCone&, void* data) override {
+    received_user_data_ = data;
+    meshcat_cone_made_ = true;
+  }
   void Reset() {
     box_made_ = false;
     capsule_made_ = false;
@@ -61,6 +65,7 @@ class ReifierTest : public ShapeReifier, public ::testing::Test {
     cylinder_made_ = false;
     convex_made_ = false;
     mesh_made_ = false;
+    meshcat_cone_made_ = false;
     received_user_data_ = nullptr;
   }
 
@@ -73,6 +78,7 @@ class ReifierTest : public ShapeReifier, public ::testing::Test {
   bool half_space_made_{false};
   bool convex_made_{false};
   bool mesh_made_{false};
+  bool meshcat_cone_made_{false};
   void* received_user_data_{nullptr};
 };
 
@@ -97,6 +103,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_FALSE(convex_made_);
   EXPECT_FALSE(ellipsoid_made_);
   EXPECT_FALSE(mesh_made_);
+  EXPECT_FALSE(meshcat_cone_made_);
   EXPECT_EQ(s.radius(), 1.0);
 
   Reset();
@@ -111,6 +118,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_FALSE(convex_made_);
   EXPECT_FALSE(ellipsoid_made_);
   EXPECT_FALSE(mesh_made_);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -126,6 +134,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_FALSE(mesh_made_);
   EXPECT_EQ(cylinder.radius(), 1);
   EXPECT_EQ(cylinder.length(), 2);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -142,6 +151,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_EQ(box.width(), 1);
   EXPECT_EQ(box.depth(), 2);
   EXPECT_EQ(box.height(), 3);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -157,6 +167,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_FALSE(mesh_made_);
   EXPECT_EQ(capsule.radius(), 2);
   EXPECT_EQ(capsule.length(), 1);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -170,6 +181,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_TRUE(convex_made_);
   EXPECT_FALSE(ellipsoid_made_);
   EXPECT_FALSE(mesh_made_);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -186,6 +198,7 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_EQ(ellipsoid.a(), 1);
   EXPECT_EQ(ellipsoid.b(), 2);
   EXPECT_EQ(ellipsoid.c(), 3);
+  EXPECT_FALSE(meshcat_cone_made_);
 
   Reset();
 
@@ -201,6 +214,24 @@ TEST_F(ReifierTest, ReificationDifferentiation) {
   EXPECT_TRUE(mesh_made_);
   EXPECT_EQ(mesh.filename(), std::string("fictitious_mesh_name.obj"));
   EXPECT_EQ(mesh.scale(), 1.4);
+  EXPECT_FALSE(meshcat_cone_made_);
+
+  Reset();
+
+  MeshcatCone cone{1.2, 3.4, 5.6};
+  cone.Reify(this);
+  EXPECT_FALSE(sphere_made_);
+  EXPECT_FALSE(half_space_made_);
+  EXPECT_FALSE(cylinder_made_);
+  EXPECT_FALSE(box_made_);
+  EXPECT_FALSE(capsule_made_);
+  EXPECT_FALSE(convex_made_);
+  EXPECT_FALSE(ellipsoid_made_);
+  EXPECT_FALSE(mesh_made_);
+  EXPECT_TRUE(meshcat_cone_made_);
+  EXPECT_EQ(cone.height(), 1.2);
+  EXPECT_EQ(cone.a(), 3.4);
+  EXPECT_EQ(cone.b(), 5.6);
 }
 
 // Confirms that the ReifiableShape properly clones the right types.

multibody/fixed_fem/dev/collision_objects.h

@@ -142,6 +142,8 @@ class CollisionObjects : public geometry::ShapeReifier {
     math::RigidTransform<T> pose_in_world;
   };
 
+  using ShapeReifier::ImplementGeometry;
+
   void ImplementGeometry(const geometry::Sphere& sphere,
                          void* user_data) override;
   void ImplementGeometry(const geometry::Cylinder& cylinder,