Enables ABA in MBP::DoCalcTimeDerivatives() (RobotLocomotion#12470)

YinjieLin · Jan 3, 2020 · f9e69b6 · f9e69b6
1 parent 5c1d90e
commit f9e69b6
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 5 deletions.
diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc
@@ -1941,7 +1941,7 @@ void MultibodyPlant<T>::CalcGeneralizedAccelerations(
   if (is_discrete())
     CalcGeneralizedAccelerationsDiscrete(context, vdot);
   else
-    CalcGeneralizedAccelerationsContinuous(context, vdot);
+    *vdot = EvalForwardDynamics(context).get_vdot();
 }
 
 template <typename T>

diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel
@@ -438,6 +438,7 @@ drake_cc_googletest(
     deps = [
         ":tree",
         "//common/test_utilities:expect_no_throw",
+        "//common/test_utilities:expect_throws_message",
         "//math:autodiff",
         "//math:gradient",
     ],

diff --git a/multibody/tree/articulated_body_inertia.h b/multibody/tree/articulated_body_inertia.h
@@ -375,11 +375,16 @@ class ArticulatedBodyInertia {
   // Checks that the ArticulatedBodyInertia is physically valid and throws an
   // exception if not. This is mostly used in Debug builds to throw an
   // appropriate exception.
+  // Since this method is used within assertions or demands, we do not try to
+  // attempt a smart way throw based on a given symbolic::Formula but instead we
+  // make these methods a no-op for non-numeric types.
   void CheckInvariants() const {
-    if (!IsPhysicallyValid()) {
-      throw std::runtime_error(
-          "The resulting articulated body inertia is not physically valid. "
-              "See ArticulatedBodyInertia::IsPhysicallyValid()");
+    if constexpr (scalar_predicate<T>::is_bool) {
+      if (!IsPhysicallyValid()) {
+        throw std::runtime_error(
+            "The resulting articulated body inertia is not physically valid. "
+            "See ArticulatedBodyInertia::IsPhysicallyValid()");
+      }
     }
   }
 };

diff --git a/multibody/tree/test/articulated_body_inertia_test.cc b/multibody/tree/test/articulated_body_inertia_test.cc
@@ -5,6 +5,7 @@
 #include "drake/common/eigen_types.h"
 #include "drake/common/symbolic.h"
 #include "drake/common/test_utilities/expect_no_throw.h"
+#include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/math/autodiff.h"
 #include "drake/math/autodiff_gradient.h"
 #include "drake/multibody/tree/spatial_inertia.h"
@@ -192,6 +193,19 @@ GTEST_TEST(ArticulatedBodyInertia, Symbolic) {
   // IsPhysicallyValid() not supported for non-numeric types.
   ArticulatedBodyInertia<symbolic::Expression> Ps;
   EXPECT_ANY_THROW(Ps.IsPhysicallyValid());
+
+  // Invariant checks are a no-op for non-numeric types, allowing us to create
+  // symbolic ABIs also in Debug builds. Therefore this test passes successfully
+  // even though we supply an invalid matrix (in this case a negative definite
+  // matrix.)
+  // The same test however is expected to throw when T = double in Debug builds.
+  DRAKE_EXPECT_NO_THROW(ArticulatedBodyInertia<symbolic::Expression> Ds(
+      -Matrix6<symbolic::Expression>::Identity()));
+  DRAKE_EXPECT_THROWS_MESSAGE_IF_ARMED(
+      ArticulatedBodyInertia<double> Ds(-Matrix6<double>::Identity()),
+      std::runtime_error,
+      "The resulting articulated body inertia is not physically "
+      "valid.[\\s\\S]*");
 }
 
 }  // namespace