Fix DART crash caused by non-positive definite spatial inertia

[XINJIANGMO]

🦟 Bug fix

Fixes #3246

Summary

Previously, if a model contained invalid inertial properties (specifically, a non-positive definite spatial inertia matrix, often caused by incorrect fluid_added_mass coefficients), the invalid matrix was passed directly to DART, triggering an assertion failure (Assertion !math::isNan(partialAcceleration) failed) and causing the server to abort.

This patch adds a validation step in Physics.cc (both in CreateModelEntities and CreateLinkEntities) using Eigen::LLT decomposition to ensure the spatial inertia matrix is positive definite before passing it to the physics engine.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)

[arjo129]

There is a regression introduced in the buoyancy tests. Is it possible to fix this?

[Copilot]

Pull request overview

This PR fixes a crash in the DART physics engine caused by non-positive definite spatial inertia matrices, often resulting from invalid fluid_added_mass coefficients. The fix adds validation using Eigen::LLT decomposition to check matrix positive definiteness before passing inertial properties to the physics engine.

Key changes:

• Added spatial inertia matrix validation in both CreateModelEntities and CreateLinkEntities functions
• Implemented fallback logic to replace invalid inertia with safe defaults (preserved mass, identity-like inertia tensor, preserved CoM pose)
• Added error logging to alert users when invalid inertial properties are detected

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[XINJIANGMO]

There is a regression introduced in the buoyancy tests. Is it possible to fix this?

I tried to fix this , can you review it ?

[arjo129]

Co-pilot rightfully points out a few minor things to fix. FWIW, if the inertia matrix is misconfigured due to added mass issues, the end user seeing weird physics is not the end of the world, but printing an error is definitely worth while as its a common complaint Ive heard of from people configuring added mass.

Lets try to avoid const_casts though.

[XINJIANGMO]

I wil do changes as belows

1. extract check logic to a function
2. avoid using const_casts
3. Consider using a more physically meaningful default