diff --git a/multibody/tree/rotational_inertia.cc b/multibody/tree/rotational_inertia.cc index 9b45b515aba5..74112b8f9fa9 100644 --- a/multibody/tree/rotational_inertia.cc +++ b/multibody/tree/rotational_inertia.cc @@ -216,31 +216,42 @@ boolean RotationalInertia< } template -void RotationalInertia::ThrowNotPhysicallyValid( - const char* func_name) const { - std::string error_message = fmt::format( - "{}(): The rotational inertia\n" - "{}did not pass the test CouldBePhysicallyValid().", - func_name, *this); - // Provide additional information if a moment of inertia is non-negative - // or if moments of inertia do not satisfy the triangle inequality. - if constexpr (scalar_predicate::is_bool) { - if (!IsNaN()) { - if (!AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality()) { - const Vector3 p = CalcPrincipalMomentsOfInertia(); - error_message += fmt::format( - "\nThe associated principal moments of inertia:" - "\n{} {} {}", - p(0), p(1), p(2)); - if (p(0) < 0 || p(1) < 0 || p(2) < 0) { - error_message += "\nare invalid since at least one is negative."; - } else { - error_message += "\ndo not satisfy the triangle inequality."; - } +std::optional RotationalInertia::CreateInvalidityReport() + const { + // Default return value is an empty string (this RotationalInertia is valid). + std::string error_message; + if (IsNaN()) { + error_message = fmt::format("\nNaN detected in RotationalInertia."); + } else if constexpr (scalar_predicate::is_bool) { + if (!AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality()) { + const Vector3 p = CalcPrincipalMomentsOfInertia(); + error_message += fmt::format( + "\nThe associated principal moments of inertia:" + "\n{} {} {}", + p(0), p(1), p(2)); + if (p(0) < 0 || p(1) < 0 || p(2) < 0) { + error_message += "\nare invalid since at least one is negative."; + } else { + error_message += "\ndo not satisfy the triangle inequality."; } } } - throw std::logic_error(error_message); + if (error_message.empty()) return std::nullopt; + return error_message; +} + +template +void RotationalInertia::ThrowIfNotPhysicallyValidImpl( + const char* func_name) const { + DRAKE_DEMAND(func_name != nullptr); + const std::optional invalidity_report = CreateInvalidityReport(); + if (invalidity_report.has_value()) { + const std::string error_message = fmt::format( + "{}(): The rotational inertia\n" + "{}did not pass the test CouldBePhysicallyValid().{}", + func_name, *this, *invalidity_report); + throw std::logic_error(error_message); + } } // TODO(Mitiguy) Consider using this code (or code similar to this) to write diff --git a/multibody/tree/rotational_inertia.h b/multibody/tree/rotational_inertia.h index ce665d0fada1..011fc587eb3f 100644 --- a/multibody/tree/rotational_inertia.h +++ b/multibody/tree/rotational_inertia.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -605,8 +606,7 @@ class RotationalInertia { /// calculated (eigenvalue solver) or if scalar type T cannot be /// converted to a double. boolean CouldBePhysicallyValid() const { - return !IsNaN() && - AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality(); + return boolean(!CreateInvalidityReport().has_value()); } /// Re-expresses `this` rotational inertia `I_BP_E` in place to `I_BP_A`. @@ -978,33 +978,26 @@ class RotationalInertia { return Ixx + epsilon >= 0 && Iyy + epsilon >= 0 && Izz + epsilon >= 0; } + // Returns an error string if `this` RotationalInertia is verifiably invalid. + // Note: Not returning an error string does not _guarantee_ validity. + std::optional CreateInvalidityReport() const; + + // No exception is thrown if type T is Symbolic. + void ThrowIfNotPhysicallyValid(const char* func_name) { + if constexpr (scalar_predicate::is_bool) { + ThrowIfNotPhysicallyValidImpl(func_name); + } + } + + // Throw an exception if CreateInvalidityReport() returns an error string. + void ThrowIfNotPhysicallyValidImpl(const char* func_name) const; + // ========================================================================== // The following set of methods, ThrowIfSomeCondition(), are 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-throw // for non-numeric types. - // This method is used to demand the physical validity of a RotationalInertia - // at either construction or after an operation that could lead to - // non-physical results when a user provides data that is not valid. For - // numerical T-types this would imply computing the rotational inertia - // eigenvalues and checking if they are positive and satisfy the triangle - // inequality. - template - typename std::enable_if_t::is_bool> - ThrowIfNotPhysicallyValid(const char* func_name) { - DRAKE_DEMAND(func_name != nullptr); - if (!CouldBePhysicallyValid()) ThrowNotPhysicallyValid(func_name); - } - - // SFINAE for non-numeric types. See documentation in the implementation for - // numeric types. - template - typename std::enable_if_t::is_bool> - ThrowIfNotPhysicallyValid(const char*) {} - - [[noreturn]] void ThrowNotPhysicallyValid(const char* func_name) const; - // Throws an exception if a rotational inertia is multiplied by a negative // number - which implies that the resulting rotational inertia is invalid. template diff --git a/multibody/tree/test/rotational_inertia_test.cc b/multibody/tree/test/rotational_inertia_test.cc index 69b076487cd6..e8f43fb0201e 100644 --- a/multibody/tree/test/rotational_inertia_test.cc +++ b/multibody/tree/test/rotational_inertia_test.cc @@ -129,9 +129,19 @@ GTEST_TEST(RotationalInertia, MakeFromMomentsAndProductsOfInertia) { /* skip_validity_check = */ true)); EXPECT_FALSE(I.CouldBePhysicallyValid()); + // Check for a thrown exception with proper error message when creating a + // rotational inertia with NaN moments/products of inertia. + std::string expected_message = "[^]*NaN detected in RotationalInertia\\."; + constexpr double nan = std::numeric_limits::quiet_NaN(); + DRAKE_EXPECT_THROWS_MESSAGE( + RotationalInertia::MakeFromMomentsAndProductsOfInertia( + nan, Iyy, Izz, /* Ixy = */ 0, /* Ixz = */ 0, /* Iyz = */ 0, + /* skip_validity_check = */ false), + expected_message); + // Check for a thrown exception with proper error message when creating an // invalid rotational inertia (a principal moment of inertia is negative). - std::string expected_message = + expected_message = "MakeFromMomentsAndProductsOfInertia\\(\\): The rotational inertia\n" "\\[ 1 -3 -3\\]\n" "\\[-3 13 -6\\]\n"