[parsing] Improve URDF inertia invalidity reporting

This patch brings inertia invalidity reporting under diagnostic policy,
and relaxes inertia problems from hard throws to warnings.

The move to diagnostic policy means that inertia warnings give file
names and line numbers, which make it easier to fix problems.

The move from hard throws to warnings means that models with
non-physical inertias can still be used with non-simulation workflows,
such as visualization, kinematics, and analysis.

multibody/parsing/detail_urdf_parser.cc

@@ -159,6 +159,13 @@ SpatialInertia<double> UrdfParser::ExtractSpatialInertiaAboutBoExpressedInB(
     ParseScalarAttribute(mass, "value", &body_mass);
   }
 
+  // If physical validity checks fail below, return a prepared point mass
+  // inertia instead. Use a plausible guess for the mass, letting actual mass
+  // validity checking happen later.
+  auto dummy_inertia = SpatialInertia<double>::PointMass(
+      body_mass > 0.0 ? body_mass : 1.0,
+      Vector3d::Zero());
+
   double ixx = 0;
   double ixy = 0;
   double ixz = 0;
@@ -176,12 +183,25 @@ SpatialInertia<double> UrdfParser::ExtractSpatialInertiaAboutBoExpressedInB(
     ParseScalarAttribute(inertia, "izz", &izz);
   }
 
-  const RotationalInertia<double> I_BBcm_Bi(ixx, iyy, izz, ixy, ixz, iyz);
+  // Yes, catching exceptions violates the coding standard. It is done here to
+  // capture math-aware exceptions into parse-time warnings, since non-physical
+  // inertias are all too common, and the thrown messages are actually pretty
+  // useful.
+  RotationalInertia<double> I_BBcm_Bi;
+  try {
+    // Use the factory method here; it doesn't change its diagnostic behavior
+    // between release and debug builds.
+    I_BBcm_Bi = RotationalInertia<double>::MakeFromMomentsAndProductsOfInertia(
+            ixx, iyy, izz, ixy, ixz, iyz);
+  } catch (const std::exception& e) {
+    Warning(*node, e.what());
+    return dummy_inertia;
+  }
 
   // If this is a massless body, return a zero SpatialInertia.
-  if (body_mass == 0. && I_BBcm_Bi.get_moments().isZero() &&
+  if (body_mass == 0.0 && I_BBcm_Bi.get_moments().isZero() &&
       I_BBcm_Bi.get_products().isZero()) {
-    return SpatialInertia<double>(body_mass, {0., 0., 0.}, {0., 0., 0});
+    return SpatialInertia<double>(body_mass, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0});
   }
   // B and Bi are not necessarily aligned.
   const math::RotationMatrix<double> R_BBi(X_BBi.rotation());
@@ -193,8 +213,14 @@ SpatialInertia<double> UrdfParser::ExtractSpatialInertiaAboutBoExpressedInB(
   // http://wiki.ros.org/urdf/XML/link#Elements
   const Vector3d p_BoBcm_B = X_BBi.translation();
 
-  return SpatialInertia<double>::MakeFromCentralInertia(
-      body_mass, p_BoBcm_B, I_BBcm_B);
+  try {
+    return SpatialInertia<double>::MakeFromCentralInertia(
+        body_mass, p_BoBcm_B, I_BBcm_B);
+  } catch (const std::exception& e) {
+    Warning(*node, e.what());
+    return dummy_inertia;
+  }
+  DRAKE_UNREACHABLE();
 }
 
 void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) {

multibody/parsing/test/detail_urdf_parser_test.cc

@@ -1207,7 +1207,7 @@ TEST_F(UrdfParserTest, PointMass) {
   EXPECT_TRUE(body.default_rotational_inertia().get_products().isZero());
 }
 
-TEST_F(UrdfParserTest, BadInertia) {
+TEST_F(UrdfParserTest, BadInertiaFormats) {
   // Test various mis-formatted inputs.
   constexpr const char* base = R"""(
     <robot name='point_mass'>
@@ -1248,36 +1248,41 @@ TEST_F(UrdfParserTest, BadInertia) {
   EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*izz.*"));
 }
 
-// TODO(rpoyner-tri): these tests don't test the parser but rather error
-// behavior of underlying implementation components. Consider moving or
-// removing them.
-class ZeroMassNonZeroInertiaTest : public UrdfParserTest {
- public:
-  void ParseZeroMassNonZeroInertia() {
-    AddModelFromUrdfString(R"""(
-<robot name='bad'>
-  <link name='bad'>
-    <inertial>
-      <mass value="0"/>
-      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
-    </inertial>
-  </link>
-</robot>)""", "");
-  }
-};
-
-TEST_F(ZeroMassNonZeroInertiaTest, ExceptionType) {
-  // Test that attempt to parse links with zero mass and non-zero inertia fails.
-  if (!::drake::kDrakeAssertIsArmed) {
-    EXPECT_THROW(ParseZeroMassNonZeroInertia(), std::runtime_error);
-  }
-}
+TEST_F(UrdfParserTest, BadInertiaValues) {
+  // Test various invalid input values.
+  constexpr const char* base = R"""(
+    <robot name='point_mass'>
+      <link name='point_mass'>
+        <inertial>
+          <mass {}/>
+          <inertia {}/>
+        </inertial>
+      </link>
+    </robot>)""";
 
-TEST_F(ZeroMassNonZeroInertiaTest, Message) {
+  // Absurd rotational inertia values.
+  AddModelFromUrdfString(
+      fmt::format(base, "value='1'",
+                  "ixx='1' ixy='4' ixz='9' iyy='16' iyz='25' izz='36'"), "a");
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*rot.*inertia.*"));
+  // Test some inertia values found in the wild.
+  AddModelFromUrdfString(
+      fmt::format(
+          base, "value='0.038'",
+          "ixx='4.30439933333e-05' ixy='9.57068e-06' ixz='5.1205e-06' "
+          "iyy='1.44451933333e-05' iyz='1.342825e-05' izz='4.30439933333e-05'"),
+      "b");
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*rot.*inertia.*"));
+  // Negative mass.
+  AddModelFromUrdfString(
+      fmt::format(base, "value='-1'",
+                  "ixx='1' ixy='0' ixz='0' iyy='1' iyz='0' izz='1'"), "c");
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*mass > 0.*fail.*"));
   // Test that attempt to parse links with zero mass and non-zero inertia fails.
-  const std::string expected_message = ".*condition 'mass > 0' failed.";
-  DRAKE_EXPECT_THROWS_MESSAGE(
-      ParseZeroMassNonZeroInertia(), expected_message);
+  AddModelFromUrdfString(
+      fmt::format(base, "value='0'",
+                  "ixx='1' ixy='0' ixz='0' iyy='1' iyz='0' izz='1'"), "d");
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*mass > 0.*fail.*"));
 }
 
 TEST_F(UrdfParserTest, BushingParsing) {