Merge pull request #1153 from GiulioRomualdi/locked_inertia

Implement KinDynComputations::getCentroidalRobotLockedInertia and KinDynComputations::getRobotLockedInertia

CHANGELOG.md

@@ -9,6 +9,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+## [10.3.0] - 2024-02-07
+
+### Added
+
+- Added getCentroidalRobotLockedInertia and getRobotLockedInertia to KinDynComputations class (https://github.com/robotology/idyntree/pull/1153).
+
 ## [10.2.1] - 2024-02-01
 
 ### Fixed

CMakeLists.txt

@@ -3,7 +3,7 @@
 
 cmake_minimum_required(VERSION 3.16)
 
-project(iDynTree VERSION 10.2.1
+project(iDynTree VERSION 10.3.0
                  LANGUAGES C CXX)
 
 # Disable in source build, unless Eclipse is used

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

@@ -916,6 +916,15 @@ class KinDynComputations {
      */
     bool getAverageVelocityJacobian(iDynTree::MatrixView<double> avgVelocityJacobian);
 
+    /**
+     * Get the robot locked inertia matrix.
+     * The quantity is expressed in (B[A]), (A) or (B) depending on the FrameVelocityConvention used.
+     *
+     * @return the locked inertia matrix of the robot.
+     *
+     */
+    SpatialInertia getRobotLockedInertia();
+
     /**
      * Get the centroidal average velocity of the robot.
      *
@@ -960,6 +969,15 @@ class KinDynComputations {
      */
     bool getCentroidalAverageVelocityJacobian(iDynTree::MatrixView<double> centroidalAvgVelocityJacobian);
 
+    /**
+     * Get the robot locked centroidal inertia matrix.
+     * The quantity is expressed in (G[A]) or (G[B]) depending on the FrameVelocityConvention used.
+     *
+     * @return the locked inertia centroidal matrix of the robot.
+     *
+     */
+    SpatialInertia getCentroidalRobotLockedInertia();
+
     /**
      * Get the linear and angular momentum of the robot.
      * The quantity is expressed in (B[A]), (A) or (B) depending on the FrameVelocityConvention used.

src/high-level/src/KinDynComputations.cpp

@@ -2199,6 +2199,29 @@ bool KinDynComputations::getAverageVelocityJacobian(MatrixView<double> avgVeloci
     return true;
 }
 
+SpatialInertia KinDynComputations::getRobotLockedInertia()
+{
+    this->computeRawMassMatrixAndTotalMomentum();
+    const SpatialInertia & base_lockedInertia = pimpl->getRobotLockedInertia();
+
+    if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION)
+    {
+        // the computeRawMassMatrixAndTotalMomentum generates the lockedInertia in body frame.
+        // So in this case there is no need to conversion
+        return base_lockedInertia;
+    }
+    else if (pimpl->m_frameVelRepr == MIXED_REPRESENTATION)
+    {
+        iDynTree::Transform B_A_H_B(pimpl->m_pos.worldBasePos().getRotation(),  Position::Zero());
+
+        // the following is not a classical multiplication. Indeed it transform the spatial inertia in a different frame.
+        return B_A_H_B * base_lockedInertia;
+    }
+
+    assert(pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION);
+    return pimpl->m_pos.worldBasePos()*base_lockedInertia;
+}
+
 void KinDynComputations::KinDynComputationsPrivateAttributes::processOnLeftSideBodyFixedCentroidalAvgVelocityJacobian(
     MatrixView<double> jac, const FrameVelocityRepresentation & leftSideRepresentation)
 {
@@ -2262,6 +2285,37 @@ Twist KinDynComputations::getCentroidalAverageVelocity()
     return newOutputFrame_X_oldOutputFrame*base_averageVelocity;
 }
 
+SpatialInertia KinDynComputations::getCentroidalRobotLockedInertia()
+{
+    this->computeRawMassMatrixAndTotalMomentum();
+
+
+    const SpatialInertia & base_lockedInertia = pimpl->getRobotLockedInertia();
+
+    // Get the center of mass in the base frame
+    const Position vectorFromComToBaseWithRotationOfBase = Position::inverse(pimpl->getRobotLockedInertia().getCenterOfMass());
+
+    Transform newOutputFrame_X_oldOutputFrame;
+    if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION)
+    {
+        // oldOutputFrame is B
+        // newOutputFrame is G[B]
+
+        newOutputFrame_X_oldOutputFrame =  Transform(Rotation::Identity(),vectorFromComToBaseWithRotationOfBase);
+    }
+    else
+    {
+        assert(pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ||
+               pimpl->m_frameVelRepr == MIXED_REPRESENTATION);
+        // oldOutputFrame is B
+        // newOutputFrame is G[A]
+        const Rotation & A_R_B = pimpl->m_pos.worldBasePos().getRotation();
+        newOutputFrame_X_oldOutputFrame = Transform(pimpl->m_pos.worldBasePos().getRotation(),A_R_B*(vectorFromComToBaseWithRotationOfBase));
+    }
+
+    return newOutputFrame_X_oldOutputFrame*base_lockedInertia;
+}
+
 bool KinDynComputations::getCentroidalAverageVelocity(Span<double> vel)
 {
     constexpr int expected_spatial_velocity_size = 6;

src/high-level/tests/KinDynComputationsUnitTest.cpp

@@ -113,15 +113,19 @@ void testRelativeTransform(iDynTree::KinDynComputations & dynComp)
 
 void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations & dynComp)
 {
-    iDynTree::Twist avgVel;
-    iDynTree::SpatialMomentum mom, centroidalMom;
-    iDynTree::Vector6 avgVelCheck, momCheck, centroidalMomCheck;
+    iDynTree::Twist avgVel, centroidalAvgVel;
+    iDynTree::SpatialMomentum mom, centroidalMom, computedMom, computedCentroidalMom;
+    iDynTree::Vector6 avgVelCheck, centroidalAvgVelCheck, momCheck, centroidalMomCheck;
     iDynTree::VectorDynSize nu(dynComp.getNrOfDegreesOfFreedom()+6);
+    iDynTree::SpatialInertia lockedInertia, centroidalLockedInertia;
     dynComp.getModelVel(nu);
 
     MomentumFreeFloatingJacobian momJac(dynComp.getRobotModel());
     MomentumFreeFloatingJacobian centroidalMomJac(dynComp.getRobotModel());
     FrameFreeFloatingJacobian    avgVelJac(dynComp.getRobotModel());
+    FrameFreeFloatingJacobian    centroidalAvgVelJac(dynComp.getRobotModel());
+
+    // momentum quantities
 
     avgVel = dynComp.getAverageVelocity();
     bool ok = dynComp.getAverageVelocityJacobian(avgVelJac);
@@ -133,17 +137,36 @@ void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations &
 
     ASSERT_IS_TRUE(ok);
 
+    lockedInertia = dynComp.getRobotLockedInertia();
+    computedMom = lockedInertia * avgVel;
+
+    // centroidal quantities
+
+    centroidalAvgVel = dynComp.getCentroidalAverageVelocity();
+    ok = dynComp.getCentroidalAverageVelocityJacobian(centroidalAvgVelJac);
+
+    ASSERT_IS_TRUE(ok);
+
     centroidalMom = dynComp.getCentroidalTotalMomentum();
     ok = dynComp.getCentroidalTotalMomentumJacobian(centroidalMomJac);
     ASSERT_IS_TRUE(ok);
 
+    centroidalLockedInertia = dynComp.getCentroidalRobotLockedInertia();
+    computedCentroidalMom = centroidalLockedInertia * centroidalAvgVel;
+
     toEigen(momCheck) = toEigen(momJac)*toEigen(nu);
     toEigen(centroidalMomCheck) = toEigen(centroidalMomJac)*toEigen(nu);
     toEigen(avgVelCheck) = toEigen(avgVelJac)*toEigen(nu);
+    toEigen(centroidalAvgVelCheck) = toEigen(centroidalAvgVelJac)*toEigen(nu);
 
     ASSERT_EQUAL_VECTOR(momCheck,mom.asVector());
     ASSERT_EQUAL_VECTOR(centroidalMomCheck,centroidalMom.asVector());
+
     ASSERT_EQUAL_VECTOR(avgVelCheck,avgVel.asVector());
+    ASSERT_EQUAL_VECTOR(centroidalAvgVelCheck, centroidalAvgVel.asVector());
+
+    ASSERT_EQUAL_VECTOR(mom.asVector(), computedMom);
+    ASSERT_EQUAL_VECTOR(centroidalMom.asVector(), computedCentroidalMom);
 }
 
 inline Eigen::VectorXd toEigen(const Vector6 & baseAcc, const VectorDynSize & jntAccs)