Handle tiny SDF inertial data and improve warnings (#2284)

Author: jslee02

dart/utils/sdf/SdfParser.cpp

@@ -1081,6 +1081,7 @@ SDFBodyNode readBodyNode(
   // Name attribute
   std::string name = getAttributeString(bodyNodeElement, "name");
   properties.mName = name;
+  const std::string bodyName = name;
 
   //--------------------------------------------------------------------------
   // gravity
@@ -1108,13 +1109,33 @@ SDFBodyNode readBodyNode(
 
   //--------------------------------------------------------------------------
   // inertia
+  constexpr double kMinReasonableMass = 1e-9; // 1 microgram
+  bool massSpecified = false;
   if (hasElement(bodyNodeElement, "inertial")) {
     const ElementPtr& inertiaElement = getElement(bodyNodeElement, "inertial");
 
     // mass
     if (hasElement(inertiaElement, "mass")) {
       double mass = getValueDouble(inertiaElement, "mass");
+      if (mass <= 0.0) {
+        DART_WARN(
+            "[SdfParser] Link [{}] has non-positive mass [{}]. Clamping to {} "
+            "to continue parsing.",
+            bodyName,
+            mass,
+            kMinReasonableMass);
+        mass = kMinReasonableMass;
+      } else if (mass < kMinReasonableMass) {
+        DART_WARN(
+            "[SdfParser] Link [{}] has a very small mass [{} kg]; clamping to "
+            "{} to avoid numerical issues.",
+            bodyName,
+            mass,
+            kMinReasonableMass);
+        mass = kMinReasonableMass;
+      }
       properties.mInertia.setMass(mass);
+      massSpecified = true;
     }
 
     // offset
@@ -1137,7 +1158,32 @@ SDFBodyNode readBodyNode(
       double iyz = getValueDouble(moiElement, "iyz");
 
       properties.mInertia.setMoment(ixx, iyy, izz, ixy, ixz, iyz);
+    } else if (massSpecified) {
+      // Keep the inertia physically meaningful by matching the moment scale
+      // to the specified mass; geometry is unknown, so use an isotropic guess.
+      const double mass = properties.mInertia.getMass();
+      properties.mInertia.setMoment(Eigen::Matrix3d::Identity() * mass);
+      DART_WARN(
+          "[SdfParser] Link [{}] defines <mass> but no <inertia>; using an "
+          "isotropic inertia tensor (mass * I). Provide <inertia> for "
+          "physically correct behavior.",
+          bodyName);
     }
+
+    if (!massSpecified) {
+      DART_WARN(
+          "[SdfParser] Link [{}] is missing <mass>; using default mass of 1 "
+          "kg. "
+          "Specify <inertial><mass> to avoid unstable simulation.",
+          bodyName);
+    }
+  } else {
+    DART_WARN(
+        "[SdfParser] Link [{}] is missing <inertial>; using default "
+        "mass/inertia "
+        "(1 kg, unit inertia). Specify <inertial> to avoid unstable "
+        "simulation.",
+        bodyName);
   }
 
   SDFBodyNode sdfBodyNode;

tests/integration/io/test_SdfParser.cpp

@@ -35,6 +35,7 @@
 #include "dart/common/Resource.hpp"
 #include "dart/common/ResourceRetriever.hpp"
 #include "dart/common/Uri.hpp"
+#include "dart/config.hpp"
 #include "dart/dynamics/FreeJoint.hpp"
 #include "dart/dynamics/MeshShape.hpp"
 #include "dart/dynamics/PlanarJoint.hpp"
@@ -52,9 +53,16 @@
 #include <filesystem>
 #include <iostream>
 #include <map>
+#include <sstream>
+#include <string>
 
 #include <cstring>
 
+#if DART_HAVE_spdlog
+  #include <spdlog/sinks/ostream_sink.h>
+  #include <spdlog/spdlog.h>
+#endif
+
 using namespace dart;
 using namespace dart::dynamics;
 using namespace dart::test;
@@ -140,6 +148,65 @@ class MemoryResourceRetriever final : public common::ResourceRetriever
   std::map<std::string, std::string> mFiles;
 };
 
+class LogCapture
+{
+public:
+  LogCapture()
+  {
+#if DART_HAVE_spdlog
+    mStream = std::make_shared<std::ostringstream>();
+    mSink = std::make_shared<spdlog::sinks::ostream_sink_mt>(*mStream);
+    mPreviousLogger = spdlog::default_logger();
+    mLogger = std::make_shared<spdlog::logger>("sdf-parser-log-capture", mSink);
+    mLogger->set_level(spdlog::level::trace);
+    mLogger->flush_on(spdlog::level::trace);
+    spdlog::set_default_logger(mLogger);
+#else
+    mOldCout = std::cout.rdbuf(mStream.rdbuf());
+    mOldCerr = std::cerr.rdbuf(mStream.rdbuf());
+#endif
+  }
+
+  ~LogCapture()
+  {
+#if DART_HAVE_spdlog
+    if (mLogger)
+      mLogger->flush();
+    spdlog::set_default_logger(mPreviousLogger);
+    if (mLogger)
+      spdlog::drop(mLogger->name());
+#else
+    std::cout.rdbuf(mOldCout);
+    std::cerr.rdbuf(mOldCerr);
+#endif
+  }
+
+  std::string contents()
+  {
+#if DART_HAVE_spdlog
+    if (mLogger)
+      mLogger->flush();
+    if (mStream)
+      return mStream->str();
+    return {};
+#else
+    return mStream.str();
+#endif
+  }
+
+private:
+#if DART_HAVE_spdlog
+  std::shared_ptr<std::ostringstream> mStream;
+  std::shared_ptr<spdlog::sinks::ostream_sink_mt> mSink;
+  std::shared_ptr<spdlog::logger> mPreviousLogger;
+  std::shared_ptr<spdlog::logger> mLogger;
+#else
+  std::ostringstream mStream;
+  std::streambuf* mOldCout;
+  std::streambuf* mOldCerr;
+#endif
+};
+
 } // namespace
 
 //==============================================================================
@@ -480,3 +547,91 @@ f 1 3 4
       });
   EXPECT_TRUE(foundMesh);
 }
+
+//==============================================================================
+TEST(SdfParser, WarnsOnMissingInertialBlock)
+{
+  auto retriever = std::make_shared<MemoryResourceRetriever>();
+  const std::string modelUri = "memory://pkg/models/missing_inertial/model.sdf";
+
+  const std::string modelSdf = R"(
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <model name="no_inertial">
+    <link name="link_without_inertial">
+      <visual name="v">
+        <geometry>
+          <box><size>0.1 0.1 0.1</size></box>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
+)";
+
+  retriever->add(modelUri, modelSdf);
+
+  LogCapture capture;
+  SdfParser::Options options;
+  options.mResourceRetriever = retriever;
+  auto skeleton = SdfParser::readSkeleton(modelUri, options);
+
+  ASSERT_TRUE(skeleton);
+  ASSERT_EQ(skeleton->getNumBodyNodes(), 1u);
+  const auto* body = skeleton->getBodyNode(0);
+  EXPECT_DOUBLE_EQ(body->getMass(), 1.0);
+
+  const auto logs = capture.contents();
+  EXPECT_NE(logs.find("missing <inertial>"), std::string::npos)
+      << "Expected warning about missing <inertial> block in logs: " << logs;
+}
+
+//==============================================================================
+TEST(SdfParser, WarnsOnTinyMassAndDefaultsInertia)
+{
+  auto retriever = std::make_shared<MemoryResourceRetriever>();
+  const std::string modelUri = "memory://pkg/models/tiny_mass/model.sdf";
+  const double tinyMass = 1e-14;
+  const double clampedMass = 1e-9; // matches parser clamp
+
+  const std::string modelSdf = std::string(R"(
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <model name="tiny_mass_model">
+    <link name="link_with_mass_only">
+      <inertial>
+        <mass>)") + std::to_string(tinyMass)
+                               + R"(</mass>
+      </inertial>
+      <collision name="c">
+        <geometry>
+          <box><size>0.1 0.1 0.1</size></box>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>
+)";
+
+  retriever->add(modelUri, modelSdf);
+
+  LogCapture capture;
+  SdfParser::Options options;
+  options.mResourceRetriever = retriever;
+  auto skeleton = SdfParser::readSkeleton(modelUri, options);
+
+  ASSERT_TRUE(skeleton);
+  ASSERT_EQ(skeleton->getNumBodyNodes(), 1u);
+  const auto* body = skeleton->getBodyNode(0);
+  const auto inertia = body->getInertia();
+  EXPECT_DOUBLE_EQ(inertia.getMass(), clampedMass);
+  const Eigen::Matrix3d expectedMoment
+      = Eigen::Matrix3d::Identity() * clampedMass;
+  EXPECT_TRUE(inertia.getMoment().isApprox(expectedMoment));
+
+  const auto logs = capture.contents();
+  EXPECT_NE(logs.find("Clamping to"), std::string::npos)
+      << "Expected warning about tiny mass clamping in logs: " << logs;
+  EXPECT_NE(logs.find("defines <mass> but no <inertia>"), std::string::npos)
+      << "Expected warning about missing inertia tensor in logs: " << logs;
+}