Wire up HydroelasticEngine to MultibodyPlant (RobotLocomotion#11928)

multibody/hydroelastics/hydroelastic_engine.cc

@@ -1,5 +1,6 @@
 #include "drake/multibody/hydroelastics/hydroelastic_engine.h"
 
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <string>
@@ -33,8 +34,16 @@ namespace internal {
 
 template <typename T>
 HydroelasticGeometry<T>::HydroelasticGeometry(
-    std::unique_ptr<HydroelasticField<T>> mesh_field)
-    : mesh_field_(std::move(mesh_field)) {}
+    std::unique_ptr<HydroelasticField<T>> mesh_field, double elastic_modulus,
+    double hunt_crossley_dissipation)
+    : mesh_field_(std::move(mesh_field)),
+      elastic_modulus_(elastic_modulus),
+      hunt_crossley_dissipation_(hunt_crossley_dissipation) {
+  DRAKE_THROW_UNLESS(std::isfinite(elastic_modulus));
+  DRAKE_THROW_UNLESS(std::isfinite(hunt_crossley_dissipation));
+  DRAKE_THROW_UNLESS(elastic_modulus > 0);
+  DRAKE_THROW_UNLESS(hunt_crossley_dissipation >= 0);
+}
 
 template <typename T>
 HydroelasticGeometry<T>::HydroelasticGeometry(
@@ -51,9 +60,13 @@ void HydroelasticEngine<T>::MakeModels(
       const Shape& shape = inspector.GetShape(geometry_id);
       const double elastic_modulus =
           properties->template GetPropertyOrDefault<double>(
-              "hydroelastics", "elastic modulus",
+              "material", "elastic_modulus",
               std::numeric_limits<double>::infinity());
-      GeometryImplementationData specs{geometry_id, elastic_modulus};
+      const double dissipation =
+          properties->template GetPropertyOrDefault<double>(
+              "material", "hunt_crossley_dissipation", 0.0);
+      GeometryImplementationData specs{geometry_id, elastic_modulus,
+                                       dissipation};
       shape.Reify(this, &specs);
     }
   }
@@ -76,6 +89,49 @@ const HydroelasticGeometry<T>* HydroelasticEngine<T>::get_model(
   return nullptr;
 }
 
+template <typename T>
+double HydroelasticEngine<T>::CalcCombinedElasticModulus(
+    geometry::GeometryId id_A, geometry::GeometryId id_B) const {
+  const HydroelasticGeometry<T>* geometry_A = get_model(id_A);
+  const HydroelasticGeometry<T>* geometry_B = get_model(id_B);
+  // We demand id_A and id_B to correspond to hydroelastic model.
+  DRAKE_DEMAND(geometry_A && geometry_B);
+  const double E_A = geometry_A->elastic_modulus();
+  const double E_B = geometry_B->elastic_modulus();
+  if (E_A == std::numeric_limits<double>::infinity()) return E_B;
+  if (E_B == std::numeric_limits<double>::infinity()) return E_A;
+  return E_A * E_B / (E_A + E_B);
+}
+
+// TODO(amcastro-tri): as of 09/18/2019 we still are discussing how to combine
+// these material properties. Update this method once the discussion is
+// resolved.
+template <typename T>
+double HydroelasticEngine<T>::CalcCombinedDissipation(
+    geometry::GeometryId id_A, geometry::GeometryId id_B) const {
+  const HydroelasticGeometry<T>* geometry_A = get_model(id_A);
+  const HydroelasticGeometry<T>* geometry_B = get_model(id_B);
+  // We demand id_A and id_B to correspond to hydroelastic model.
+  DRAKE_DEMAND(geometry_A && geometry_B);
+  const double E_A = geometry_A->elastic_modulus();
+  const double E_B = geometry_B->elastic_modulus();
+  const double d_A = geometry_A->hunt_crossley_dissipation();
+  const double d_B = geometry_B->hunt_crossley_dissipation();
+  const double Estar = CalcCombinedElasticModulus(id_A, id_B);
+
+  // Both bodies are rigid. We simply return the arithmetic average.
+  if (Estar == std::numeric_limits<double>::infinity())
+    return 0.5 * (d_A + d_B);
+
+  // At least one body is soft.
+  double d_star = 0;
+  if (E_A != std::numeric_limits<double>::infinity())
+    d_star += Estar / E_A * d_A;
+  if (E_B != std::numeric_limits<double>::infinity())
+    d_star += Estar / E_B * d_B;
+  return d_star;
+}
+
 template <typename T>
 std::vector<ContactSurface<T>> HydroelasticEngine<T>::ComputeContactSurfaces(
     const geometry::QueryObject<T>& query_object) const {
@@ -178,6 +234,7 @@ void HydroelasticEngine<T>::ImplementGeometry(const Sphere& sphere,
   const GeometryImplementationData& specs =
       *reinterpret_cast<GeometryImplementationData*>(user_data);
   const double elastic_modulus = specs.elastic_modulus;
+  const double dissipation = specs.dissipation;
   if (elastic_modulus == std::numeric_limits<double>::infinity()) {
     drake::log()->warn(
         "HydroelasticEngine. The current hydroelastic model implementation "
@@ -190,10 +247,8 @@ void HydroelasticEngine<T>::ImplementGeometry(const Sphere& sphere,
   const int refinement_level = 2;
   auto sphere_field =
       MakeSphereHydroelasticField<T>(refinement_level, sphere.get_radius());
-  auto model =
-      std::make_unique<HydroelasticGeometry<T>>(std::move(sphere_field));
-  model->set_elastic_modulus(elastic_modulus);
-
+  auto model = std::make_unique<HydroelasticGeometry<T>>(
+      std::move(sphere_field), elastic_modulus, dissipation);
   model_data_.geometry_id_to_model_[specs.id] = std::move(model);
 }
 

multibody/hydroelastics/hydroelastic_engine.h

@@ -25,9 +25,16 @@ class HydroelasticGeometry {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(HydroelasticGeometry)
 
-  /// Creates a soft model from a given %HydroelasticField object.
-  explicit HydroelasticGeometry(
-      std::unique_ptr<HydroelasticField<T>> mesh_field);
+  /// Creates a soft model from a given %HydroelasticField object, elastic
+  /// modulus and dissipation.
+  /// `elastic_modulus` must be strictly positive and have units of Pa.
+  /// `hunt_crossley_dissipation` must be non-negative and have units of s/m.
+  /// @throws std::exception if `elastic_modulus` is infinite or <= 0.
+  /// @throws std::exception if `hunt_crossley_dissipation` is infinite and or
+  /// negative.
+  HydroelasticGeometry(
+      std::unique_ptr<HydroelasticField<T>> mesh_field, double elastic_modulus,
+      double hunt_crossley_dissipation);
 
   /// Constructor for a rigid model with its geometry represented by the
   /// zero-level of a level set function.
@@ -54,32 +61,27 @@ class HydroelasticGeometry {
     return *level_set_;
   }
 
-  /// Returns the modulus of elasticity for `this` model.
+  /// Returns the modulus of elasticity for `this` model, with units of Pa.
   /// Iff infinity, the model is considered to be rigid.
   double elastic_modulus() const { return elastic_modulus_; }
 
-  /// Sets the modulus of elasticity for `this` model.
-  /// If infinity, the model is considered to be rigid.
-  /// @throws std::exception if the geometry is rigid.
-  /// @throws std::exception if `elastic_modulus` is not strictly positive.
-  /// @throws std::exception if `elastic_modulus` is infinity.
-  void set_elastic_modulus(double elastic_modulus) {
-    DRAKE_THROW_UNLESS(is_soft());
-    DRAKE_THROW_UNLESS(elastic_modulus > 0);
-    DRAKE_THROW_UNLESS(elastic_modulus !=
-                       std::numeric_limits<double>::infinity());
-    elastic_modulus_ = elastic_modulus;
+  /// Returns the Hunt & Crossley dissipation constant for `this` model, with
+  /// units of s/m.
+  double hunt_crossley_dissipation() const {
+    return hunt_crossley_dissipation_;
   }
 
  private:
-  // Model is rigid by default.
-  double elastic_modulus_{std::numeric_limits<double>::infinity()};
   std::unique_ptr<HydroelasticField<T>> mesh_field_;
   std::unique_ptr<LevelSetField<T>> level_set_;
+  // Model is rigid by default.
+  double elastic_modulus_{std::numeric_limits<double>::infinity()};
+  // Model has zero dissipation by default.
+  double hunt_crossley_dissipation_{0};
 };
 
-/// The underlying engine to perform the geometric computations needed by the
-/// hydroelastic model described in:
+/// The underlying engine to perform the computation of contact surfaces and
+/// fields needed by the hydroelastic model described in:
 /// [Elandt, 2019]  R. Elandt, E. Drumwright, M. Sherman, and A. Ruina.
 /// A pressure field model for fast, robust approximation of net contact force
 /// and moment between nominally rigid objects. Proc. IEEE/RSJ Intl. Conf. on
@@ -105,6 +107,11 @@ class HydroelasticGeometry {
 ///
 /// They are already available to link against in the containing library.
 /// No other values for T are currently supported.
+///
+/// %HydroelasticEngine can be instantiated on symbolic::Expression to allow the
+/// compilation of calling code with this scalar type. However usage of any of
+/// the engine's APIs will throw a runtime exception when used with T =
+/// symbolic::Expression.
 template <typename T>
 class HydroelasticEngine final : public geometry::ShapeReifier {
  public:
@@ -136,6 +143,19 @@ class HydroelasticEngine final : public geometry::ShapeReifier {
   /// hydroelastic model for this geometry.
   const HydroelasticGeometry<T>* get_model(geometry::GeometryId id) const;
 
+  /// Computes the combined elastic modulus for geometries with ids `id_A` and
+  /// `id_B`.
+  /// Refer to @ref mbp_hydroelastic_materials_properties "Hydroelastic model
+  /// material properties" for further details.
+  double CalcCombinedElasticModulus(geometry::GeometryId id_A,
+                                    geometry::GeometryId id_B) const;
+
+  /// Computes the combined Hunt & Crossley dissipation for geometries with ids
+  /// `id_A` and `id_B`. Refer to @ref mbp_hydroelastic_materials_properties
+  /// "Hydroelastic model material properties" for further details.
+  double CalcCombinedDissipation(geometry::GeometryId id_A,
+                                 geometry::GeometryId id_B) const;
+
   /// For a given state of `query_object`, this method computes the contact
   /// surfaces for all geometries in contact.
   /// @warning Unsupported geometries are ignored unless the broadphase pass
@@ -150,6 +170,7 @@ class HydroelasticEngine final : public geometry::ShapeReifier {
   struct GeometryImplementationData {
     geometry::GeometryId id;
     double elastic_modulus;
+    double dissipation;
   };
 
   // This struct holds the engines's data, created by the call to MakeModels().
@@ -181,6 +202,35 @@ class HydroelasticEngine final : public geometry::ShapeReifier {
   ModelData model_data_;
 };
 
+// Specialization to support compilation and linking with T =
+// symbolic::Expression. Even though the Clang compiler does not need this, the
+// GCC compiler does need it at linking time.
+template <>
+class HydroelasticEngine<symbolic::Expression> {
+ public:
+  using T = symbolic::Expression;
+
+  void MakeModels(const geometry::SceneGraphInspector<T>&) {
+    Throw("MakeModels");
+  }
+
+  std::vector<geometry::ContactSurface<T>> ComputeContactSurfaces(
+      const geometry::QueryObject<T>&) const {
+    Throw("ComputeContactSurfaces");
+    return std::vector<geometry::ContactSurface<T>>();
+  }
+
+  // TODO(amcastro-tri): Update this list of methods if the API in
+  // HydroelasticEngine grows.
+
+ private:
+  static void Throw(const char* operation_name) {
+    throw std::logic_error(fmt::format(
+        "Cannot call `{}` on a HydroelasticEngine<symbolic::Expression>",
+        operation_name));
+  }
+};
+
 }  // namespace internal
 }  // namespace hydroelastics
 }  // namespace multibody

multibody/hydroelastics/test/hydroelastic_engine_test.cc

@@ -20,10 +20,92 @@ namespace internal {
 namespace {
 
 using drake::geometry::ContactSurface;
+using drake::geometry::GeometryId;
+using drake::geometry::SceneGraph;
 using drake::geometry::SurfaceMesh;
 using drake::math::RigidTransformd;
 using Eigen::Vector3d;
 
+GeometryId AddSoftBody(MultibodyPlant<double>* plant,
+                       const std::string& body_name, double elastic_modulus,
+                       double dissipation) {
+  const RigidBody<double>& body =
+      plant->AddRigidBody(body_name, SpatialInertia<double>());
+  GeometryId geometry_id = plant->RegisterCollisionGeometry(
+      body, RigidTransformd(), geometry::Sphere(1.0), body_name + "_geometry",
+      CoulombFriction<double>());
+  plant->set_elastic_modulus(geometry_id, elastic_modulus);
+  plant->set_hunt_crossley_dissipation(geometry_id, dissipation);
+  return geometry_id;
+}
+
+GeometryId AddRigidBody(MultibodyPlant<double>* plant,
+                        const std::string& body_name) {
+  const RigidBody<double>& body =
+      plant->AddRigidBody(body_name, SpatialInertia<double>());
+  GeometryId geometry_id = plant->RegisterCollisionGeometry(
+      body, RigidTransformd(), geometry::HalfSpace(), body_name + "_geometry",
+      CoulombFriction<double>());
+  return geometry_id;
+}
+
+GTEST_TEST(HydroelasticEngine, CombineSoftAndRigidMaterialProperties) {
+  MultibodyPlant<double> plant;
+  SceneGraph<double> scene_graph;
+  plant.RegisterAsSourceForSceneGraph(&scene_graph);
+
+  const double E_A = 10.0;
+  const double d_A = 1.0;
+  GeometryId geometry_A = AddSoftBody(&plant, "SoftBody", E_A, d_A);
+  GeometryId geometry_B = AddRigidBody(&plant, "RigidBody");
+
+  // Done defining the model.
+  plant.Finalize();
+
+  // Create an engine.
+  HydroelasticEngine<double> engine;
+  engine.MakeModels(scene_graph.model_inspector());
+
+  // The combination must be symmetric.
+  EXPECT_EQ(engine.CalcCombinedElasticModulus(geometry_A, geometry_B), E_A);
+  EXPECT_EQ(engine.CalcCombinedDissipation(geometry_A, geometry_B), d_A);
+  EXPECT_EQ(engine.CalcCombinedElasticModulus(geometry_B, geometry_A), E_A);
+  EXPECT_EQ(engine.CalcCombinedDissipation(geometry_B, geometry_A), d_A);
+}
+
+GTEST_TEST(HydroelasticEngine, CombineSoftAndSoftMaterialProperties) {
+  MultibodyPlant<double> plant;
+  SceneGraph<double> scene_graph;
+  plant.RegisterAsSourceForSceneGraph(&scene_graph);
+
+  const double E_A = 2.0;
+  const double d_A = 1.0;
+  const double E_B = 8.0;
+  const double d_B = 4.0;
+  // Expected combined properties.
+  const double Estar = 1.6;
+  const double dstar = 1.6;
+  GeometryId geometry_A = AddSoftBody(&plant, "SoftBodyA", E_A, d_A);
+  GeometryId geometry_B = AddSoftBody(&plant, "SoftBodyB", E_B, d_B);
+
+  // Done defining the model.
+  plant.Finalize();
+
+  // Create an engine.
+  HydroelasticEngine<double> engine;
+  engine.MakeModels(scene_graph.model_inspector());
+
+  // The combination must be symmetric.
+  EXPECT_NEAR(engine.CalcCombinedElasticModulus(geometry_A, geometry_B), Estar,
+              std::numeric_limits<double>::epsilon());
+  EXPECT_NEAR(engine.CalcCombinedDissipation(geometry_A, geometry_B), dstar,
+              std::numeric_limits<double>::epsilon());
+  EXPECT_NEAR(engine.CalcCombinedElasticModulus(geometry_B, geometry_A), Estar,
+              std::numeric_limits<double>::epsilon());
+  EXPECT_NEAR(engine.CalcCombinedDissipation(geometry_B, geometry_A), dstar,
+              std::numeric_limits<double>::epsilon());
+}
+
 class SphereVsPlaneTest : public ::testing::Test {
  public:
   void SetUp() override {

multibody/plant/BUILD.bazel

@@ -53,6 +53,7 @@ drake_cc_library(
         ":contact_results",
         ":coulomb_friction",
         ":externally_applied_spatial_force",
+        ":hydroelastic_traction",
         ":implicit_stribeck_solver",
         ":implicit_stribeck_solver_results",
         "//common:default_scalars",
@@ -61,6 +62,7 @@ drake_cc_library(
         "//geometry:scene_graph",
         "//math:geometric_transform",
         "//math:orthonormal_basis",
+        "//multibody/hydroelastics:hydroelastic_engine",
         "//multibody/topology:multibody_graph",
         "//multibody/tree",
         "//systems/framework:diagram_builder",
@@ -337,6 +339,15 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "multibody_plant_hydroelastic_test",
+    deps = [
+        ":plant",
+        "//math:geometric_transform",
+        "//systems/framework:diagram",
+    ],
+)
+
 drake_cc_googletest(
     name = "multibody_plant_jacobians_test",
     data = [

multibody/plant/hydroelastic_traction_calculator.cc

@@ -26,10 +26,9 @@ namespace internal {
 
 template <class T>
 void HydroelasticTractionCalculator<T>::
-ComputeSpatialForcesAtBodyOriginsFromHydroelasticModel(
-    const Data& data,
-    double dissipation, double mu_coulomb,
-    SpatialForce<T>* F_Ao_W, SpatialForce<T>* F_Bo_W) const {
+    ComputeSpatialForcesAtBodyOriginsFromHydroelasticModel(
+        const Data& data, double dissipation, double mu_coulomb,
+        SpatialForce<T>* F_Ao_W, SpatialForce<T>* F_Bo_W) const {
   DRAKE_DEMAND(F_Ao_W && F_Bo_W);
 
   // Use a second-order Gaussian quadrature rule. For linear pressure fields,