[WIP] Bug with hydro geometries in marginal contact

geometry/test/proximity_engine_test.cc

@@ -397,6 +397,61 @@ GTEST_TEST(ProximityEngineTests, HydroelasticAabbInflation) {
   }
 }
 
+GTEST_TEST(ProximityEngineTests, HydroelasticAabbInMarginalContact) {
+  ProximityEngine<double> engine;
+  // All of the geometries will have a scale comparable to edge_length, so that
+  // the mesh creation is as cheap as possible. The exception is Mesh
+  // geometry since we have no re-meshing.
+  const double kLength = 0.5;
+  const double kResHint = kLength / 5;
+  const double E = 1e8;  // Elastic modulus.
+  const double kMarginValue = 0.01;
+  const double kDistance = 1.9 * kMarginValue;
+
+  const Sphere shape(kLength);
+
+  ProximityProperties no_hydro_properties;
+  no_hydro_properties.AddProperty(kHydroGroup, kMargin, kMarginValue);
+
+  ProximityProperties soft_properties(no_hydro_properties);
+  AddCompliantHydroelasticProperties(kResHint, E, &soft_properties);
+
+  ProximityProperties rigid_properties(no_hydro_properties);
+  AddRigidHydroelasticProperties(kResHint, &rigid_properties);
+
+  (void)no_hydro_properties;
+  (void)soft_properties;
+  (void)rigid_properties;
+
+  const GeometryId idA = GeometryId::get_new_id();
+  engine.AddDynamicGeometry(shape, {}, idA, soft_properties);
+
+  const GeometryId idB = GeometryId::get_new_id();
+  engine.AddDynamicGeometry(shape, {}, idB, soft_properties);
+
+  const std::unordered_map<GeometryId, math::RigidTransformd> poses = {
+      {idA, RigidTransformd(Vector3d(0, 0, 0))},
+      {idB, RigidTransformd(Vector3d(2.0 * kLength + kDistance, 0, 0))}};
+  engine.UpdateWorldPoses(poses);
+
+  const SignedDistancePair<double> sdf_result =
+      engine.ComputeSignedDistancePairClosestPoints(idA, idB, poses);
+
+  fmt::print("distance: {}\n", sdf_result.distance);
+  EXPECT_NEAR(sdf_result.distance, kDistance,
+              std::numeric_limits<double>::epsilon());
+
+  std::vector<ContactSurface<double>> surfaces = engine.ComputeContactSurfaces(
+      HydroelasticContactRepresentation::kTriangle, poses);
+  fmt::print("#surfs: {}\n", ssize(surfaces));
+  EXPECT_EQ(ssize(surfaces), 1);
+
+  std::vector<PenetrationAsPointPair<double>> penetrations =
+      engine.ComputePointPairPenetration(poses);
+  fmt::print("#penetrations: {}\n", ssize(penetrations));
+  EXPECT_EQ(ssize(penetrations), 0);
+}
+
 // Test a combination that used to throw an exception.
 GTEST_TEST(ProximityEngineTests, ProcessVtkMeshUndefHydro) {
   ProximityEngine<double> engine;

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,10 @@ class HydroelasticModelTests : public ::testing::Test {
         dissipation, {},
         CoulombFriction<double>(friction_coefficient, friction_coefficient),
         &props);
+    if (margin > 0) {
+      props.AddProperty(geometry::internal::kHydroGroup,
+                        geometry::internal::kMargin, margin);
+    }
     plant->RegisterCollisionGeometry(body, X_BG, shape, "BodyCollisionGeometry",
                                      std::move(props));
     return body;
@@ -411,6 +419,114 @@ TEST_F(HydroelasticModelTests, Parameters) {
   }
 }
 
+// We create a model with margin and later update the sphere's role to remove
+// margin.
+TEST_F(HydroelasticModelTests, ModelWithMarginLaterRemoved) {
+  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* props_with_margin =
+      scene_graph_->model_inspector().GetProximityProperties(gid);
+  ASSERT_TRUE(props_with_margin != nullptr);
+  ASSERT_TRUE(props_with_margin->HasProperty(geometry::internal::kHydroGroup,
+                                             geometry::internal::kMargin));
+
+  // Calculate contact results with MbP's APIs.
+  const ContactResults<double> results_with_margin =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(results_with_margin.num_hydroelastic_contacts(), 1);
+
+  // Compute contact surfaces with SG's APSs.
+  const auto& query_object =
+      scene_graph_->get_query_output_port().Eval<geometry::QueryObject<double>>(
+          *scene_graph_context_);
+  auto surfaces_with_margin = query_object.ComputeContactSurfaces(
+      geometry::HydroelasticContactRepresentation::kTriangle);
+  EXPECT_EQ(ssize(surfaces_with_margin), 1);
+
+  // Remove margin.
+  ProximityProperties props_wo_margin(*props_with_margin);
+  props_wo_margin.RemoveProperty(geometry::internal::kHydroGroup,
+                                 geometry::internal::kMargin);
+  scene_graph_->AssignRole(scene_graph_context_,
+                           plant_->get_source_id().value(), gid,
+                           props_wo_margin, geometry::RoleAssign::kReplace);
+
+  // Calculate the revised ("new") contact results.
+  const ContactResults<double> results_with_margin_removed =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(results_with_margin_removed.num_hydroelastic_contacts(), 0);
+
+  // And with SG.
+  auto surfaces_with_margin_removed = query_object.ComputeContactSurfaces(
+      geometry::HydroelasticContactRepresentation::kTriangle);
+  EXPECT_EQ(ssize(surfaces_with_margin_removed), 0);
+}
+
+// We create a model without margin and later update the sphere's role to have
+// margin.
+TEST_F(HydroelasticModelTests, ModelWoMarginLaterAdded) {
+  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 properties, without margin.
+  const std::vector<geometry::GeometryId>& g_ids =
+      plant_->GetCollisionGeometriesForBody(*body_);
+  ASSERT_EQ(g_ids.size(), 1);
+  GeometryId gid = g_ids[0];
+  const ProximityProperties* props_wo_margin =
+      scene_graph_->model_inspector().GetProximityProperties(gid);
+  ASSERT_TRUE(props_wo_margin != nullptr);
+  ASSERT_FALSE(props_wo_margin->HasProperty(geometry::internal::kHydroGroup,
+                                            geometry::internal::kMargin));
+
+  // Calculate the initial ("old") contact results.
+  const ContactResults<double> results_wo_margin =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(results_wo_margin.num_hydroelastic_contacts(), 0);
+
+  // Compute contact surfaces with SG's APSs.
+  const auto& query_object =
+      scene_graph_->get_query_output_port().Eval<geometry::QueryObject<double>>(
+          *scene_graph_context_);
+  auto surfaces_wo_margin = query_object.ComputeContactSurfaces(
+      geometry::HydroelasticContactRepresentation::kTriangle);
+  EXPECT_EQ(ssize(surfaces_wo_margin), 0);
+
+  // Add margin.
+  ProximityProperties props_with_margin(*props_wo_margin);
+  props_with_margin.UpdateProperty(geometry::internal::kHydroGroup,
+                                   geometry::internal::kMargin, kMarginValue);
+  scene_graph_->AssignRole(scene_graph_context_,
+                           plant_->get_source_id().value(), gid,
+                           props_with_margin, geometry::RoleAssign::kReplace);
+
+  // Calculate contact results with margin.
+  const ContactResults<double> results_with_margin =
+      plant_->get_contact_results_output_port()
+          .template Eval<ContactResults<double>>(*plant_context_);
+  EXPECT_EQ(results_with_margin.num_hydroelastic_contacts(), 1);
+
+  // Compute contact surfaces with SG's APSs.
+  auto surfaces_with_margin = query_object.ComputeContactSurfaces(
+      geometry::HydroelasticContactRepresentation::kTriangle);
+  EXPECT_EQ(ssize(surfaces_with_margin), 1);
+}
+
 // Verify the results of a simulation using the discrete approximation of
 // hydroelastics.
 TEST_F(HydroelasticModelTests, DiscreteTamsiSolverRigidCompliant) {