[algorithm/dynamics] add option to take damped inverse when doing cholesky factorization of JMinvJt

bindings/python/algorithm/expose-dynamics.cpp

@@ -30,7 +30,7 @@ namespace se3
       bp::def("forwardDynamics",
               (const VectorXd & (*)(const Model &, Data &,
                                     const VectorXd &, const VectorXd &, const VectorXd &,
-                                    const MatrixXd &, const VectorXd &, const bool))
+                                    const MatrixXd &, const VectorXd &, const double, const bool))
               &forwardDynamics,
               bp::args("Model","Data",
                        "Joint configuration q (size Model::nq)",
@@ -38,7 +38,8 @@ namespace se3
                        "Joint torque tau (size Model::nv)",
                        "Contact Jacobian J (size nb_constraint * Model::nv)",
                        "Contact drift gamma (size nb_constraint)",
-                       "Update kinematics (if true, it updates the dynamic variable according to the current state)"),
+                       "(double) Damping factor for cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.",                       
+                       "Update kinematics (if true, it updates the dynamic variable according to the current state )"),
               "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c",
               bp::return_value_policy<bp::return_by_value>());
       

src/algorithm/dynamics.hpp

@@ -56,6 +56,7 @@ namespace se3
                                                  const Eigen::VectorXd & tau,
                                                  const Eigen::MatrixXd & J,
                                                  const Eigen::VectorXd & gamma,
+                                                 const double inv_damping = 0.,
                                                  const bool updateKinematics = true
                                                  )
   {
@@ -85,6 +86,8 @@ namespace se3
     for(int k=0;k<model.nv;++k) data.sDUiJt.row(k) /= sqrt(data.D[k]);
     
     data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
+
+    data.JMinvJt.diagonal().array() += inv_damping;
     data.llt_JMinvJt.compute(data.JMinvJt);
     
     // Compute the Lagrange Multipliers

unittest/dynamics.cpp

@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
   
   Eigen::MatrixXd H(J.transpose());
   
-  se3::forwardDynamics(model, data, q, v, tau, J, gamma, true);
+  se3::forwardDynamics(model, data, q, v, tau, J, gamma, 0.,true);
   data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
   
   MatrixXd Minv (data.M.inverse());
@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE (timings_fd_llt)
   PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
   SMOOTH(NBT)
   {
-    se3::forwardDynamics(model, data, q, v, tau, J, gamma, true);
+    se3::forwardDynamics(model, data, q, v, tau, J, gamma, 0., true);
   }
   timer.toc(std::cout,NBT);