Allows calling MBP::set_penetration_allowance() pre-finalize (RobotLo…

…comotion#13435)

multibody/plant/multibody_plant.cc

@@ -734,7 +734,7 @@ void MultibodyPlant<T>::FinalizePlantOnly() {
   DeclareStateCacheAndPorts();
   if (num_collision_geometries() > 0 &&
       penalty_method_contact_parameters_.time_scale < 0)
-    set_penetration_allowance();
+    EstimatePointContactParameters(penetration_allowance_);
   if (num_collision_geometries() > 0 &&
       friction_model_.stiction_tolerance() < 0)
     set_stiction_tolerance();
@@ -998,10 +998,19 @@ void MultibodyPlant<T>::CalcNormalAndTangentContactJacobians(
   }
 }
 
-template<typename T>
+template <typename T>
 void MultibodyPlant<T>::set_penetration_allowance(
     double penetration_allowance) {
-  DRAKE_MBP_THROW_IF_NOT_FINALIZED();
+  penetration_allowance_ = penetration_allowance;
+  // We update the point contact parameters when this method is called
+  // post-finalize.
+  if (this->is_finalized())
+    EstimatePointContactParameters(penetration_allowance);
+}
+
+template <typename T>
+void MultibodyPlant<T>::EstimatePointContactParameters(
+    double penetration_allowance) {
   // Default to Earth's gravity for this estimation.
   const UniformGravityFieldElement<T>& gravity = gravity_field();
   const double g = (!gravity.gravity_vector().isZero())

multibody/plant/multibody_plant.h

@@ -662,8 +662,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
     collision_geometries_ = other.collision_geometries_;
     X_WB_default_list_ = other.X_WB_default_list_;
     contact_model_ = other.contact_model_;
-    if (geometry_source_is_registered())
-      DeclareSceneGraphPorts();
+    penetration_allowance_ = other.penetration_allowance_;
+    if (geometry_source_is_registered()) DeclareSceneGraphPorts();
 
     // MultibodyTree::CloneToScalar() already called MultibodyTree::Finalize()
     // on the new MultibodyTree on U. Therefore we only Finalize the plant's
@@ -1404,6 +1404,9 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// @throws std::exception iff called post-finalize.
   void set_contact_model(ContactModel model);
 
+  // TODO(amcastro-tri): per work in #13064, we should reconsider whether to
+  // deprecate/remove this method alltogether or at least promote to proper
+  // camel case per GSG.
   /// Sets the penetration allowance used to estimate the coefficients in the
   /// penalty method used to impose non-penetration among bodies. Refer to the
   /// section @ref mbp_penalty_method "Contact by penalty method" for further
@@ -3667,6 +3670,16 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   // Declare the system-level cache entries specific to MultibodyPlant.
   void DeclareCacheEntries();
 
+  // Estimates a global set of point contact parameters given a
+  // `penetration_allowance`. See set_penetration_allowance()` for details.
+  // TODO(amcastro-tri): Once #13064 is resolved, make this a method outside MBP
+  // with signature:
+  // EstimatePointContactParameters(double penetration_allowance,
+  //                                MultibodyPlant<double>* plant)
+  // We will document the heuristics used by this method thoroughly so that we
+  // have a place we can refer users to for details.
+  void EstimatePointContactParameters(double penetration_allowance);
+
   // Helper method to assemble actuation input vector from the appropriate
   // ports.
   VectorX<T> AssembleActuationInput(
@@ -4147,6 +4160,10 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   };
   ContactByPenaltyMethodParameters penalty_method_contact_parameters_;
 
+  // Penetration allowance used to estimate ContactByPenaltyMethodParameters.
+  // See set_penetration_allowance() for details.
+  double penetration_allowance_{1.0e-3};
+
   // Stribeck model of friction.
   class StribeckModel {
    public:

multibody/plant/test/inclined_plane_test.cc

@@ -94,8 +94,11 @@ TEST_P(InclinedPlaneTest, RollingSphereTest) {
       coefficient_friction_inclined_plane, coefficient_friction_sphere,
       mass, radius, &plant);
 
-  plant.Finalize();
+  // We should be able to set the penetration allowance pre- and post-finalize.
+  // For this test we decide to set it pre-finalize.
   plant.set_penetration_allowance(penetration_allowance_);  // (in meters)
+
+  plant.Finalize();
   plant.set_stiction_tolerance(stiction_tolerance_);
   const RigidBody<double>& ball = plant.GetRigidBodyByName("BodyB");