[core] Revert to 'crba' to ensure positive definite inertia matrix.

.gitignore

@@ -2,6 +2,7 @@
 *egg-info*
 **/__pycache__
 venv/
+.cache
 .vscode
 .DS_Store
 docs/html

core/include/jiminy/core/robot/pinocchio_overload_algorithms.h

@@ -24,9 +24,10 @@
 #include "pinocchio/multibody/joint/fwd.hpp"  // `pinocchio::JointModelBase`, `pinocchio::JointDataBase`, ...
 #include "pinocchio/algorithm/aba.hpp"       // `pinocchio::aba`
 #include "pinocchio/algorithm/rnea.hpp"      // `pinocchio::rnea`
-#include "pinocchio/algorithm/crba.hpp"      // `pinocchio::crbaMinimal`
+#include "pinocchio/algorithm/crba.hpp"      // `pinocchio::crba`, `pinocchio::crbaMinimal`
 #include "pinocchio/algorithm/energy.hpp"    // `pinocchio::computeKineticEnergy`
 #include "pinocchio/algorithm/cholesky.hpp"  // `pinocchio::cholesky::`
+#include "pinocchio/algorithm/jacobian.hpp"  // `pinocchio::computeJointJacobians`
 
 #include "jiminy/core/fwd.h"
 #include "jiminy/core/engine/engine_multi_robot.h"
@@ -106,9 +107,22 @@ namespace jiminy::pinocchio_overload
     inline const typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs &
     crba(const pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl> & model,
          pinocchio::DataTpl<Scalar, Options, JointCollectionTpl> & data,
-         const Eigen::MatrixBase<ConfigVectorType> & q)
+         const Eigen::MatrixBase<ConfigVectorType> & q,
+         bool fastMath = false)
     {
-        pinocchio::crbaMinimal(model, data, q);
+        /* `pinocchio::crbaMinimal` is faster than `crba` as it also compute the joint jacobians as
+           a by-product without having to call `pinocchio::computeJointJacobians` manually.
+           However, it is numerical instable compared to the classical and thoroughly-tested
+           `pinocchio::crba`, so its use must be avoided in practice. */
+        if (fastMath)
+        {
+            pinocchio::crbaMinimal(model, data, q);
+        }
+        else
+        {
+            pinocchio::crba(model, data, q);
+            pinocchio::computeJointJacobians(model, data);
+        }
         data.M.diagonal() += model.rotorInertia;
         return data.M;
     }
@@ -494,7 +508,7 @@ namespace jiminy::pinocchio_overload
         // Make sure the decomposition of the mass matrix is valid
         if ((data.Dinv.array() < 0.0).any())
         {
-            PRINT_ERROR("The inertia matrix is not strictly positive definite.");
+            PRINT_ERROR("The inertia matrix is not positive definite.");
             return hresult_t::ERROR_BAD_INPUT;
         }
 

core/src/robot/model.cc

@@ -17,7 +17,6 @@
 #include "pinocchio/algorithm/center-of-mass.hpp"       // `pinocchio::centerOfMass`
 #include "pinocchio/algorithm/joint-configuration.hpp"  // `pinocchio::neutral`
 #include "pinocchio/algorithm/kinematics.hpp"           // `pinocchio::forwardKinematics`
-#include "pinocchio/algorithm/jacobian.hpp"             // `pinocchio::computeJointJacobians`
 #include "pinocchio/algorithm/geometry.hpp"             // `pinocchio::updateGeometryPlacements`
 #include "pinocchio/algorithm/cholesky.hpp"             // `pinocchio::cholesky::`
 
@@ -1198,10 +1197,8 @@ namespace jiminy
             return;
         }
 
-        /* Compute inertia matrix, taking into account armature.
-           Note that `crbaMinimal` is faster than `crba` as it also compute the joint jacobians as
-           a by-product without having to call `computeJointJacobians` manually. */
-        pinocchio_overload::crba(pinocchioModel_, pinocchioData_, q);
+        // Compute inertia matrix (taking into account rotor armatures) along with joint jacobians
+        pinocchio_overload::crba(pinocchioModel_, pinocchioData_, q, false);
 
         /* Computing forward kinematics without acceleration to get the drift.
            Note that it will alter the actual joints spatial accelerations, so it is necessary to

python/jiminy_pywrap/src/helpers.cc

@@ -351,7 +351,7 @@ namespace jiminy::python
         bp::def("crba",
                 &pinocchio_overload::crba<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>,
-                (bp::arg("pinocchio_model"), "pinocchio_data", "q"),
+                (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false),
                 "Computes CRBA, store the result in Data and return it.",
                 bp::return_value_policy<result_converter<false>>());
         bp::def("computeKineticEnergy",