From 4470c794d5768851d30ae9d1c7c44ac0dedb4749 Mon Sep 17 00:00:00 2001
From: mitiguy <paul.mitiguy@tri.global>
Date: Wed, 9 Oct 2024 17:30:44 -0700
Subject: [PATCH] Fix exception message when center of mass is NAN.

---
 multibody/tree/spatial_inertia.cc           | 42 ++++++++++++++++-----
 multibody/tree/test/spatial_inertia_test.cc | 17 +++++++++
 2 files changed, 49 insertions(+), 10 deletions(-)

diff --git a/multibody/tree/spatial_inertia.cc b/multibody/tree/spatial_inertia.cc
index fa3191ac6179..ee93642e74b5 100644
--- a/multibody/tree/spatial_inertia.cc
+++ b/multibody/tree/spatial_inertia.cc
@@ -339,6 +339,11 @@ boolean<T> SpatialInertia<T>::IsPhysicallyValid() const {
   // This spatial inertia is not physically valid if the mass is negative or
   // non-finite or the center of mass or unit inertia matrix have NaN elements.
   boolean<T> ret_value = is_nonnegative_finite(mass_);
+  if constexpr (scalar_predicate<T>::is_bool) {
+    if (!p_PScm_E_.allFinite() || !G_SP_E_.get_moments().allFinite() ||
+        !G_SP_E_.get_products().allFinite())
+      ret_value = false;
+  }
   if (ret_value) {
     // Form a rotational inertia about the body's center of mass and then use
     // the well-documented tests in RotationalInertia to test validity.
@@ -356,13 +361,19 @@ void SpatialInertia<T>::ThrowNotPhysicallyValid() const {
   std::string error_message =
       fmt::format("Spatial inertia fails SpatialInertia::IsPhysicallyValid().");
   const T& mass = get_mass();
+  const Vector3<T>& p_PBcm = get_com();
+
   if (!is_positive_finite(mass)) {
     error_message +=
         fmt::format("\nmass = {} is not positive and finite.\n", mass);
+  } else if (!p_PBcm.array().allFinite()) {
+    error_message += CenterOfMassPositionEqualsString(*this) +=
+        " is not finite.\n";
   } else {
     error_message += fmt::format("{}", *this);
     WriteExtraCentralInertiaProperties(&error_message);
   }
+
   throw std::runtime_error(error_message);
 }
 
@@ -554,6 +565,21 @@ void SpatialInertia<T>::WriteExtraCentralInertiaProperties(
   }
 }
 
+template <typename T>
+std::string CenterOfMassPositionEqualsString(const SpatialInertia<T>& M) {
+  // TODO(jwnimmer-tri) Rewrite this to use fmt to our advantage.
+  const Vector3<T>& p_PBcm = M.get_com();
+  if constexpr (scalar_predicate<T>::is_bool) {
+    const T& x = p_PBcm.x();
+    const T& y = p_PBcm.y();
+    const T& z = p_PBcm.z();
+    return fmt::format("\n Center of mass = [{}  {}  {}]", x, y, z);
+  } else {
+    // Print symbolic results.
+    return fmt::format("\n Center of mass = {}", fmt_eigen(p_PBcm.transpose()));
+  }
+}
+
 template <typename T>
 std::ostream& operator<<(std::ostream& out, const SpatialInertia<T>& M) {
   // Write the data associated with the spatial inertia M of a body
@@ -561,23 +587,19 @@ std::ostream& operator<<(std::ostream& out, const SpatialInertia<T>& M) {
   // Typically point P is either Bo (B's origin) or Bcm (B's center of mass)
   // and frame E is usually the body frame B.  More spatial inertia information
   // can be written via SpatialInertia::WriteExtraCentralInertiaProperties().
-  const T& mass = M.get_mass();
-  const Vector3<T>& p_PBcm = M.get_com();
-  const T& x = p_PBcm.x();
-  const T& y = p_PBcm.y();
-  const T& z = p_PBcm.z();
 
   // TODO(jwnimmer-tri) Rewrite this to use fmt to our advantage.
+  const T& mass = M.get_mass();
   if constexpr (scalar_predicate<T>::is_bool) {
-    out << "\n"
-        << fmt::format(" mass = {}\n", mass)
-        << fmt::format(" Center of mass = [{}  {}  {}]\n", x, y, z);
+    out << fmt::format("\n mass = {}", mass);
   } else {
     // Print symbolic results.
-    out << " mass = " << mass << "\n"
-        << fmt::format(" Center of mass = {}\n", fmt_eigen(p_PBcm.transpose()));
+    out << "\n mass = " << mass;
   }
 
+  // Append a string similar to "\n Center of mass = [x  y  z]\n".
+  out << CenterOfMassPositionEqualsString(M) << "\n";
+
   // Get G_BP (unit inertia about point P) and use it to calculate I_BP
   // (rotational inertia about P) without validity checks such as
   // IsPhysicallyValid().  Hence, this method works for error messages.
diff --git a/multibody/tree/test/spatial_inertia_test.cc b/multibody/tree/test/spatial_inertia_test.cc
index 3286bcd1b792..59a47cd0b877 100644
--- a/multibody/tree/test/spatial_inertia_test.cc
+++ b/multibody/tree/test/spatial_inertia_test.cc
@@ -975,6 +975,23 @@ GTEST_TEST(SpatialInertia, IsPhysicallyValidWithBadInertia) {
       expected_message);
 }
 
+// Ensure IsPhysicallyValid() fails if the center of mass position is NAN.
+// Note: This tests that the old exception message was improved from:
+// "CalcPrincipalMomentsAndMaybeAxesOfInertia(): Unable to calculate eigenvalues
+// or eigenvectors of the 3x3 matrix associated with a RotationalInertia." to
+// a message that clearly communicates a problem with center of mass position.
+GTEST_TEST(SpatialInertia, IsPhysicallyValidWithCMPositionAsNAN) {
+  const std::string expected_message =
+      "Spatial inertia fails SpatialInertia::IsPhysicallyValid\\(\\).\n"
+      " Center of mass = \\[7  nan  9\\] is not finite.\n";
+  const Vector3<double> p_BoBcm_B(7, NAN, 9);
+  const UnitInertia<double> G_BBo_B =
+      UnitInertia<double>::SolidSphere(/* radius = */ 1.0);
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      SpatialInertia<double>(/* mass = */ 1.0, p_BoBcm_B, G_BBo_B),
+      expected_message);
+}
+
 // Tests IsPhysicallyValid() fails within the constructor since the COM given is
 // inconsistently too far out for the unit inertia provided.
 GTEST_TEST(SpatialInertia, IsPhysicallyValidWithCOMTooFarOut) {