diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index f61844d7acc7..3ba56cf4a295 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1941,7 +1941,7 @@ void MultibodyPlant::CalcGeneralizedAccelerations( if (is_discrete()) CalcGeneralizedAccelerationsDiscrete(context, vdot); else - CalcGeneralizedAccelerationsContinuous(context, vdot); + *vdot = EvalForwardDynamics(context).get_vdot(); } template diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index d48970fdc7c7..4b157addcdf9 100644 --- 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 index 15dfe86f9b13..0e871ef24d50 100644 --- 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::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 index f01fe6399df3..80aff0a2a6c3 100644 --- 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 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 Ds( + -Matrix6::Identity())); + DRAKE_EXPECT_THROWS_MESSAGE_IF_ARMED( + ArticulatedBodyInertia Ds(-Matrix6::Identity()), + std::runtime_error, + "The resulting articulated body inertia is not physically " + "valid.[\\s\\S]*"); } } // namespace