diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 9f659a9c..4eb31b63 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -921,31 +921,35 @@ void mjCBody::GeomFrame(void) { // compute full inertia const char* mjCBody::FullInertia(double quat[4], double inertia[3]) { - if (mjuu_defined(fullinertia[0])) { - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { - fullinertia[0], fullinertia[3], fullinertia[4], - fullinertia[3], fullinertia[1], fullinertia[5], - fullinertia[4], fullinertia[5], fullinertia[2] - }; + if (!mjuu_defined(fullinertia[0])) { + return 0; + } - mju_eig3(eigval, eigvec, quattmp, full); + mjtNum eigval[3], eigvec[9], quattmp[4]; + mjtNum full[9] = { + fullinertia[0], fullinertia[3], fullinertia[4], + fullinertia[3], fullinertia[1], fullinertia[5], + fullinertia[4], fullinertia[5], fullinertia[2] + }; - // copy - for (int i=0; i<4; i++) { - quat[i] = quattmp[i]; - } - if (inertia) { - for (int i=0; i<3; i++) { - inertia[i] = eigval[i]; - } - } + mju_eig3(eigval, eigvec, quattmp, full); - // check mimimal eigenvalue - if (eigval[2]inertia); // inertia are sometimes 0 in URDF files: ignore error in altres, fix later