Make SAP the default discrete contact solver (#21294)

This PR changes the default discrete solver from TAMSI to SAP, using the kLagged convex approximation of TAMSI's contact model. The update essentially resolves to making MultibodyPlantConfig::discrete_contact_approximation="lagged". Though this is not a breaking change, it will definitely cause different simulation results, even if more correct. This might not be noticeable in most cases, but users should at least be aware of this. There is not much of a difference in terms of model files for those users previously using kTamsi, since kSimilar simply is a convex approximation of the same TAMSI's models. For those users that have been using kSap, the model of dissipation changes from a linear Kelvin-Voigt model controlled by relaxation_time to Hunt & Crossley, controlled by hunt_crossley_dissipation. Re the "release notes: removal of deprecated" label. MbP::set_discrete_contact_solver() is removed. Co-authored-by: Jeremy Nimmer <[email protected]>
RobotLocomotion · Dec 17, 2024 · f1f0cc3 · f1f0cc3
1 parent 147e334
commit f1f0cc3
Show file tree

Hide file tree

Showing 22 changed files with 165 additions and 336 deletions.
diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc
@@ -1106,17 +1106,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("set_contact_model", &Class::set_contact_model, py::arg("model"),
             cls_doc.set_contact_model.doc)
         .def("get_contact_model", &Class::get_contact_model,
-            cls_doc.get_contact_model.doc);
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-    cls  // BR
-        .def("set_discrete_contact_solver",
-            WrapDeprecated(cls_doc.set_discrete_contact_solver.doc_deprecated,
-                &Class::set_discrete_contact_solver),
-            py::arg("contact_solver"),
-            cls_doc.set_discrete_contact_solver.doc_deprecated);
-#pragma GCC diagnostic pop
-    cls  // BR
+            cls_doc.get_contact_model.doc)
         .def("get_discrete_contact_solver", &Class::get_discrete_contact_solver,
             cls_doc.get_discrete_contact_solver.doc)
         .def("set_discrete_contact_approximation",

diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -1232,9 +1232,7 @@ def do_test_multibody_tree_kinematics(self, T, time_step):
             with self.assertRaises(Exception) as cm:
                 a_com = plant.CalcCenterOfMassTranslationalAccelerationInWorld(
                     context=context)
-            self.assertIn(
-                "This method doesn't support T = Expression",
-                str(cm.exception))
+            self.assertIn("Expression", str(cm.exception))
         else:
             a_com = plant.CalcCenterOfMassTranslationalAccelerationInWorld(
                 context=context)
@@ -1246,9 +1244,7 @@ def do_test_multibody_tree_kinematics(self, T, time_step):
             with self.assertRaises(Exception) as cm:
                 a_com = plant.CalcCenterOfMassTranslationalAccelerationInWorld(
                     context=context, model_instances=[instance])
-            self.assertIn(
-                "This method doesn't support T = Expression",
-                str(cm.exception))
+            self.assertIn("Expression", str(cm.exception))
         else:
             a_com = plant.CalcCenterOfMassTranslationalAccelerationInWorld(
                 context=context, model_instances=[instance])
@@ -1370,9 +1366,7 @@ def do_test_multibody_tree_kinematics(self, T, time_step):
             with self.assertRaises(Exception) as cm:
                 A_base = plant.EvalBodySpatialAccelerationInWorld(
                     context, base)
-            self.assertIn(
-                "This method doesn't support T = Expression",
-                str(cm.exception))
+            self.assertIn("Expression", str(cm.exception))
         else:
             A_base = plant.EvalBodySpatialAccelerationInWorld(context, base)
             self.assert_sane(A_base.rotational(), nonzero=False)
@@ -2960,20 +2954,6 @@ def test_contact_model(self):
             plant.set_contact_model(model)
             self.assertEqual(plant.get_contact_model(), model)
 
-    # N.B. MultibodyPlant::set_discrete_contact_solver() is deprecated and will
-    # be removed on or after 2024-04-01. This entire unit test can be removed
-    # entirely with the removal of discrete_contact_solver().
-    def test_discrete_contact_solver(self):
-        plant = MultibodyPlant_[float](0.1)
-        models = [
-            DiscreteContactSolver.kTamsi,
-            DiscreteContactSolver.kSap,
-        ]
-        for model in models:
-            with catch_drake_warnings(expected_count=1) as w:
-                plant.set_discrete_contact_solver(model)
-            self.assertEqual(plant.get_discrete_contact_solver(), model)
-
     def test_discrete_contact_approximation(self):
         plant = MultibodyPlant_[float](0.1)
         approximations = [

diff --git a/bindings/pydrake/visualization/BUILD.bazel b/bindings/pydrake/visualization/BUILD.bazel
@@ -169,6 +169,7 @@ drake_py_unittest(
     name = "model_visualizer_test",
     timeout = "moderate",
     data = [
+        "test/massless_robot.urdf",
         ":model_visualizer",
         "//manipulation/util:test_models",
         "//multibody/benchmarks/acrobot:models",

diff --git a/bindings/pydrake/visualization/model_visualizer.py b/bindings/pydrake/visualization/model_visualizer.py
@@ -166,6 +166,17 @@ def _main():
             visualizer.AddModels(url=item)
         else:
             visualizer.AddModels(item)
+
+    # Not all model files are compatible with computing dynamics (e.g., they
+    # might have zero-mass floating bodies). We'll try computing to see if it
+    # works, but if not then we'll need to turn off contact visualization.
+    try:
+        visualizer.Finalize()
+    except RuntimeError:
+        logging.warning("Contact results cannot be visualized")
+        visualizer._publish_contacts = False
+        visualizer._reload()
+
     visualizer.Run(position=args.position, loop_once=args.loop_once)
 
 

diff --git a/bindings/pydrake/visualization/test/massless_robot.urdf b/bindings/pydrake/visualization/test/massless_robot.urdf
@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<robot name="massless_robot" xmlns:drake="http://drake.mit.edu">
+  <link name="massless_link"/>
+</robot>
diff --git a/bindings/pydrake/visualization/test/model_visualizer_test.py b/bindings/pydrake/visualization/test/model_visualizer_test.py
@@ -50,6 +50,26 @@ def test_model_visualizer(self):
                     args.append(f"--compliance_type=compliant")
                 subprocess.check_call(args)
 
+    def test_model_with_invalid_dynamics(self):
+        """
+        Test on a model with invalid dynamics.
+        The visualizer script will disable visualization of contact forces.
+        """
+
+        # Model containing a free body with zero inertias.
+        # Obviously, physics cannot be computed, and thus visualization of
+        # contact forces is turned off.
+        result = subprocess.run([
+            self.dut,
+            "bindings/pydrake/visualization/test/massless_robot.urdf",
+            "--loop_once"],
+            stderr=subprocess.PIPE,
+            text=True)
+
+        # If the model is handled as expected, the visualizer script prints a
+        # WARNING message.
+        self.assertRegex(result.stderr, "WARNING.*Contact results cannot")
+
     def test_package_url(self):
         """Test that a package URL works."""
         subprocess.check_call([

diff --git a/examples/allegro_hand/joint_control/allegro_single_object_simulation.cc b/examples/allegro_hand/joint_control/allegro_single_object_simulation.cc
@@ -93,8 +93,6 @@ void DoMain() {
 
   auto [plant, scene_graph] = multibody::AddMultibodyPlantSceneGraph(
       &builder, FLAGS_mbp_discrete_update_period);
-  plant.set_discrete_contact_approximation(
-      multibody::DiscreteContactApproximation::kLagged);
 
   std::string hand_model_url;
   if (FLAGS_use_right_hand) {

diff --git a/examples/hydroelastic/python_nonconvex_mesh/drop_pepper.py b/examples/hydroelastic/python_nonconvex_mesh/drop_pepper.py
@@ -28,8 +28,7 @@ def make_pepper_bowl_table(contact_model, time_step):
                            MultibodyPlantConfig(
                              time_step=time_step,
                              contact_model=contact_model,
-                             contact_surface_representation="polygon",
-                             discrete_contact_solver="sap"),
+                             contact_surface_representation="polygon"),
                            builder)
 
     parser = Parser(plant)

diff --git a/examples/multibody/deformable/bubble_gripper.cc b/examples/multibody/deformable/bubble_gripper.cc
@@ -75,8 +75,6 @@ int do_main() {
   MultibodyPlantConfig plant_config;
   DRAKE_DEMAND(FLAGS_discrete_time_step > 0.0);
   plant_config.time_step = FLAGS_discrete_time_step;
-  /* Deformable simulation only works with SAP solver. */
-  plant_config.discrete_contact_approximation = "sap";
 
   auto [plant, scene_graph] = AddMultibodyPlant(plant_config, &builder);
 

diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
@@ -313,20 +313,10 @@ MultibodyPlant<T>::MultibodyPlant(double time_step)
   DRAKE_DEMAND(contact_model_ == ContactModel::kHydroelasticWithFallback);
   DRAKE_DEMAND(MultibodyPlantConfig{}.contact_model ==
                "hydroelastic_with_fallback");
-  // By default, MultibodyPlantConfig::discrete_contact_approximation and
-  // MultibodyPlantConfig::discrete_contact_solver are empty, indicating that
-  // TAMSI is the default approximation and solver.
-  // TODO(amcastro-tri): Along the removal of
-  // MultibodyPlant::set_discrete_contact_solver() on or after 2024-04-01,
-  // the code below should be updated to:
-  //   DRAKE_DEMAND(MultibodyPlantConfig{}.discrete_contact_approximation ==
-  //     "[approximation]");
-  // with [approximation] the name of the default contact approximation
-  // consistent with MultibodyPlantConfig.
   DRAKE_DEMAND(discrete_contact_approximation_ ==
-               DiscreteContactApproximation::kTamsi);
-  DRAKE_DEMAND(MultibodyPlantConfig{}.discrete_contact_solver == "");
-  DRAKE_DEMAND(MultibodyPlantConfig{}.discrete_contact_approximation == "");
+               DiscreteContactApproximation::kLagged);
+  DRAKE_DEMAND(MultibodyPlantConfig{}.discrete_contact_approximation ==
+               "lagged");
 }
 
 template <typename T>
@@ -728,27 +718,6 @@ void MultibodyPlant<T>::set_contact_model(ContactModel model) {
   contact_model_ = model;
 }
 
-template <typename T>
-void MultibodyPlant<T>::set_discrete_contact_solver(
-    DiscreteContactSolver contact_solver) {
-  DRAKE_MBP_THROW_IF_FINALIZED();
-  switch (contact_solver) {
-    case DiscreteContactSolver::kTamsi:
-      if (num_constraints() > 0) {
-        throw std::runtime_error(fmt::format(
-            "You selected TAMSI as the solver, but you have constraints "
-            "registered with this model (num_constraints() == {}). TAMSI does "
-            "not support constraints.",
-            num_constraints()));
-      }
-      discrete_contact_approximation_ = DiscreteContactApproximation::kTamsi;
-      break;
-    case DiscreteContactSolver::kSap:
-      discrete_contact_approximation_ = DiscreteContactApproximation::kSap;
-      break;
-  }
-}
-
 template <typename T>
 DiscreteContactSolver MultibodyPlant<T>::get_discrete_contact_solver() const {
   // Only the TAMSI approximation uses the TAMSI solver.

diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h
@@ -2349,31 +2349,13 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   /// @throws std::exception iff called post-finalize.
   void set_contact_model(ContactModel model);
 
-  /// Sets the contact solver type used for discrete %MultibodyPlant models.
-  ///
-  /// @note Calling this method also sets a default discrete approximation of
-  /// contact (see set_discrete_contact_approximation()) according to:
-  /// - DiscreteContactSolver::kTamsi sets the approximation to
-  ///   DiscreteContactApproximation::kTamsi.
-  /// - DiscreteContactSolver::kSap sets the approximation to
-  ///   DiscreteContactApproximation::kSap.
-  ///
-  /// @warning This function is a no-op for continuous models (when
-  /// is_discrete() is false.)
-  /// @throws std::exception iff called post-finalize.
-  DRAKE_DEPRECATED(
-      "2024-04-01",
-      "Use set_discrete_contact_approximation() to set the contact model "
-      "approximation. The underlying solver will be inferred automatically.")
-  void set_discrete_contact_solver(DiscreteContactSolver contact_solver);
-
   /// Returns the contact solver type used for discrete %MultibodyPlant models.
   DiscreteContactSolver get_discrete_contact_solver() const;
 
   /// Sets the discrete contact model approximation.
   ///
   /// @note Calling this method also sets the contact solver type (see
-  /// set_discrete_contact_solver()) according to:
+  /// get_discrete_contact_solver()) according to:
   /// - DiscreteContactApproximation::kTamsi sets the solver to
   ///   DiscreteContactSolver::kTamsi.
   /// - DiscreteContactApproximation::kSap,
@@ -6186,7 +6168,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
 
   // The contact model approximation used by discrete MultibodyPlant models.
   DiscreteContactApproximation discrete_contact_approximation_{
-      DiscreteContactApproximation::kTamsi};
+      DiscreteContactApproximation::kLagged};
 
   // Near rigid regime parameter from [Castro et al., 2021]. Refer to
   // set_near_rigid_threshold() for details.

diff --git a/multibody/plant/multibody_plant_config.h b/multibody/plant/multibody_plant_config.h
@@ -22,7 +22,6 @@ struct MultibodyPlantConfig {
     a->Visit(DRAKE_NVP(stiction_tolerance));
     a->Visit(DRAKE_NVP(contact_model));
     a->Visit(DRAKE_NVP(discrete_contact_approximation));
-    a->Visit(DRAKE_NVP(discrete_contact_solver));
     a->Visit(DRAKE_NVP(sap_near_rigid_threshold));
     a->Visit(DRAKE_NVP(contact_surface_representation));
     a->Visit(DRAKE_NVP(adjacent_bodies_collision_filters));
@@ -55,29 +54,6 @@ struct MultibodyPlantConfig {
   /// - "hydroelastic_with_fallback"
   std::string contact_model{"hydroelastic_with_fallback"};
 
-  /// Configures the MultibodyPlant::set_discrete_contact_solver().
-  /// Refer to drake::multibody::DiscreteContactSolver for details.
-  /// Valid strings are:
-  /// - "tamsi", uses the TAMSI model approximation.
-  /// - "sap"  , uses the SAP model approximation.
-  ///
-  /// @note If empty, the contact solver is determined by
-  /// discrete_contact_approximation, see
-  /// MultibodyPlant::set_discrete_contact_approximation(). If both
-  /// discrete_contact_solver and discrete_contact_approximation are empty, the
-  /// default model (and solver) is TAMSI.
-  ///
-  /// @warning discrete_contact_solver is deprecated.
-  /// Use discrete_contact_approximation to set the contact model approximation.
-  /// The underlying solver will be inferred automatically. The deprecated code
-  /// will be removed from Drake on or after 2024-04-01.
-  std::string discrete_contact_solver{""};
-
-  // TODO(amcastro-tri): Along the removal of discrete_contact_solver on or
-  // after 2024-04-01, update the default value of
-  // discrete_contact_approximation to be non-empty. The value will be either
-  // "similar" or "lagged", to be decided based on the information we collect
-  // from our users.
   /// Configures the MultibodyPlant::set_discrete_contact_approximation().
   /// Refer to drake::multibody::DiscreteContactApproximation for details.
   /// Valid strings are:
@@ -88,12 +64,7 @@ struct MultibodyPlantConfig {
   ///
   /// Refer to MultibodyPlant::set_discrete_contact_approximation() and the
   /// references therein for further details.
-  ///
-  /// @note If empty, the contact approximation is determined by
-  /// discrete_contact_solver, see set_discrete_contact_solver(). If both
-  /// discrete_contact_solver and discrete_contact_approximation are empty, the
-  /// default model (and solver) is TAMSI.
-  std::string discrete_contact_approximation{""};
+  std::string discrete_contact_approximation{"lagged"};
 
   // TODO(amcastro-tri): Change default to zero, or simply eliminate.
   /// Non-negative dimensionless number typically in the range [0.0, 1.0],