Add mass matrix regularziation in TSID joint dynamic task (#722)

Author: CarlottaSartore

CHANGELOG.md

@@ -17,6 +17,7 @@ All notable changes to this project are documented in this file.
 - Implement inequality operator for the `PlannedContact` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/750)
 - Finalize `RobotDynamicsEstimator` library and add complete library test (https://github.com/ami-iit/bipedal-locomotion-framework/pull/744)
 - Add Python bindings for `RobotDynamicsEstimator` library (https://github.com/ami-iit/bipedal-locomotion-framework/pull/755)
+- Add possibility to set the regularization on the mass matrix for the `TSID::JointDynamicsTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/722)
 
 ### Changed
 - Remove the possibility to disable the telemetry in `System::AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/726)

src/TSID/include/BipedalLocomotion/TSID/JointDynamicsTask.h

@@ -46,7 +46,8 @@ class JointDynamicsTask : public TSIDLinearTask
     System::VariablesHandler::VariableDescription m_jointsTorqueVariable; /**< Variable
                                                                              describing the joint
                                                                              torques */
-
+    bool m_useMassMatrixRegularizationTerm{false}; /**< True if the mass matrix regularization is used*/
+    Eigen::MatrixXd m_massMatrixRegularizationTerm; /**< Variable storing mass matrix regularization term*/
     struct ContactWrench
     {
         iDynTree::FrameIndex frameIndex; /**< Frame used to express the contact wrench */
@@ -108,6 +109,16 @@ class JointDynamicsTask : public TSIDLinearTask
      */
     bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;
 
+    /**
+     * Set the mass matrix regularization term. i.e. \f$\bar{M} = M + M _ {reg}\f$. Where  \f$M\f$
+     * is the mass matrix and  \f$M_{reg}\f$ is the matrix regularization term.
+     * @param matrix the regularization term for the mass matrix.
+     * @notice Calling this function is not mandatory. Call it only if you want to add a
+     * regularization term.
+     * @return true in case of success, false otherwise.
+     */
+    bool setMassMatrixRegularization(const Eigen::Ref<const Eigen::MatrixXd>& matrix);
+
     /**
      * Update the content of the element.
      * @return True in case of success, false otherwise.

src/TSID/src/JointDynamicsTask.cpp

@@ -37,7 +37,8 @@ bool JointDynamicsTask::setVariablesHandler(const System::VariablesHandler& vari
         return false;
     }
 
-    if (!variablesHandler.getVariable(m_robotAccelerationVariable.name, m_robotAccelerationVariable))
+    if (!variablesHandler.getVariable(m_robotAccelerationVariable.name,
+                                      m_robotAccelerationVariable))
     {
         log()->error("{} Error while retrieving the robot acceleration variable.", errorPrefix);
         return false;
@@ -70,7 +71,7 @@ bool JointDynamicsTask::setVariablesHandler(const System::VariablesHandler& vari
         return false;
     }
 
-    for(auto& contact : m_contactWrenches)
+    for (auto& contact : m_contactWrenches)
     {
         if (!variablesHandler.getVariable(contact.variable.name, contact.variable))
         {
@@ -101,14 +102,45 @@ bool JointDynamicsTask::setVariablesHandler(const System::VariablesHandler& vari
     return true;
 }
 
-bool JointDynamicsTask::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
+bool JointDynamicsTask::setMassMatrixRegularization(const Eigen::Ref<const Eigen::MatrixXd>& matrix)
+{
+    constexpr auto logPrefix = "[JointDynamicsTask::"
+                               "setMassMatrixRegularization]";
+
+    if ((m_kinDyn == nullptr) || (!m_kinDyn->isValid()))
+    {
+        log()->error("{} Please call setKinDyn() before.", logPrefix);
+        return false;
+    }
+
+    if ((m_kinDyn->getNrOfDegreesOfFreedom() != matrix.rows()) || (matrix.cols() != matrix.rows()))
+    {
+        const auto rightSize = m_kinDyn->getNrOfDegreesOfFreedom();
+
+        log()->error("{} The size of the regularization matrix is not correct. The correct size "
+                     "is:  {} x {}. While the input of the function is a {} x {} matrix.",
+                     logPrefix,
+                     rightSize,
+                     rightSize,
+                     matrix.rows(),
+                     matrix.cols());
+        return false;
+    }
+
+    m_massMatrixRegularizationTerm = matrix;
+    m_useMassMatrixRegularizationTerm = true;
+
+    return true;
+}
+
+bool JointDynamicsTask::initialize(
+    std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
 {
     constexpr auto errorPrefix = "[JointDynamicsTask::initialize]";
 
     if (m_kinDyn == nullptr || !m_kinDyn->isValid())
     {
-        log()->error("{} KinDynComputations object is not valid.",
-                     errorPrefix);
+        log()->error("{} KinDynComputations object is not valid.", errorPrefix);
         return false;
     }
 
@@ -211,8 +243,17 @@ bool JointDynamicsTask::update()
     }
 
     m_b = m_generalizedBiasForces.tail(m_kinDyn->getNrOfDegreesOfFreedom());
-    iDynTree::toEigen(this->subA(m_robotAccelerationVariable))
-        = -m_massMatrix.bottomRows(m_kinDyn->getNrOfDegreesOfFreedom());
+
+    if (m_useMassMatrixRegularizationTerm)
+    {
+        iDynTree::toEigen(this->subA(m_robotAccelerationVariable))
+            = -(m_massMatrix.bottomRows(m_kinDyn->getNrOfDegreesOfFreedom())
+                + m_massMatrixRegularizationTerm);
+    } else
+    {
+        iDynTree::toEigen(this->subA(m_robotAccelerationVariable))
+            = -m_massMatrix.bottomRows(m_kinDyn->getNrOfDegreesOfFreedom());
+    }
 
     for (const auto& contactWrench : m_contactWrenches)
     {