[multibody] Enable MBP to compute contact with HydroelasticWithFallba…

…ck (RobotLocomotion#12745)

1. MBP
  - Introduce new cache entry to handle the hydroelastic fallback query.
    It creates its own abstract data.
  - Eval{PointPairPenetrations|ContactSurfaces) methods now evaluate
    different cache entries based on the contact model.
   - CalcContactResultsContinuous updated to produce results for fallback
     contact.
   - CalcSpatialContactForcesContinuous updated to produce results for
     fallback contact.
   - Tests to make sure the old contact-model-dependent code works as well
     as the hybrid.
2. Rolling sphere demo extended to illustrate fallback model (called
   "hybrid" in the flags).
   - Can change compliance type of rolling sphere.
   - Can introduce wall such that the sphere will have simultaneous forces
     from two different types of contact (point and hydroelastic).

examples/multibody/rolling_sphere/README.md

@@ -0,0 +1,152 @@
+#Rolling Sphere Example
+
+This directory contains an example for illustrating the difference between
+contact models. It consists of a rolling sphere on a horizontal ground surface.
+
+```
+                                <─── wy
+                                ooooooo
+                              o         o
+                             o     vx    o
+                             o     ──>   o
+                             o           o
+                              o         o
+                                ooooooo
+━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+                                Ground
+```
+__Figure 1, Default configuration:__ The example creates a sphere with initial
+translational velocity in the +Wx direction and an angular velocity around the
+-Wy axis (illustrated by the two vectors labeled with the application's
+corresponding parameters vx and wy, respectively). The ball will begin sliding
+in the +Wx direction but eventually friction will cause the ball to slow and
+then accelerate in the -Wx direction.
+
+__Contact models__
+
+There are three contact models that can be exercised (via the `--contact_model`
+parameter). They differ in how they model the contact between two penetrating
+objects:
+
+  - "point": modeled as a point pair.
+  - "hydroelastic": _possibly_ modeled as a contact surface.
+  - "hybrid": modeled as a contact surface where possible, and as a point.
+    pair otherwise. More formally known as "hydroelastic callback with
+    fallback".
+
+The "hydroelastic" model doesn't have full support yet. There are some types
+of geometries that cannot yet be given a hydroelastic representation and some
+types of contact which aren't modeled yet. For example:
+  - Drake `HalfSpace` shapes can't be represented at all
+  - Drake `Mesh` shapes can only be modeled with _rigid_ hydroelastic
+    representation
+  - Contact between two rigid or two soft objects aren't supported.
+
+__Changing the configuration__
+
+This example allows you to experiment with the contact models by changing the
+configuration in various ways:
+  - Add in an optional wall (as shown below). When using "hybrid" or
+    "hydroelastic" contact models, the wall will have a _soft_ hydroelastic
+    representation.
+  - Change the compliance type of the sphere from soft (default) to rigid for
+    the "hybrid" and "hydroelastic" models.
+
+```
+     ▒▒▒▒▒
+     ▒▒▒▒▒
+     ▒▒▒▒▒
+  w  ▒▒▒▒▒
+  a  ▒▒▒▒▒                       <─── wy
+  l  ▒▒▒▒▒                       ooooooo
+  l  ▒▒▒▒▒                     o         o
+     ▒▒▒▒▒                    o     vx    o
+     ▒▒▒▒▒                    o     ──>   o
+     ▒▒▒▒▒                    o           o
+     ▒▒▒▒▒                     o         o
+     ▒▒▒▒▒                       ooooooo
+━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+                                Ground
+```
+__Figure 2, Adding the wall__: by adding the wall, the ball will initially
+behave as described in __Figure 1__ but eventually hit the wall.
+
+
+The following table enumerates some configuration options and the simulation
+outcome. Default values are indicated as "(d)". (Note: the ground is _always_
+rigid for the "hydroelastic" and "hybrid" contact models.)
+
+|  Wall  |  Sphere  | Contact Model | Note |
+| ------ | -------- | ------------- | ------- |
+| no (d) | soft (d) |   point (d)   | Behavior as described in Figure 1 - point pairs visualized |
+| no (d) | soft (d) | hydroelastic  | Behavior as described in Figure 1 - contact surfaces visualized |
+| no (d) | soft (d) |    hybrid     | Same as "hydroelastic" |
+| no (d) |  rigid   |   point (d)   | Behavior as described in Figure 1 - the sphere's rigid declaration is meaningless for point contact and ignored |
+| no (d) |  rigid   | hydroelastic  | Throws _immediate_ exception -- cannot support rigid-rigid contact surface between ground and sphere |
+| no (d) |  rigid   |    hybrid     | Behavior as described in Figure 1 - rigid-rigid contact (between sphere and ground) uses point-pair contact |
+|  yes   | soft (d) |   point (d)   | Behavior as described in Figure 2 - point pairs visualized |
+|  yes   | soft (d) | hydroelastic  | Behavior as described in Figure 2 - Throws exception when sphere hits wall; cannot support soft-soft contact |
+|  yes   | soft (d) |    hybrid     | Behavior as described in Figure 2 - sphere-ground contact is contact surface, sphere-wall contact is point pair |
+|  yes   |  rigid   |    hybrid     | Behavior as described in Figure 2 - sphere-ground contact is point pair, sphere-wall contact is contact surface |
+
+
+## Prerequisites
+
+All instructions assume that you are launching from the `drake`
+workspace directory.
+```
+cd drake
+```
+
+Ensure that you have built the Drake visualizer with
+```
+bazel build //tools:drake_visualizer
+```
+
+Build the example in this directory
+```
+bazel build //examples/multibody/rolling_sphere/...
+```
+
+## Visualizer
+
+Before running the example, launch the visualizer (building it as necessary):
+```
+bazel-bin/tools/drake_visualizer
+```
+
+## Example
+
+Various incarnations of the example combining the parameters in ways to
+illustrate the differences that contact model and hydroelastic representation
+can make.
+
+##### Default behavior
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics
+```
+
+##### Default behavior with hydroelastic contact
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics --contact_model=hydroelastic
+```
+
+##### Default behavior with hybrid contact
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics --contact_model=hybrid
+```
+
+##### Add the wall with hydroelastic contact -- throw when sphere hits the wall
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics --contact_model=hydroelastic --add_wall=1
+```
+
+##### Add the wall with hybrid contact
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics --contact_model=hybrid --add_wall=1
+```
+
+##### Add the wall, make the sphere rigid with hybrid contact
+```
+bazel-bin/examples/multibody/rolling_sphere/rolling_sphere_run_dynamics --contact_model=hybrid --add_wall=1 --rigid_ball=1
+```

examples/multibody/rolling_sphere/make_rolling_sphere_plant.cc

@@ -26,7 +26,8 @@ using drake::math::RigidTransformd;
 std::unique_ptr<drake::multibody::MultibodyPlant<double>> MakeBouncingBallPlant(
     double radius, double mass, double elastic_modulus, double dissipation,
     const CoulombFriction<double>& surface_friction,
-    const Vector3<double>& gravity_W, SceneGraph<double>* scene_graph) {
+    const Vector3<double>& gravity_W, bool rigid_sphere,
+    SceneGraph<double>* scene_graph) {
   auto plant = std::make_unique<MultibodyPlant<double>>();
 
   UnitInertia<double> G_Bcm = UnitInertia<double>::SolidSphere(radius);
@@ -60,7 +61,11 @@ std::unique_ptr<drake::multibody::MultibodyPlant<double>> MakeBouncingBallPlant(
     ProximityProperties ball_props;
     AddContactMaterial(elastic_modulus, dissipation, surface_friction,
                        &ball_props);
-    AddSoftHydroelasticProperties(radius, &ball_props);
+    if (rigid_sphere) {
+      AddRigidHydroelasticProperties(radius, &ball_props);
+    } else {
+      AddSoftHydroelasticProperties(radius, &ball_props);
+    }
     plant->RegisterCollisionGeometry(ball, X_BS, Sphere(radius), "collision",
                                      std::move(ball_props));
 

examples/multibody/rolling_sphere/make_rolling_sphere_plant.h

@@ -31,6 +31,10 @@ namespace bouncing_ball {
 ///   The Coulomb's law coefficients of friction.
 /// @param[in] gravity_W
 ///   The acceleration of gravity vector, expressed in the world frame W.
+/// @param[in] rigid_sphere
+///   If `true`, the sphere will have a _rigid_ hydroelastic representation;
+///   strict hydroelastic contact against the rigid ground will _not_ work. Use
+///   either point or hybrid contact models.
 /// @param scene_graph
 ///   If a SceneGraph is provided with this argument, this factory method
 ///   will register the new multibody plant to be a source for that geometry
@@ -43,7 +47,7 @@ MakeBouncingBallPlant(
     double radius, double mass,
     double elastic_modulus, double dissipation,
     const drake::multibody::CoulombFriction<double>& surface_friction,
-    const Vector3<double>& gravity_W,
+    const Vector3<double>& gravity_W, bool rigid_sphere,
     geometry::SceneGraph<double>* scene_graph = nullptr);
 
 }  // namespace bouncing_ball

examples/multibody/rolling_sphere/rolling_sphere_run_dynamics.cc

@@ -4,6 +4,7 @@
 
 #include "drake/common/drake_assert.h"
 #include "drake/examples/multibody/rolling_sphere/make_rolling_sphere_plant.h"
+#include "drake/geometry/geometry_instance.h"
 #include "drake/geometry/geometry_visualization.h"
 #include "drake/geometry/scene_graph.h"
 #include "drake/lcm/drake_lcm.h"
@@ -24,7 +25,7 @@ DEFINE_double(target_realtime_rate, 0.2,
 DEFINE_string(integration_scheme, "implicit_euler",
               "Integration scheme to be used. Available options are: "
               "'semi_explicit_euler','runge_kutta2','runge_kutta3',"
-              "'implicit_euler'");
+              "'implicit_euler'.");
 
 // Integration parameters.
 DEFINE_double(simulation_time, 2.0,
@@ -35,12 +36,24 @@ DEFINE_double(max_time_step, 1.0e-3,
 
 // Contact model parameters.
 DEFINE_string(contact_model, "point",
-              "Contact model. Options are: 'point', 'hydroelastic'.");
+              "Contact model. Options are: 'point', 'hydroelastic', 'hybrid'.");
 DEFINE_double(elastic_modulus, 5.0e4,
-              "For hydroelastics, elastic modulus, [Pa].");
+              "For hydroelastic (and hybrid) contact, elastic modulus, [Pa].");
 DEFINE_double(dissipation, 5.0,
-              "For hydroelastics, Hunt & Crossley dissipation, [s/m].");
+              "For hydroelastic (and hybrid) contact, Hunt & Crossley "
+              "dissipation, [s/m].");
 DEFINE_double(friction_coefficient, 0.3, "friction coefficient.");
+DEFINE_bool(rigid_ball, false,
+            "If true, the ball is given a rigid hydroelastic representation "
+            "(instead of the default soft value). Making the ball rigid will"
+            "cause the 'hydroelastic' model to fail, you'll need either 'point'"
+            " or 'hybrid'.");
+DEFINE_bool(add_wall, false,
+            "If true, adds a wall with soft hydroelastic representation in the "
+            "path of the default ball trajectory. This will cause the "
+            "simulation to throw when the ball hits the wall with the "
+            "'hydroleastic' model; use the 'hybrid' or 'point' contact model "
+            "to simulate beyond this contact.");
 
 // Sphere's spatial velocity.
 DEFINE_double(vx, 1.5,
@@ -58,7 +71,7 @@ DEFINE_double(wz, 0.0,
 
 // Sphere's pose.
 DEFINE_double(roll, 0.0, "Sphere's initial roll in degrees.");
-DEFINE_double(pitch, 0.0, "Sphere's initial pitch in degrees");
+DEFINE_double(pitch, 0.0, "Sphere's initial pitch in degrees.");
 DEFINE_double(yaw, 0.0, "Sphere's initial yaw in degrees.");
 DEFINE_double(z0, 0.05, "Sphere's initial position in the z-axis.");
 
@@ -71,9 +84,11 @@ namespace {
 using Eigen::AngleAxisd;
 using Eigen::Matrix3d;
 using Eigen::Vector3d;
+using Eigen::Vector4d;
 using drake::geometry::SceneGraph;
 using drake::geometry::SourceId;
 using drake::lcm::DrakeLcm;
+using drake::math::RigidTransformd;
 using drake::multibody::ContactModel;
 using drake::multibody::CoulombFriction;
 using drake::multibody::MultibodyPlant;
@@ -104,7 +119,23 @@ int do_main() {
 
   MultibodyPlant<double>& plant = *builder.AddSystem(MakeBouncingBallPlant(
       radius, mass, FLAGS_elastic_modulus, FLAGS_dissipation, coulomb_friction,
-      -g * Vector3d::UnitZ(), &scene_graph));
+      -g * Vector3d::UnitZ(), FLAGS_rigid_ball, &scene_graph));
+
+  if (FLAGS_add_wall) {
+    geometry::Box wall{0.2, 4, 0.4};
+    const RigidTransformd X_WB(Vector3d{-0.5, 0, 0});
+    geometry::ProximityProperties prox_prop;
+    geometry::AddContactMaterial(1e8, {}, CoulombFriction<double>(),
+                                 &prox_prop);
+    geometry::AddSoftHydroelasticProperties(0.1, &prox_prop);
+    plant.RegisterCollisionGeometry(plant.world_body(), X_WB, wall,
+                                    "wall_collision", std::move(prox_prop));
+
+    geometry::IllustrationProperties illus_prop;
+    illus_prop.AddProperty("phong", "diffuse", Vector4d(0.7, 0.5, 0.4, 0.5));
+    plant.RegisterVisualGeometry(plant.world_body(), X_WB, wall, "wall_visual",
+                                 std::move(illus_prop));
+  }
 
   // Set contact model and parameters.
   if (FLAGS_contact_model == "hydroelastic") {
@@ -115,6 +146,10 @@ int do_main() {
     plant.Finalize();
     // Set how much penetration (in meters) we are willing to accept.
     plant.set_penetration_allowance(0.001);
+  } else if (FLAGS_contact_model == "hybrid") {
+    plant.set_contact_model(ContactModel::kHydroelasticWithFallback);
+    plant.Finalize();
+    plant.set_penetration_allowance(0.001);
   } else {
     throw std::runtime_error("Invalid contact model '" + FLAGS_contact_model +
                              "'.");

geometry/proximity_properties.h

@@ -85,6 +85,9 @@ std::ostream& operator<<(std::ostream& out, const HydroelasticType& type);
 
 
 /** Adds contact material properties to the given set of proximity `properties`.
+ Only the parameters that carry values will be added to the given set of
+ `properties`; no default values will be provided. Downstream consumers of the
+ contact materials can optionally provide defaults for missing properties.
 
  @throws std::logic_error if `elastic_modulus` is not positive, `dissipation` is
                           negative, or any of the contact material properties

multibody/plant/BUILD.bazel

@@ -399,6 +399,7 @@ drake_cc_googletest(
     deps = [
         ":plant",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
         "//math:geometric_transform",
         "//systems/framework:diagram",
     ],