Fix Coriolis Matrix Algorithm for any floating base choice

src/high-level/src/KinDynComputations.cpp

@@ -2633,13 +2633,26 @@ void KinDynComputations::KinDynComputationsPrivateAttributes::
     // with (b_v_a,b)x being the 6x6 matrix associated to the cross product with the spatial
     // velocity of the base expressed in body-fixed coordinates.
 
-    Transform A_T_B;
-    Twist vel_for_derivative;
+    Transform A_T_B; // newFrame_H_B
+    Twist vel_for_derivative; // B_v_newFrame,B
 
     if (m_frameVelRepr == BODY_FIXED_REPRESENTATION)
     {
-        // In body-fixed representation nothing to do
-        return;
+        if (m_isFloatingBaseFrame)
+        {
+            // body-fixed with an additional frame of the link as floating base
+            // The frame is fixed to the link, so in the body-fixed frame of the link,
+            // it has no relative velocity
+            A_T_B = m_baseLinkToBaseFrame.inverse();
+            Vector3 zero_linvel, zero_ang_vel;
+            zero_linvel.zero();
+            zero_ang_vel.zero();
+            vel_for_derivative = Twist(zero_linvel, zero_ang_vel);
+
+        } else {
+            // body-fixed with base link frame as floating base - nothing to do
+            return;
+        }
     } else if (m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION)
     {
         A_T_B = m_pos.worldBasePos();
@@ -3391,8 +3404,8 @@ bool KinDynComputations::getCoriolisAndMassMatrices(
     // As stated by Theorem 3.3 in S. Traversaro "Modelling, Estimation and Identification of
     // Humanoid Robots Dynamics" PhD thesis, the property M_dot - 2C is skew symmetric also holds
     // for different velocity representations. So we can compute M_dot as C + C^T.
-    if (pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION
-        || pimpl->m_frameVelRepr == MIXED_REPRESENTATION)
+    if (pimpl->m_frameVelRepr != BODY_FIXED_REPRESENTATION
+        || pimpl->m_isFloatingBaseFrame)
     {
         toEigen(freeFloatingMassMatrixDerivative)
             = toEigen(freeFloatingCoriolisMatrix) + toEigen(freeFloatingCoriolisMatrix).transpose();
@@ -3537,12 +3550,26 @@ bool KinDynComputations::inverseDynamics(const Vector6& baseFrameAcc,
                      pimpl->m_invDynInternalWrenches,
                      baseForceAndJointTorques);
 
-    // Convert output base force
+    // The base wrench is currently expressed in body-fixed representation of the base link frame
+    // Need to convert it to the used representation and, if needed, to the floating base frame
+    iDynTree::Transform world_X_baseFrame;
+
+    if (this->pimpl->m_isFloatingBaseFrame)
+    {
+        // Floating base is an additional frame - need to convert from base link frame
+        iDynTree::Transform world_X_baseLink
+            = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+        world_X_baseFrame = world_X_baseLink * pimpl->m_baseLinkToBaseFrame;
+        baseForceAndJointTorques.baseWrench()
+            = pimpl->m_baseLinkToBaseFrame.inverse() * baseForceAndJointTorques.baseWrench();
+    } else
+    {
+        world_X_baseFrame = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+    }
+    // Convert base wrench to the used representation
     baseForceAndJointTorques.baseWrench()
         = pimpl->fromBodyFixedToUsedRepresentation(baseForceAndJointTorques.baseWrench(),
-                                                   pimpl->m_linkPos(
-                                                       pimpl->m_traversal.getBaseLink()
-                                                           ->getIndex()));
+                                                   world_X_baseFrame);
 
     return true;
 }
@@ -3684,12 +3711,26 @@ bool KinDynComputations::generalizedBiasForces(FreeFloatingGeneralizedTorques& g
                      pimpl->m_invDynInternalWrenches,
                      generalizedBiasForces);
 
-    // Convert output base force
+    // The base wrench is currently expressed in body-fixed representation of the base link frame
+    // Need to convert it to the used representation and, if needed, to the floating base frame
+    iDynTree::Transform world_X_baseFrame;
+
+    if (this->pimpl->m_isFloatingBaseFrame)
+    {
+        // Floating base is an additional frame - need to convert from base link frame
+        iDynTree::Transform world_X_baseLink
+            = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+        world_X_baseFrame = world_X_baseLink * pimpl->m_baseLinkToBaseFrame;
+        generalizedBiasForces.baseWrench()
+            = pimpl->m_baseLinkToBaseFrame.inverse() * generalizedBiasForces.baseWrench();
+    } else
+    {
+        world_X_baseFrame = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+    }
+    // Convert base wrench to the used representation
     generalizedBiasForces.baseWrench()
         = pimpl->fromBodyFixedToUsedRepresentation(generalizedBiasForces.baseWrench(),
-                                                   pimpl->m_linkPos(
-                                                       pimpl->m_traversal.getBaseLink()
-                                                           ->getIndex()));
+                                                   world_X_baseFrame);
 
     return true;
 }
@@ -3756,12 +3797,26 @@ bool KinDynComputations::generalizedGravityForces(
                      pimpl->m_invDynInternalWrenches,
                      generalizedGravityForces);
 
-    // Convert output base force
+    // The base wrench is currently expressed in body-fixed representation of the base link frame
+    // Need to convert it to the used representation and, if needed, to the floating base frame
+    iDynTree::Transform world_X_baseFrame;
+
+    if (this->pimpl->m_isFloatingBaseFrame)
+    {
+        // Floating base is an additional frame - need to convert from base link frame
+        iDynTree::Transform world_X_baseLink
+            = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+        world_X_baseFrame = world_X_baseLink * pimpl->m_baseLinkToBaseFrame;
+        generalizedGravityForces.baseWrench()
+            = pimpl->m_baseLinkToBaseFrame.inverse() * generalizedGravityForces.baseWrench();
+    } else
+    {
+        world_X_baseFrame = pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex());
+    }
+    // Convert base wrench to the used representation
     generalizedGravityForces.baseWrench()
         = pimpl->fromBodyFixedToUsedRepresentation(generalizedGravityForces.baseWrench(),
-                                                   pimpl->m_linkPos(
-                                                       pimpl->m_traversal.getBaseLink()
-                                                           ->getIndex()));
+                                                   world_X_baseFrame);
 
     return true;
 }