Hydroelastic contact surface visualization (RobotLocomotion#11901)

attic/multibody/rigid_body_plant/BUILD.bazel

@@ -103,8 +103,8 @@ drake_cc_library(
     ],
     deps = [
         ":rigid_body_plant",
-        "//lcmtypes:contact_info_for_viz",
         "//lcmtypes:contact_results_for_viz",
+        "//lcmtypes:point_pair_contact_info_for_viz",
     ],
 )
 

attic/multibody/rigid_body_plant/contact_results_to_lcm.cc

@@ -26,11 +26,13 @@ void ContactResultsToLcmSystem<T>::CalcLcmContactOutput(
   auto& msg = *output;
 
   msg.timestamp = static_cast<int64_t>(context.get_time() * 1e6);
-  msg.num_contacts = contact_results.get_num_contacts();
-  msg.contact_info.resize(msg.num_contacts);
+  msg.num_point_pair_contacts =
+      contact_results.get_num_contacts();
+  msg.point_pair_contact_info.resize(msg.num_point_pair_contacts);
 
   for (int i = 0; i < contact_results.get_num_contacts(); ++i) {
-    lcmt_contact_info_for_viz& info_msg = msg.contact_info[i];
+    lcmt_point_pair_contact_info_for_viz& info_msg =
+        msg.point_pair_contact_info[i];
     info_msg.timestamp = static_cast<int64_t>(context.get_time() * 1e6);
 
     const ContactInfo<T>& contact_info = contact_results.get_contact_info(i);

bindings/pydrake/multibody/plant_py.cc

@@ -108,11 +108,30 @@ void DoScalarDependentDefinitions(py::module m, T) {
         m, "ContactResults", param, cls_doc.doc);
     cls  // BR
         .def(py::init<>(), cls_doc.ctor.doc)
-        .def("num_contacts", &Class::num_contacts, cls_doc.num_contacts.doc)
-        .def("AddContactInfo", &Class::AddContactInfo,
-            py::arg("point_pair_info"), cls_doc.AddContactInfo.doc)
-        .def("contact_info", &Class::point_pair_contact_info, py::arg("i"),
-            "Deprecated. Use point_pair_contact_info() instead.")
+        .def("num_contacts",
+            [](Class* self) {
+              WarnDeprecated(
+                  "Deprecated and will be removed on or around "
+                  "12-01-2019. Use num_point_pair_contacts() instead.");
+              return self->num_point_pair_contacts();
+            },
+            cls_doc.num_contacts.doc_deprecated)
+        .def("num_point_pair_contacts", &Class::num_point_pair_contacts,
+            cls_doc.num_point_pair_contacts.doc)
+        .def("AddContactInfo",
+            [](Class* self, const PointPairContactInfo<T>& contact_info) {
+              self->AddContactInfo(contact_info);
+            },
+            py::arg("point_pair_info"),
+            cls_doc.AddContactInfo.doc_1args_point_pair_info)
+        .def("contact_info",
+            [](Class* self, int i) {
+              WarnDeprecated(
+                  "Deprecated and will be removed on or around "
+                  "12-01-2019. Use point_pair_contact_info() instead.");
+              return self->point_pair_contact_info(i);
+            },
+            py::arg("i"), cls_doc.contact_info.doc_deprecated)
         .def("point_pair_contact_info", &Class::point_pair_contact_info,
             py::arg("i"), cls_doc.point_pair_contact_info.doc);
     AddValueInstantiation<Class>(m);

bindings/pydrake/multibody/test/plant_test.py

@@ -1091,7 +1091,7 @@ def test_contact(self, T):
         # ContactResults
         contact_results = ContactResults()
         contact_results.AddContactInfo(contact_info)
-        self.assertTrue(contact_results.num_contacts() == 1)
+        self.assertTrue(contact_results.num_point_pair_contacts() == 1)
         self.assertTrue(
             isinstance(contact_results.point_pair_contact_info(0),
                        PointPairContactInfo))

bindings/pydrake/systems/meshcat_visualizer.py

@@ -421,7 +421,7 @@ def _draw_contact_forces(self, context, X_WB_map):
         # If False, add the new contact to self._contacts
         vis = self._meshcat_viz.vis
         prefix = self._meshcat_viz.prefix
-        for i_contact in range(contact_results.num_contacts()):
+        for i_contact in range(contact_results.num_point_pair_contacts()):
             contact_info_i = contact_results.point_pair_contact_info(i_contact)
 
             # Do not display small forces.

bindings/pydrake/systems/test/meshcat_visualizer_test.py

@@ -183,7 +183,7 @@ def test_contact_force(self):
             contact_viz_context,
             contact_input_port.get_index()).get_value()
 
-        self.assertGreater(contact_results.num_contacts(), 0)
+        self.assertGreater(contact_results.num_point_pair_contacts(), 0)
         self.assertEqual(contact_viz._contact_key_counter, 4)
 
     def test_texture_override(self):

examples/valkyrie/BUILD.bazel

@@ -106,8 +106,8 @@ drake_cc_library(
         "//attic/multibody/rigid_body_plant:drake_visualizer",
         "//common:find_resource",
         "//lcm",
-        "//lcmtypes:contact_info_for_viz",
         "//lcmtypes:contact_results_for_viz",
+        "//lcmtypes:point_pair_contact_info_for_viz",
         "//systems/analysis",
         "//systems/framework",
         "//systems/lcm:lcm_pubsub_system",

geometry/proximity/mesh_field.h

@@ -55,7 +55,7 @@ class MeshField {
    mesh.
    */
   DRAKE_NODISCARD std::unique_ptr<MeshField> CloneAndSetMesh(
-      MeshType* new_mesh) const {
+      const MeshType* new_mesh) const {
     DRAKE_DEMAND(new_mesh != nullptr);
     DRAKE_DEMAND(new_mesh->num_vertices() == mesh_->num_vertices());
     // TODO(DamrongGuoy): Check that the `new_mesh` is equivalent to the

geometry/proximity/test/contact_surface_test.cc

@@ -135,9 +135,9 @@ ContactSurface<T> TestContactSurface() {
   EXPECT_EQ(id_N, contact_surface.id_N());
   // Check memory address of the mesh. We don't want to compare the mesh
   // objects themselves.
-  EXPECT_EQ(&surface_mesh_ref, &contact_surface.mesh());
-  EXPECT_EQ(2, contact_surface.mesh().num_faces());
-  EXPECT_EQ(4, contact_surface.mesh().num_vertices());
+  EXPECT_EQ(&surface_mesh_ref, &contact_surface.mesh_W());
+  EXPECT_EQ(2, contact_surface.mesh_W().num_faces());
+  EXPECT_EQ(4, contact_surface.mesh_W().num_vertices());
   // Tests evaluation of `e` on face f0 {0, 1, 2}.
   {
     const SurfaceFaceIndex f0(0);
@@ -162,8 +162,8 @@ ContactSurface<T> TestContactSurface() {
   }
   // Tests area() of triangular faces.
   {
-    EXPECT_EQ(T(0.5), contact_surface.mesh().area(SurfaceFaceIndex(0)));
-    EXPECT_EQ(T(0.5), contact_surface.mesh().area(SurfaceFaceIndex(1)));
+    EXPECT_EQ(T(0.5), contact_surface.mesh_W().area(SurfaceFaceIndex(0)));
+    EXPECT_EQ(T(0.5), contact_surface.mesh_W().area(SurfaceFaceIndex(1)));
   }
 
   return contact_surface;
@@ -178,13 +178,13 @@ GTEST_TEST(ContactSurfaceTest, TestCopy) {
 
   // Confirm that it was a deep copy, i.e., the `original` mesh and the `copy`
   // mesh are different objects.
-  EXPECT_NE(&original.mesh(), &copy.mesh());
+  EXPECT_NE(&original.mesh_W(), &copy.mesh_W());
 
   EXPECT_EQ(original.id_M(), copy.id_M());
   EXPECT_EQ(original.id_N(), copy.id_N());
   // We use `num_faces()` as a representative of the mesh. We do not check
   // everything in the mesh.
-  EXPECT_EQ(original.mesh().num_faces(), copy.mesh().num_faces());
+  EXPECT_EQ(original.mesh_W().num_faces(), copy.mesh_W().num_faces());
 
   // We check evaluation of field values only at one position.
   const SurfaceFaceIndex f(0);
@@ -198,7 +198,7 @@ GTEST_TEST(ContactSurfaceTest, TestCopy) {
 GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
   // Create the original contact surface for comparison later.
   const ContactSurface<double> original = TestContactSurface<double>();
-  auto mesh = std::make_unique<SurfaceMesh<double>>(original.mesh());
+  auto mesh = std::make_unique<SurfaceMesh<double>>(original.mesh_W());
   SurfaceMesh<double>* mesh_pointer = mesh.get();
   // TODO(DamrongGuoy): Remove `original_tester` when ContactSurface allows
   //  direct access to e_MN and grad_h_MN_W.
@@ -233,17 +233,17 @@ GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
            f1.vertex(2) == f2.vertex(2);
   };
   // Face winding is changed.
-  for (SurfaceFaceIndex f(0); f < original.mesh().num_faces(); ++f) {
+  for (SurfaceFaceIndex f(0); f < original.mesh_W().num_faces(); ++f) {
     EXPECT_FALSE(
-        are_identical(dut.mesh().element(f), original.mesh().element(f)));
+        are_identical(dut.mesh_W().element(f), original.mesh_W().element(f)));
   }
 
   // Test the mesh fields by evaluating each field, once per face for an
   // arbitrary point Q on the interior of the triangle. We expect:
   //    e_MN function hasn't changed.
   //    grad_H function has been mirrored.
   const SurfaceMesh<double>::Barycentric b_Q{0.25, 0.25, 0.5};
-  for (SurfaceFaceIndex f(0); f < original.mesh().num_faces(); ++f) {
+  for (SurfaceFaceIndex f(0); f < original.mesh_W().num_faces(); ++f) {
     EXPECT_EQ(dut.EvaluateE_MN(f, b_Q), original.EvaluateE_MN(f, b_Q));
     const Vector3d expected_normal = -original.EvaluateGrad_h_MN_W(f, b_Q);
     EXPECT_TRUE(CompareMatrices(dut.EvaluateGrad_h_MN_W(f, b_Q),

geometry/proximity/test/mesh_intersection_test.cc

@@ -855,14 +855,14 @@ void TestComputeContactSurfaceSoftRigid() {
 
   // Mesh invariants:
   //   Meshes are the same "size" (topologically).
-  EXPECT_EQ(contact_SR->mesh().num_faces(), contact_RS->mesh().num_faces());
-  EXPECT_EQ(contact_SR->mesh().num_vertices(),
-            contact_RS->mesh().num_vertices());
+  EXPECT_EQ(contact_SR->mesh_W().num_faces(), contact_RS->mesh_W().num_faces());
+  EXPECT_EQ(contact_SR->mesh_W().num_vertices(),
+            contact_RS->mesh_W().num_vertices());
 
   //   Test one and assume all share the same property.
   const SurfaceVertexIndex v_index(0);
-  EXPECT_TRUE(CompareMatrices(contact_SR->mesh().vertex(v_index).r_MV(),
-                              contact_RS->mesh().vertex(v_index).r_MV()));
+  EXPECT_TRUE(CompareMatrices(contact_SR->mesh_W().vertex(v_index).r_MV(),
+                              contact_RS->mesh_W().vertex(v_index).r_MV()));
 
   // TODO(SeanCurtis-TRI): Test that the face winding has been reversed, once
   //  that is officially documented as a property of the ContactSurface.
@@ -955,12 +955,12 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) {
             id_S, *soft_epsilon, X_WS, id_R, *rigid_mesh, X_WR);
     // TODO(DamrongGuoy): More comprehensive checks on the mesh of the contact
     //  surface. Here we only check the number of triangles.
-    EXPECT_EQ(4, contact_SR_W->mesh().num_faces());
+    EXPECT_EQ(4, contact_SR_W->mesh_W().num_faces());
 
     const Vector3d p_MQ = Vector3d::Zero();
     SurfaceFaceIndex face_Q;
     SurfaceMesh<double>::Barycentric b_Q;
-    bool found = FindFaceVertex(p_MQ, contact_SR_W->mesh(), &face_Q, &b_Q);
+    bool found = FindFaceVertex(p_MQ, contact_SR_W->mesh_W(), &face_Q, &b_Q);
     ASSERT_TRUE(found);
     const auto epsilon_SR = contact_SR_W->EvaluateE_MN(face_Q, b_Q);
     EXPECT_NEAR(1.0, epsilon_SR, kEps);
@@ -998,13 +998,13 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) {
             id_S, *soft_epsilon, X_WS, id_R, *rigid_mesh, X_WR);
     // TODO(DamrongGuoy): More comprehensive checks on the mesh of the contact
     //  surface.  Here we only check the number of triangles.
-    EXPECT_EQ(4, contact_SR_W->mesh().num_faces());
+    EXPECT_EQ(4, contact_SR_W->mesh_W().num_faces());
 
     const Vector3d p_MQ{0, -0.5,
                         0};  // The center vertex of the pyramid "bottom".
     SurfaceFaceIndex face_Q;
     SurfaceMesh<double>::Barycentric b_Q;
-    bool found = FindFaceVertex(p_MQ, contact_SR_W->mesh(), &face_Q, &b_Q);
+    bool found = FindFaceVertex(p_MQ, contact_SR_W->mesh_W(), &face_Q, &b_Q);
     ASSERT_TRUE(found);
     const auto e_SR = contact_SR_W->EvaluateE_MN(face_Q, b_Q);
     EXPECT_NEAR(0.5, e_SR, kEps);

geometry/query_results/contact_surface.h

@@ -217,9 +217,13 @@ class ContactSurface {
     return grad_h_MN_W_->EvaluateAtVertex(vertex);
   }
 
-  /** Returns a reference to the surface mesh.
+  DRAKE_DEPRECATED("2019-12-01", "Use mesh_W() instead.")
+  const SurfaceMesh<T>& mesh() const { return mesh_W(); }
+
+  /** Returns a reference to the surface mesh whose vertex
+   positions are measured and expressed in the world frame.
    */
-  const SurfaceMesh<T>& mesh() const {
+  const SurfaceMesh<T>& mesh_W() const {
     DRAKE_DEMAND(mesh_W_ != nullptr);
     return *mesh_W_;
   }

lcmtypes/BUILD.bazel

@@ -67,17 +67,14 @@ drake_lcm_cc_library(
     lcm_srcs = ["lcmt_constrained_values.lcm"],
 )
 
-drake_lcm_cc_library(
-    name = "contact_info_for_viz",
-    lcm_package = "drake",
-    lcm_srcs = ["lcmt_contact_info_for_viz.lcm"],
-)
-
 drake_lcm_cc_library(
     name = "contact_results_for_viz",
     lcm_package = "drake",
     lcm_srcs = ["lcmt_contact_results_for_viz.lcm"],
-    deps = [":contact_info_for_viz"],
+    deps = [
+        ":hydroelastic_contact_surface_for_viz",
+        ":point_pair_contact_info_for_viz",
+    ],
 )
 
 drake_lcm_cc_library(
@@ -107,6 +104,19 @@ drake_lcm_cc_library(
     deps = [":constrained_values"],
 )
 
+drake_lcm_cc_library(
+    name = "hydroelastic_contact_surface_for_viz",
+    lcm_package = "drake",
+    lcm_srcs = ["lcmt_hydroelastic_contact_surface_for_viz.lcm"],
+    deps = [":hydroelastic_contact_surface_tri_for_viz"],
+)
+
+drake_lcm_cc_library(
+    name = "hydroelastic_contact_surface_tri_for_viz",
+    lcm_package = "drake",
+    lcm_srcs = ["lcmt_hydroelastic_contact_surface_tri_for_viz.lcm"],
+)
+
 drake_lcm_cc_library(
     name = "inverse_dynamics_debug_info",
     lcm_package = "drake",
@@ -133,6 +143,12 @@ drake_lcm_cc_library(
     deps = [":polynomial_matrix"],
 )
 
+drake_lcm_cc_library(
+    name = "point_pair_contact_info_for_viz",
+    lcm_package = "drake",
+    lcm_srcs = ["lcmt_point_pair_contact_info_for_viz.lcm"],
+)
+
 drake_lcm_cc_library(
     name = "polynomial",
     lcm_package = "drake",
@@ -329,20 +345,22 @@ LCMTYPES_CC = [
     ":body_motion_data",
     ":body_wrench_data",
     ":constrained_values",
-    ":contact_info_for_viz",
     ":contact_information",
     ":contact_results_for_viz",
     ":desired_body_motion",
     ":desired_centroidal_momentum_dot",
     ":desired_dof_motions",
     ":drake_signal",
+    ":hydroelastic_contact_surface_for_viz",
+    ":hydroelastic_contact_surface_tri_for_viz",
     ":iiwa",
     ":inverse_dynamics_debug_info",
     ":jaco",
     ":joint_pd_override",
     ":manipulator_plan_move_end_effector",
     ":piecewise_polynomial",
     ":plan_eval_debug_info",
+    ":point_pair_contact_info_for_viz",
     ":polynomial",
     ":polynomial_matrix",
     ":qp_controller_input",

lcmtypes/lcmt_contact_results_for_viz.lcm

@@ -4,6 +4,11 @@ struct lcmt_contact_results_for_viz {
   // in microseconds
   int64_t timestamp;
 
-  int32_t num_contacts;
-  lcmt_contact_info_for_viz contact_info[num_contacts];
+  int32_t num_point_pair_contacts;
+  lcmt_point_pair_contact_info_for_viz point_pair_contact_info[
+      num_point_pair_contacts];
+
+  int32_t num_hydroelastic_contacts;
+  lcmt_hydroelastic_contact_surface_for_viz hydroelastic_contacts[
+      num_hydroelastic_contacts];
 }

lcmtypes/lcmt_hydroelastic_contact_surface_for_viz.lcm

@@ -0,0 +1,11 @@
+package drake;
+
+struct lcmt_hydroelastic_contact_surface_for_viz {
+  // Names of the colliding bodies
+  string body1_name;
+  string body2_name;
+
+  // Get the number of triangles.
+  int32_t num_triangles;
+  lcmt_hydroelastic_contact_surface_tri_for_viz triangles[num_triangles];
+}

lcmtypes/lcmt_hydroelastic_contact_surface_tri_for_viz.lcm

@@ -0,0 +1,8 @@
+package drake;
+
+struct lcmt_hydroelastic_contact_surface_tri_for_viz {
+  // The three vertices of each triangle, all expressed in the world frame.
+  double p_WA[3];
+  double p_WB[3];
+  double p_WC[3];
+}

lcmtypes/lcmt_contact_info_for_viz.lcm → lcmtypes/lcmt_point_pair_contact_info_for_viz.lcm

@@ -1,6 +1,8 @@
 package drake;
 
-struct lcmt_contact_info_for_viz {
+struct lcmt_point_pair_contact_info_for_viz {
+  // TODO(edrumwri) Consider removing this data which is already present in the
+  // structure in which this message is stored (lcmt_contact_results_for_viz).
   // In microseconds
   int64_t timestamp;
 

multibody/hydroelastics/test/hydroelastic_engine_test.cc

@@ -154,7 +154,7 @@ TEST_F(SphereVsPlaneTest, VerifyModelSizeAndResults) {
   // has thorough unit tests.
   // Since id_M corresponds to the ground geometry, the mesh is expressed in
   // frame G of the ground.
-  const SurfaceMesh<double>& mesh_G = surface.mesh();
+  const SurfaceMesh<double>& mesh_G = surface.mesh_W();
   EXPECT_GT(mesh_G.num_vertices(), 0);
   const double kTolerance = 5.0 * std::numeric_limits<double>::epsilon();