Adding drake contact material properties to sdf/urdf parsing (RobotLo…

…comotion#12464)

This introduces a new XML sub-tree that is largely common between both SDF
and URDF.

SDF version:

  <drake:proximity_properties>
    <drake:mesh_resolution_hint>1.3</drake:hydroelastic_resolution_hint>
    <drake:elastic_modulus>1e8</drake:elastic_modulus>
    <drake:hunt_crossley_dissipation>0.25</drake:hunt_crossley_dissipation>
    <drake:mu_dynamic>0.7</drake:mu_dynamic>
    <drake:mu_static>0.7</drake:mu_static>
  </drake:proximity_properties>

URDF version:

  <drake:proximity_properties>
    <drake:mesh_resolution_hint value="1.3" />
    <drake:elastic_modulus value="1e8" />
    <drake:hunt_crossley_dissipation value="0.25" />
    <drake:mu_dynamic value="0.7" />
    <drake:mu_static value="0.7" />
  </drake:proximity_properties>

(URDF differs because URDF has a style that uses attributes whereas SDF
eschews them.)

 - Parsing collision geometry now creates a populated ProximityProperties
   and stores the parsed properties there.
 - There is some code that provides legacy compatibility with old
   mechanisms for specifying coefficients of friction so that existing
   files will still work (albeit with warnings).
 - MBP API has been extended to accept proximity properties compatible with
   this parsing logic. The old API needs to be deprecated (in a follow up
   commit).

We still need to add "soft" deprecation for when we use the legacy
functionality. It can't be compile-time, so it'll have to be warnings with
a time stamp.

bindings/pydrake/multibody/plant_py.cc

@@ -129,6 +129,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("dynamic_friction", &Class::dynamic_friction,
             cls_doc.dynamic_friction.doc);
 
+    AddValueInstantiation<CoulombFriction<T>>(m);
+
     m.def("CalcContactFrictionFromSurfaceProperties",
         [](const multibody::CoulombFriction<T>& surface_properties1,
             const multibody::CoulombFriction<T>& surface_properties2) {
@@ -597,13 +599,24 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("body"), py::arg("X_BG"), py::arg("shape"), py::arg("name"),
             py::arg("diffuse_color"), py::arg("scene_graph") = nullptr,
             doc_iso3_deprecation)
+        .def("RegisterCollisionGeometry",
+            py::overload_cast<const Body<T>&, const RigidTransform<double>&,
+                const geometry::Shape&, const std::string&,
+                geometry::ProximityProperties>(
+                &Class::RegisterCollisionGeometry),
+            py::arg("body"), py::arg("X_BG"), py::arg("shape"), py::arg("name"),
+            py::arg("properties"),
+            cls_doc.RegisterCollisionGeometry
+                .doc_5args_body_X_BG_shape_name_properties)
         .def("RegisterCollisionGeometry",
             py::overload_cast<const Body<T>&, const RigidTransform<double>&,
                 const geometry::Shape&, const std::string&,
                 const CoulombFriction<double>&>(
                 &Class::RegisterCollisionGeometry),
             py::arg("body"), py::arg("X_BG"), py::arg("shape"), py::arg("name"),
-            py::arg("coulomb_friction"), cls_doc.RegisterCollisionGeometry.doc)
+            py::arg("coulomb_friction"),
+            cls_doc.RegisterCollisionGeometry
+                .doc_5args_body_X_BG_shape_name_coulomb_friction)
         .def("get_source_id", &Class::get_source_id, cls_doc.get_source_id.doc)
         .def("get_geometry_query_input_port",
             &Class::get_geometry_query_input_port, py_reference_internal,

bindings/pydrake/multibody/test/plant_test.py

@@ -63,6 +63,7 @@
     GeometryId,
     Role,
     PenetrationAsPointPair_,
+    ProximityProperties,
     SceneGraph_,
     SignedDistancePair_,
     SignedDistanceToPoint_,
@@ -182,6 +183,19 @@ def test_multibody_plant_construction_api(self, T):
             self.assertEqual(plant.default_coulomb_friction(
                 plant.GetCollisionGeometriesForBody(body)[0]
                 ).dynamic_friction(), 0.5)
+            explicit_props = ProximityProperties()
+            explicit_props.AddProperty("material", "coulomb_friction",
+                                       CoulombFriction(1.1, 0.8))
+            plant.RegisterCollisionGeometry(
+                body=body, X_BG=body_X_BG, shape=box,
+                name="new_body_collision2", properties=explicit_props)
+            self.assertGreater(plant.num_collision_geometries(), 1)
+            self.assertEqual(plant.default_coulomb_friction(
+                plant.GetCollisionGeometriesForBody(body)[1]
+            ).static_friction(), 1.1)
+            self.assertEqual(plant.default_coulomb_friction(
+                plant.GetCollisionGeometriesForBody(body)[1]
+            ).dynamic_friction(), 0.8)
 
     @numpy_compare.check_all_types
     def test_multibody_plant_api_via_parsing(self, T):

examples/scene_graph/simple_contact_surface_vis.cc

@@ -99,7 +99,8 @@ class MovingBall final : public LeafSystem<double> {
                                       make_unique<Sphere>(1.0), "ball"));
 
     ProximityProperties prox_props;
-    prox_props.AddProperty(geometry::kMaterialGroup, geometry::kElastic, 1e8);
+    prox_props.AddProperty(geometry::internal::kMaterialGroup,
+                           geometry::internal::kElastic, 1e8);
     AddSoftHydroelasticProperties(FLAGS_length, &prox_props);
     scene_graph->AssignRole(source_id_, geometry_id_, prox_props);
 
@@ -250,7 +251,8 @@ int do_main() {
           X_WB, make_unique<Box>(edge_len, edge_len, edge_len), "box"));
   ProximityProperties rigid_props;
   // A rigid hydroelastic geometry must have an infinite elastic modulus.
-  rigid_props.AddProperty(geometry::kMaterialGroup, geometry::kElastic,
+  rigid_props.AddProperty(geometry::internal::kMaterialGroup,
+                          geometry::internal::kElastic,
                           std::numeric_limits<double>::infinity());
   AddRigidHydroelasticProperties(edge_len, &rigid_props);
   scene_graph.AssignRole(source_id, ground_id, rigid_props);

geometry/geometry_roles.h

@@ -144,6 +144,8 @@ namespace geometry {
 
 /** The set of properties for geometry used in a _proximity_ role.
 
+ <!-- TODO(SeanCurtis-TRI): When the hydroelastic geometry module is written,
+  put a reference to the discussion of ProximityProperties here.  -->
  Examples of functionality that depends on the proximity role:
    - n/a
  */

geometry/proximity_properties.cc

@@ -2,26 +2,33 @@
 
 namespace drake {
 namespace geometry {
+namespace internal {
 
 const char* const kMaterialGroup = "material";
 const char* const kElastic = "elastic_modulus";
+const char* const kFriction = "coulomb_friction";
+const char* const kHcDissipation = "hunt_crossley_dissipation";
 
 const char* const kHydroGroup = "hydroelastic";
 const char* const kRezHint = "resolution_hint";
 
+}  // namespace internal
+
 // NOTE: Although these functions currently do the same thing, we're leaving
 // the two functions in place to facilitate future differences.
 
 void AddRigidHydroelasticProperties(double resolution_hint,
                                     ProximityProperties* properties) {
   DRAKE_DEMAND(properties);
-  properties->AddProperty(kHydroGroup, kRezHint, resolution_hint);
+  properties->AddProperty(internal::kHydroGroup, internal::kRezHint,
+                          resolution_hint);
 }
 
 void AddSoftHydroelasticProperties(double resolution_hint,
                                    ProximityProperties* properties) {
   DRAKE_DEMAND(properties);
-  properties->AddProperty(kHydroGroup, kRezHint, resolution_hint);
+  properties->AddProperty(internal::kHydroGroup, internal::kRezHint,
+                          resolution_hint);
 }
 
 }  // namespace geometry

geometry/proximity_properties.h

@@ -11,19 +11,30 @@
 
 namespace drake {
 namespace geometry {
+namespace internal {
 
 /** @name  Declaring general contact material properties
 
  String constants used to access the contact material properties that
  SceneGraph depends on. These are not the exhaustive set of contact material
  properties that downstream consumers (e.g., MultibodyPlant) may require.
 
+ As the namespace indicates, these are for internal use only, so Drake entities
+ can implicitly coordinate the values they use to define proximity properties.
+ These strings don't suggest what constitutes a valid property *value*. For
+ those definitions, one should refer to the consumer of the properties (as
+ called out in the documentation of the ProximityProperties class).
+
  <!-- TODO(SeanCurtis-TRI): Extend this to include other contact material
  properties and an API for setting them more conveniently.  */
 //@{
 
 extern const char* const kMaterialGroup;  ///< The contact material group name.
 extern const char* const kElastic;        ///< Elastic modulus property name.
+extern const char* const kFriction;       ///< Friction coefficients property
+                                          ///< name.
+extern const char* const kHcDissipation;  ///< Hunt-Crossley dissipation
+                                          ///< property name.
 
 //@}
 
@@ -49,6 +60,8 @@ extern const char* const kElastic;        ///< Elastic modulus property name.
 extern const char* const kHydroGroup;      ///< Hydroelastic group name.
 extern const char* const kRezHint;         ///< Resolution hint property name.
 
+}  // namespace internal
+
 /** Adds properties to the given set of proximity properties sufficient to cause
  the associated geometry to generate a rigid hydroelastic representation.
 

geometry/test/proximity_properties_test.cc

@@ -12,8 +12,10 @@ GTEST_TEST(ProximityPropertiesTest, AddRigidProperties) {
   for (double length : {1e-5, 1.25, 1e7}) {
     ProximityProperties props;
     AddRigidHydroelasticProperties(length, &props);
-    EXPECT_TRUE(props.HasProperty(kHydroGroup, kRezHint));
-    EXPECT_EQ(props.GetProperty<double>(kHydroGroup, kRezHint), length);
+    EXPECT_TRUE(props.HasProperty(internal::kHydroGroup, internal::kRezHint));
+    EXPECT_EQ(
+        props.GetProperty<double>(internal::kHydroGroup, internal::kRezHint),
+        length);
   }
 }
 
@@ -25,8 +27,10 @@ GTEST_TEST(ProximityPropertiesTest, AddSoftProperties) {
   for (double length : {1e-5, 1.25, 1e7}) {
     ProximityProperties props;
     AddSoftHydroelasticProperties(length, &props);
-    EXPECT_TRUE(props.HasProperty(kHydroGroup, kRezHint));
-    EXPECT_EQ(props.GetProperty<double>(kHydroGroup, kRezHint), length);
+    EXPECT_TRUE(props.HasProperty(internal::kHydroGroup, internal::kRezHint));
+    EXPECT_EQ(
+        props.GetProperty<double>(internal::kHydroGroup, internal::kRezHint),
+        length);
   }
 }
 

multibody/parsing/detail_scene_graph.cc

@@ -8,6 +8,7 @@
 #include <sdf/sdf.hh>
 
 #include "drake/geometry/geometry_instance.h"
+#include "drake/geometry/proximity_properties.h"
 #include "drake/multibody/parsing/detail_common.h"
 #include "drake/multibody/parsing/detail_ignition.h"
 #include "drake/multibody/parsing/detail_path_utils.h"
@@ -22,6 +23,7 @@ using std::make_unique;
 
 using geometry::GeometryInstance;
 using geometry::IllustrationProperties;
+using geometry::ProximityProperties;
 using math::RigidTransformd;
 
 namespace {
@@ -350,6 +352,87 @@ RigidTransformd MakeGeometryPoseFromSdfCollision(
   return X_LC;
 }
 
+ProximityProperties MakeProximityPropertiesForCollision(
+    const sdf::Collision& sdf_collision) {
+  geometry::ProximityProperties properties;
+  const sdf::ElementPtr collision_element = sdf_collision.Element();
+  DRAKE_DEMAND(collision_element != nullptr);
+
+  const sdf::Element* const drake_element =
+      MaybeGetChildElement(*collision_element, "drake:proximity_properties");
+
+  if (drake_element != nullptr) {
+    auto read_double = [drake_element, &properties](const char* element_name,
+                                                    const char* group_name,
+                                                    const char* property_name) {
+      if (MaybeGetChildElement(*drake_element, element_name) != nullptr) {
+        const double value =
+            GetChildElementValueOrThrow<double>(*drake_element, element_name);
+        properties.AddProperty(group_name, property_name, value);
+      }
+    };
+
+    read_double("drake:mesh_resolution_hint", geometry::internal::kHydroGroup,
+                geometry::internal::kRezHint);
+    read_double("drake:elastic_modulus", geometry::internal::kMaterialGroup,
+                geometry::internal::kElastic);
+    read_double("drake:hunt_crossley_dissipation",
+                geometry::internal::kMaterialGroup,
+                geometry::internal::kHcDissipation);
+
+    auto[mu_dynamic, dynamic_read] =
+        drake_element->Get<double>("drake:mu_dynamic", -1.0);
+    auto[mu_static, static_read] =
+        drake_element->Get<double>("drake:mu_static", -1.0);
+    // Note: we rely on the constructor of CoulombFriction to detect negative
+    // values and bad relationship between static and dynamic coefficients.
+    if (dynamic_read && static_read) {
+      properties.AddProperty(geometry::internal::kMaterialGroup,
+                             geometry::internal::kFriction,
+                             CoulombFriction<double>{mu_static, mu_dynamic});
+    } else if (dynamic_read) {
+      properties.AddProperty(geometry::internal::kMaterialGroup,
+                             geometry::internal::kFriction,
+                             CoulombFriction<double>{mu_dynamic, mu_dynamic});
+    } else if (static_read) {
+      properties.AddProperty(geometry::internal::kMaterialGroup,
+                             geometry::internal::kFriction,
+                             CoulombFriction<double>{mu_static, mu_static});
+    }
+  }
+
+  if (!properties.HasProperty(geometry::internal::kMaterialGroup,
+                              geometry::internal::kFriction)) {
+    properties.AddProperty(
+        geometry::internal::kMaterialGroup, geometry::internal::kFriction,
+        MakeCoulombFrictionFromSdfCollisionOde(sdf_collision));
+  } else {
+    // We parsed friction from <drake:proximity_properties>; test for the
+    // existence of the legacy mechanism and warn we're not using it.
+    const sdf::Element* const surface_element =
+        MaybeGetChildElement(*collision_element, "surface");
+    if (surface_element) {
+      const sdf::Element* friction_element =
+          MaybeGetChildElement(*surface_element, "friction");
+      if (friction_element) {
+        const sdf::Element* ode_element =
+            MaybeGetChildElement(*friction_element, "ode");
+        if (MaybeGetChildElement(*ode_element, "mu") ||
+        MaybeGetChildElement(*ode_element, "mu2")) {
+          logging::Warn one_time(
+              "When drake contact parameters are fully specified in the "
+              "<drake:proximity_properties> tag, the <surface><friction><ode>"
+              "<mu*> tags are ignored. While parsing, there was at least one "
+              "instance where friction coefficients were defined in both "
+              "locations.");
+        }
+      }
+    }
+  }
+
+  return properties;
+}
+
 CoulombFriction<double> MakeCoulombFrictionFromSdfCollisionOde(
     const sdf::Collision& sdf_collision) {
 

multibody/parsing/detail_scene_graph.h

@@ -5,6 +5,7 @@
 
 #include <sdf/sdf.hh>
 
+#include "drake/geometry/geometry_roles.h"
 #include "drake/geometry/scene_graph.h"
 #include "drake/math/rigid_transform.h"
 #include "drake/multibody/parsing/package_map.h"
@@ -26,7 +27,7 @@ std::unique_ptr<geometry::Shape> MakeShapeFromSdfGeometry(
  file, this method makes a new drake::geometry::GeometryInstance object from
  this specification.
  This method returns nullptr when the given SDF specification corresponds
- to a geometry of type `sdf::GeometryType::EMPTY` (<empty/> SDF tag.)  */
+ to a geometry of type `sdf::GeometryType::EMPTY` (`<empty/>` SDF tag.)  */
 std::unique_ptr<geometry::GeometryInstance> MakeGeometryInstanceFromSdfVisual(
     const sdf::Visual& sdf_visual);
 
@@ -58,7 +59,7 @@ std::unique_ptr<geometry::GeometryInstance> MakeGeometryInstanceFromSdfVisual(
  https://github.com/RobotLocomotion/drake/issues/10193 -->
 
  The material properties come from the child <material> tag. E.g.,
- ```xml
+ @code{xml}
  <visual>
    <geometry>
    ...
@@ -70,7 +71,7 @@ std::unique_ptr<geometry::GeometryInstance> MakeGeometryInstanceFromSdfVisual(
      <emissive>r_e g_e b_e a_e</emissive>
    </material>
  </visual>
- ```
+ @endcode
 
  An instance of geometry::IllustrationProperties will always be returned. If
  there is no material tag, no material property tags, or no successfully
@@ -84,6 +85,54 @@ geometry::IllustrationProperties MakeVisualPropertiesFromSdfVisual(
 math::RigidTransformd MakeGeometryPoseFromSdfCollision(
     const sdf::Collision& sdf_collision);
 
+/** Parses the drake-relevant collision properties from a <collision> element.
+
+ Specifically, looks for <drake:proximity_properties> tag to find drake-specific
+ geometry collision (or "proximity") properties. The set of tags are enumerated
+ in the table below. Each tag should be of the form:
+
+ @code{xml}
+   <tag>real_value</tag>
+ @endcode
+
+ As long as no exceptions are thrown, the function is guaranteed to return
+ a valid instance of ProximityProperties.
+
+ Mapping from SDF tag to geometry property. See
+ @ref YET_TO_BE_WRITTEN_HYDROELATIC_GEOMETRY_MODULE for details on the semantics
+ of these properties.
+ | Tag                              | Group        | Property                  | Notes                                           |
+ | :------------------------------: | :----------: | :-----------------------: | :---------------------------------------------: |
+ | drake:mesh_resolution_hint       | hydroelastic | resolution_hint           | Required for hydroelastic contact.              |
+ | drake:elastic_modulus            | material     | elastic_modulus           | ∞ for rigid hydrolelastic models; < ∞ for soft. |
+ | drake:hunt_crossley_dissipation  | material     | hunt_crossley_dissipation |                                                 |
+ | drake:mu_dynamic                 | material     | coulomb_friction          | See note below on friction.                     |
+ | drake:mu_static                  | material     | coulomb_friction          | See note below on friction.                     |
+
+ <h3>Coefficients of friction</h3>
+
+ Parsing coefficients of friction has a relatively complicated protocol:
+
+   1. If one of `<drake:mu_dynamic>` *or* `<drake:mu_static>` is present, the
+      property of type CoulombFriction<double> will be instantiated with both
+      values initialized to the single value. An exception will be thrown
+        - if the value is negative.
+   2. If both `<drake:mu_dynamic>` and `<drake:mu_static>` tags are present, the
+      CoulombFriction<double> will contain both values. An exception will be
+      thrown if:
+        - either value is negative, or
+        - `mu_dynamic` is greater than `mu_static`.
+   3. If *both* tags are missing, the parser will look for two coefficients
+      in the SDF tag path: `<surface><friction><ode><mu>` and
+      `<surface><friction><ode><mu2>`.
+        a. See MakeCoulombFrictionFromSdfCollisionOde() for failure modes.
+   4. If no meaningful friction coefficients are found, a default value will be
+      created (see default_friction()).
+ As long as no exception is thrown, the resulting ProximityProperties will have
+ the ('material', 'coulomb_friction') property.  */
+geometry::ProximityProperties MakeProximityPropertiesForCollision(
+        const sdf::Collision& sdf_collision);
+
 /** Parses friction coefficients from `sdf_collision`.
  This method looks for the definitions specific to ODE, as given by the SDF
  specification in `<collision><surface><friction><ode>`. Drake understands
@@ -101,8 +150,8 @@ math::RigidTransformd MakeGeometryPoseFromSdfCollision(
      </surface>
    </collision>
  ```
- If a `<surface>` is not found, it returns the coefficients for a
- frictionless surface. If `<surface>` is found, all other nested elements
+ If a `<surface>` is not found, it returns arbitrary default coefficients
+ (typically mu = mu2 = 1). If `<surface>` is found, all other nested elements
  are required and an exception is thrown if not present.  */
 CoulombFriction<double> MakeCoulombFrictionFromSdfCollisionOde(
     const sdf::Collision& sdf_collision);