Implement KinDynComputations::getCentroidalTotalMomentumJacobian() method

CHANGELOG.md

@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 - Added a new CMake option `IDYNTREE_COMPILES_TOOLS` to disable compilation of iDynTree tools.
+- Added a `KinDynComputations::getCentroidalTotalMomentumJacobian()` method (https://github.com/robotology/idyntree/pull/706)
 
 ### Removed 
 - Remove the CMake option IDYNTREE_USES_KDL and all the classes available when enabling it. They were deprecated in iDynTree 1.0 .

src/high-level/include/iDynTree/KinDynComputations.h

@@ -623,6 +623,19 @@ class KinDynComputations {
      */
     iDynTree::SpatialMomentum getCentroidalTotalMomentum();
 
+    /**
+     * @brief Get the total centroidal momentum jacobian of the robot.
+     * If G is the center of mass, this quantity is expressed in (G[A]), (G[A]) or (G[B]) depending
+     * on the FrameVelocityConvention used.
+     * @param[out] centroidalTotalMomentumJacobian the (6) times (6+getNrOfDOFs()) output centroidal
+     * total momentum jacobian.
+     * @return true if all went well, false otherwise.
+     * @note If the chosen FrameVelocityRepresentation is MIXED_REPRESENTATION or
+     * INERTIAL_FIXED_REPRESENTATION, the function computes the Centroidal Momentum Matrix (CMM)
+     * introduced in https://doi.org/10.1109/IROS.2008.4650772 .
+     */
+    bool getCentroidalTotalMomentumJacobian(MatrixDynSize& centroidalTotalMomentumJacobian);
+
     //@}
 
 

src/high-level/src/KinDynComputations.cpp

@@ -1798,6 +1798,74 @@ SpatialMomentum KinDynComputations::getCentroidalTotalMomentum()
     return newOutputFrame_X_oldOutputFrame*base_momentum;
 }
 
+bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centroidalMomentumJacobian)
+{
+    if (!getLinearAngularMomentumJacobian(centroidalMomentumJacobian))
+    {
+        return false;
+    }
+
+    // get CoM and base pose
+    Position com = getCenterOfMassPosition();
+    const Position& basePosition = pimpl->m_pos.worldBasePos().getPosition();
+    const Rotation& A_R_B = pimpl->m_pos.worldBasePos().getRotation();
+
+    switch (pimpl->m_frameVelRepr)
+    {
+    case BODY_FIXED_REPRESENTATION:
+    {
+        // The getLinearAngularMomentumJacobian returns a quantity expressed in (B). Here we want to
+        // express the quantity in (G[B]).
+        // J_G[B] = G[B]_X_B * J_B
+        // where is G[B]_X_B is the adjoint wrench transformation
+
+        Transform com_T_base_in_base(Rotation::Identity(), A_R_B.inverse() * (basePosition - com));
+
+        // The eval() solves the Eigen aliasing problem
+        toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_base.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval();
+
+        break;
+    }
+
+    case MIXED_REPRESENTATION:
+    {
+        // The getLinearAngularMomentumJacobian returns a quantity expressed in (B[A]). Here we want
+        // to express the quantity in (G[A]).
+        // J_G[A] = G[A]_X_B[A] * J_B[A]
+        // where is G[A]_X_B[A] is the adjoint wrench transformation
+
+        Transform com_T_base_in_inertial(Rotation::Identity(), basePosition - com);
+
+        // The eval() solves the Eigen aliasing problem
+        toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval();
+
+        break;
+    }
+
+    case INERTIAL_FIXED_REPRESENTATION:
+    {
+        // The getLinearAngularMomentumJacobian returns a quantity expressed in (A). Here we want
+        // to express the quantity in (G[A]).
+        // J_G[A] = G[A]_X_A * J_A
+        // where is G[A]_X_A is the adjoint wrench transformation
+        assert(pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION);
+
+        iDynTree::Transform com_T_inertial(Rotation::Identity(),  -com);
+
+        // The eval() solves the Eigen aliasing problem
+        toEigen(centroidalMomentumJacobian) = (toEigen(com_T_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval();
+
+        break;
+    }
+
+    default:
+        assert(false);
+        return false;
+    }
+
+    return true;
+}
+
 bool KinDynComputations::getFreeFloatingMassMatrix(MatrixDynSize& freeFloatingMassMatrix)
 {
     // Compute the body-fixed-body-fixed mass matrix, if necessary

src/high-level/tests/KinDynComputationsUnitTest.cpp

@@ -118,12 +118,13 @@ void testRelativeTransform(iDynTree::KinDynComputations & dynComp)
 void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations & dynComp)
 {
     iDynTree::Twist avgVel;
-    iDynTree::SpatialMomentum mom;
-    iDynTree::Vector6 avgVelCheck, momCheck;
+    iDynTree::SpatialMomentum mom, centroidalMom;
+    iDynTree::Vector6 avgVelCheck, momCheck, centroidalMomCheck;
     iDynTree::VectorDynSize nu(dynComp.getNrOfDegreesOfFreedom()+6);
     dynComp.getModelVel(nu);
 
     MomentumFreeFloatingJacobian momJac(dynComp.getRobotModel());
+    MomentumFreeFloatingJacobian centroidalMomJac(dynComp.getRobotModel());
     FrameFreeFloatingJacobian    avgVelJac(dynComp.getRobotModel());
 
     avgVel = dynComp.getAverageVelocity();
@@ -136,10 +137,16 @@ void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations &
 
     ASSERT_IS_TRUE(ok);
 
+    centroidalMom = dynComp.getCentroidalTotalMomentum();
+    ok = dynComp.getCentroidalTotalMomentumJacobian(centroidalMomJac);
+    ASSERT_IS_TRUE(ok);
+
     toEigen(momCheck) = toEigen(momJac)*toEigen(nu);
+    toEigen(centroidalMomCheck) = toEigen(centroidalMomJac)*toEigen(nu);
     toEigen(avgVelCheck) = toEigen(avgVelJac)*toEigen(nu);
 
     ASSERT_EQUAL_VECTOR(momCheck,mom.asVector());
+    ASSERT_EQUAL_VECTOR(centroidalMomCheck,centroidalMom.asVector());
     ASSERT_EQUAL_VECTOR(avgVelCheck,avgVel.asVector());
 }