Add debug msg to log auto computed inertial values (#2749)

Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · Jan 31, 2025 · 60a359a · 60a359a
1 parent 1739bdb
commit 60a359a
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -539,6 +539,19 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model,
 
     this->SetParent(linkEntity, modelEntity);
 
+    if (link->AutoInertia())
+    {
+      gzdbg << "Link has auto-inertial enabled: "
+            << scopedName(linkEntity, *this->dataPtr->ecm, "::", false) << "\n";
+      gzdbg << "  pose: " << link->Inertial().Pose() << "\n";
+      gzdbg << "  mass: " << link->Inertial().MassMatrix().Mass() << "\n";
+      gzdbg << "  ixx iyy izz: "
+            << link->Inertial().MassMatrix().DiagonalMoments() << "\n";
+      gzdbg << "  ixy ixz iyz: "
+            << link->Inertial().MassMatrix().OffDiagonalMoments()
+            << std::endl;
+    }
+
     if (canonicalLink == link)
     {
       this->dataPtr->ecm->CreateComponent(linkEntity,