Unify RotationalInertia validity tests and exception messages.

Co-Authored-By: Paul Mitiguy <[email protected]>

multibody/tree/rotational_inertia.cc

@@ -216,31 +216,42 @@ boolean<T> RotationalInertia<
 }
 
 template <typename T>
-void RotationalInertia<T>::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<T>::is_bool) {
-    if (!IsNaN()) {
-      if (!AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality()) {
-        const Vector3<double> 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<std::string> RotationalInertia<T>::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<T>::is_bool) {
+    if (!AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality()) {
+      const Vector3<double> 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 <typename T>
+void RotationalInertia<T>::ThrowIfNotPhysicallyValidImpl(
+    const char* func_name) const {
+  DRAKE_DEMAND(func_name != nullptr);
+  const std::optional<std::string> 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

multibody/tree/rotational_inertia.h

@@ -4,6 +4,7 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <optional>
 #include <ostream>
 #include <sstream>
 #include <string>
@@ -605,8 +606,7 @@ class RotationalInertia {
   ///         calculated (eigenvalue solver) or if scalar type T cannot be
   ///         converted to a double.
   boolean<T> CouldBePhysicallyValid() const {
-    return !IsNaN() &&
-           AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality();
+    return boolean<T>(!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<std::string> CreateInvalidityReport() const;
+
+  // No exception is thrown if type T is Symbolic.
+  void ThrowIfNotPhysicallyValid(const char* func_name) {
+    if constexpr (scalar_predicate<T>::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 T1 = T>
-  typename std::enable_if_t<scalar_predicate<T1>::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 T1 = T>
-  typename std::enable_if_t<!scalar_predicate<T1>::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 <typename T1 = T>

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<double>::quiet_NaN();
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      RotationalInertia<double>::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"