search

opensim-org / opensim-core

SimTK OpenSim C++ libraries and command-line applications, and Java/Python wrapping.
https://opensim.stanford.edu
Apache License 2.0
796 stars 319 forks source link

SimTK Execption - Inertial Matrix Triangle Inequality Bug #3941

Open alexbeattie42 opened 4 weeks ago

alexbeattie42 commented 4 weeks ago

Problem

When running the opensim example for the IMU-IK solver ("opensense") the following exception is thrown:

what(): SimTK Exception thrown at MassProperties.h:542: Error detected by Simbody method Inertia::operator-=(): Diagonals of an Inertia matrix must satisfy the triangle inequality; got 0.000105365,0.00021073,0.00105365. (Required condition 'Ixx+Iyy+Slop>=Izz && Ixx+Izz+Slop>=Iyy && Iyy+Izz+Slop>=Ixx' was not met.)

Reproduction

I have created a minimum reproduction of the issue here.

Solution

I'm unsure if the warning is correct and the provided example is wrong or the example is correct and the warning is wrong. Once that is established a solution can be determined.

Notes

nickbianco commented 3 weeks ago

This looks to me to not necessarily be a bug in the code, but rather an issue with the mass properties in the model. But I wouldn't expect such an error running the example out-of-the-box like this. @alexbeattie42, perhaps run the example through a debugger so we can take a look at a stack trace?

alexbeattie42 commented 3 weeks ago

Sure thing. @nickbianco Here's the full backtrace from gdb:

#0  __pthread_kill_implementation (threadid=<optimized out>, signo=signo@entry=6, no_tid=no_tid@entry=0) at pthread_kill.c:44
#1  0x00007fffec4a5463 in __pthread_kill_internal (threadid=<optimized out>, signo=6) at pthread_kill.c:78
#2  0x00007fffec44c120 in __GI_raise (sig=sig@entry=6) at ../sysdeps/posix/raise.c:26
#3  0x00007fffec4334c3 in __GI_abort () at abort.c:79
#4  0x00007fffebc97b2c in __gnu_cxx::__verbose_terminate_handler () at /usr/src/debug/gcc/gcc/libstdc++-v3/libsupc++/vterminate.cc:95
#5  0x00007fffebcadf3a in __cxxabiv1::__terminate (handler=<optimized out>)
    at /usr/src/debug/gcc/gcc/libstdc++-v3/libsupc++/eh_terminate.cc:48
#6  0x00007fffebc9752a in std::terminate () at /usr/src/debug/gcc/gcc/libstdc++-v3/libsupc++/eh_terminate.cc:58
#7  0x00007fffebcae1f6 in __cxxabiv1::__cxa_throw (obj=<optimized out>, tinfo=0x555555597f00 <typeinfo for SimTK::Exception::ErrorCheck>, 
    dest=0x555555571440 <SimTK::Exception::ErrorCheck::~ErrorCheck()>) at /usr/src/debug/gcc/gcc/libstdc++-v3/libsupc++/eh_throw.cc:98
#8  0x00007ffff5871e17 in SimTK::Inertia_<double>::errChk(char const*) const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#9  0x00007ffff5a4de95 in SimTK::Inertia_<double>::operator-=(SimTK::Inertia_<double> const&) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#10 0x00007ffff5b28f3c in SimTK::Inertia_<double> SimTK::operator-<double>(SimTK::Inertia_<double> const&, SimTK::Inertia_<double> const&) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#11 0x00007ffff5b2871c in SimTK::MassProperties_<double>::calcCentralInertia() const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#12 0x00007ffff5a503a0 in RigidBodyNode::RigidBodyNode(SimTK::MassProperties_<double> const&, SimTK::Transform_<double> const&, SimTK::Transform_<double> const&, RigidBodyNode::QDotHandling, RigidBodyNode::QuaternionUse, bool) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#13 0x00007ffff5bc0204 in ImmobileRigidBodyNode::ImmobileRigidBodyNode(SimTK::MassProperties_<double> const&, SimTK::Transform_<double> const&, SimTK::Transform_<double> const&, SimTK::UIndex const&, SimTK::USquaredIndex const&, SimTK::QIndex const&) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#14 0x00007ffff5bc2057 in RBNodeWeld::RBNodeWeld(SimTK::MassProperties_<double> const&, SimTK::Transform_<double> const&, SimTK::Transform_<double> const&, SimTK::UIndex const&, SimTK::USquaredIndex const&, SimTK::QIndex const&) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#15 0x00007ffff5bbfa64 in SimTK::MobilizedBody::WeldImpl::createRigidBodyNode(SimTK::UIndex&, SimTK::USquaredIndex&, SimTK::QIndex&) const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#16 0x00007ffff59dd4ce in SimTK::MobilizedBodyImpl::realizeTopology(SimTK::State&, SimTK::UIndex&, SimTK::USquaredIndex&, SimTK::QIndex&) const () from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#17 0x00007ffff5befa3a in SimbodyMatterSubsystemRep::endConstruction(SimTK::State&) ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#18 0x00007ffff5bf0022 in SimbodyMatterSubsystemRep::realizeSubsystemTopologyImpl(SimTK::State&) const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#19 0x00007ffff48e500e in SimTK::Subsystem::Guts::realizeSubsystemTopology(SimTK::State&) const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKcommon.so.3.8
#20 0x00007ffff5a1e755 in SimTK::MultibodySystemRep::realizeTopologyImpl(SimTK::State&) const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKsimbody.so.3.8
#21 0x00007ffff48eb981 in SimTK::System::Guts::realizeTopology() const ()
   from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKcommon.so.3.8
#22 0x00007ffff48ea9d0 in SimTK::System::realizeTopology() const () from /home/alexbeat/opensim-core/sdk/Simbody/lib/libSimTKcommon.so.3.8
#23 0x00007ffff6b80edd in OpenSim::Model::initializeState (this=this@entry=0x5555558ce1c0)
    at /home/alexbeat/opensim-workspace/opensim-core-source/OpenSim/Simulation/Model/Model.cpp:529
#24 0x00007ffff6d3e18f in OpenSim::Model::initSystem (this=0x5555558ce1c0)
--Type <RET> for more, q to quit, c to continue without paging--c
    at /home/alexbeat/opensim-workspace/opensim-core-source/OpenSim/Simulation/Model/Model.h:404
#25 OpenSim::IMUPlacer::run (this=0x7fffffffafb0, visualizeResults=false)
    at /home/alexbeat/opensim-workspace/opensim-core-source/OpenSim/Simulation/OpenSense/IMUPlacer.cpp:106
#26 0x000055555556c6a2 in main ()
nickbianco commented 3 weeks ago

Okay, the exception occurs after this initSystem call inside IMUPlacer::run(). The model is loaded in a few lines before without any modifications, so it does seem to be an issue with the model file. You could try loading the model separately from IMUPlacer and calling initSystem to confirm. The next thing to do is figure out what part of the model is causing the error.

The following line from the stack trace suggests that it has something to do with a body in the model associated with a WeldJoint:

15 0x00007ffff5bbfa64 in SimTK::MobilizedBody::WeldImpl::createRigidBodyNode(SimTK::UIndex&, SimTK::USquaredIndex&, SimTK::QIndex&) const ()

The following joints in the model from the example data are welded: "mtp_l", "mtp_r", "subtalar_l", "subtalar_r", "radius_hand_l", and "radius_hand_r".

alexbeattie42 commented 1 week ago

Following up on this issue, none of the Rajagopal models (or any variant of them) load in the GUI for me. I have tested other models which do load and work properly (like subject07 or gait2329). This leads me to believe there is a problem with the Rajagopal models and the latest version of OpenSim.