Unit test MbP in marginal contact

multibody/plant/test/multibody_plant_hydroelastic_test.cc

@@ -70,15 +70,16 @@ class HydroelasticModelTests : public ::testing::Test {
   // @param compliant_hydroelastic_modulus is required for the compliant box
   //                                       but not the rigid box.
   void SetUpModel(double mbp_dt, BoxType box_type,
-                  std::optional<double> compliant_box_hydroelastic_modulus) {
+                  std::optional<double> compliant_box_hydroelastic_modulus,
+                  double margin = 0.0) {
     systems::DiagramBuilder<double> builder;
     std::tie(plant_, scene_graph_) =
         AddMultibodyPlantSceneGraph(&builder, mbp_dt);
 
     AddGroundBox(kFrictionCoefficient, plant_, box_type,
                  compliant_box_hydroelastic_modulus);
     body_ = &AddObject(plant_, kSphereRadius, kElasticModulus, kDissipation,
-                       kFrictionCoefficient);
+                       kFrictionCoefficient, margin);
 
     // The default contact model today is hydroelastic with fallback.
     EXPECT_EQ(plant_->get_contact_model(),
@@ -88,6 +89,8 @@ class HydroelasticModelTests : public ::testing::Test {
     plant_->set_contact_model(ContactModel::kHydroelastic);
     ASSERT_EQ(plant_->get_contact_model(), ContactModel::kHydroelastic);
 
+    plant_->SetUseSampledOutputPorts(false);
+
     plant_->Finalize();
 
     // Connect visualizer. Useful for when this test is used for debugging.
@@ -135,7 +138,8 @@ class HydroelasticModelTests : public ::testing::Test {
   const RigidBody<double>& AddObject(MultibodyPlant<double>* plant,
                                      double radius, double hydroelastic_modulus,
                                      double dissipation,
-                                     double friction_coefficient) {
+                                     double friction_coefficient,
+                                     double margin) {
     // Inertial properties are only needed when verifying accelerations since
     // hydro forces are only a function of state.
     const SpatialInertia<double> M_BBcm =
@@ -160,6 +164,8 @@ class HydroelasticModelTests : public ::testing::Test {
         dissipation, {},
         CoulombFriction<double>(friction_coefficient, friction_coefficient),
         &props);
+    props.AddProperty(geometry::internal::kHydroGroup,
+                      geometry::internal::kMargin, margin);
     plant->RegisterCollisionGeometry(body, X_BG, shape, "BodyCollisionGeometry",
                                      std::move(props));
     return body;
@@ -411,6 +417,80 @@ TEST_F(HydroelasticModelTests, Parameters) {
   }
 }
 
+TEST_F(HydroelasticModelTests, ModelWithMargin) {
+  const double kDt = 10e-3;
+  const double kMarginValue = 0.01;
+  const double penetration = -1e-6;  // kMarginValue / 2.0;
+  SetUpModel(kDt, BoxType::kRigid, std::nullopt, kMarginValue);
+  SetPose(penetration);
+
+  // Grab the initial ("old") properties.
+  const std::vector<geometry::GeometryId>& g_ids =
+      plant_->GetCollisionGeometriesForBody(*body_);
+  ASSERT_EQ(g_ids.size(), 1);
+  GeometryId gid = g_ids[0];
+  const ProximityProperties* old_props =
+      scene_graph_->model_inspector().GetProximityProperties(gid);
+  ASSERT_TRUE(old_props != nullptr);
+
+  // Calculate the initial ("old") contact results.
+  const ContactResults<double> old_contact_results =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(old_contact_results.num_hydroelastic_contacts(), 1);
+
+  // Remove margin.
+  ProximityProperties new_props(*old_props);
+  new_props.RemoveProperty(geometry::internal::kHydroGroup,
+                           geometry::internal::kMargin);
+  scene_graph_->AssignRole(scene_graph_context_,
+                           plant_->get_source_id().value(), gid, new_props,
+                           geometry::RoleAssign::kReplace);
+
+  // Calculate the revised ("new") contact results.
+  const ContactResults<double> new_contact_results =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(new_contact_results.num_hydroelastic_contacts(), 0);
+}
+
+TEST_F(HydroelasticModelTests, UpdateMargin) {
+  const double kDt = 10e-3;
+  SetUpModel(kDt, BoxType::kRigid, std::nullopt);
+  const double penetration = -1e-6;
+  const double kMarginValue = 0.01;
+  SetPose(penetration);
+
+  // Grab the initial ("old") properties.
+  const std::vector<geometry::GeometryId>& g_ids =
+      plant_->GetCollisionGeometriesForBody(*body_);
+  ASSERT_EQ(g_ids.size(), 1);
+  GeometryId gid = g_ids[0];
+  const ProximityProperties* old_props =
+      scene_graph_->model_inspector().GetProximityProperties(gid);
+  ASSERT_TRUE(old_props != nullptr);
+
+  // Calculate the initial ("old") contact results.
+  const ContactResults<double> old_contact_results =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(old_contact_results.num_hydroelastic_contacts(), 0);
+
+  // Change margin to non-zero value.
+  ProximityProperties new_props(*old_props);
+  new_props.UpdateProperty(geometry::internal::kHydroGroup,
+                           geometry::internal::kMargin, kMarginValue);
+  scene_graph_->AssignRole(scene_graph_context_,
+                           plant_->get_source_id().value(), gid, new_props,
+                           geometry::RoleAssign::kReplace);
+
+  // Calculate the revised ("new") contact results.
+  const ContactResults<double> new_contact_results =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(new_contact_results.num_hydroelastic_contacts(), 1);
+}
+
 // Verify the results of a simulation using the discrete approximation of
 // hydroelastics.
 TEST_F(HydroelasticModelTests, DiscreteTamsiSolverRigidCompliant) {