diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 0f261ecd72..ea4fcfb720 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/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; } diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp index 42e60f16dc..c1b432b60e 100644 --- a/src/high-level/tests/KinDynComputationsUnitTest.cpp +++ b/src/high-level/tests/KinDynComputationsUnitTest.cpp @@ -1185,6 +1185,10 @@ void testFloatingBaseFrameConsistency(std::string modelFilePath, ddq_joint, externalWrenches, baseForceAndJointTorques_with_l_foot_base)); + FreeFloatingGeneralizedTorques biasForces_with_l_foot_base(dynComp.model()); + ASSERT_IS_TRUE(dynComp.generalizedBiasForces(biasForces_with_l_foot_base)); + FreeFloatingGeneralizedTorques gravityForces_with_l_foot_base(dynComp.model()); + ASSERT_IS_TRUE(dynComp.generalizedGravityForces(gravityForces_with_l_foot_base)); // Now set floating base to l_sole ok = dynComp.setFloatingBase(baseFrameName); @@ -1217,6 +1221,10 @@ void testFloatingBaseFrameConsistency(std::string modelFilePath, ddq_joint, externalWrenches, baseForceAndJointTorques_with_l_sole_base)); + FreeFloatingGeneralizedTorques biasForces_with_l_sole_base(dynComp.model()); + ASSERT_IS_TRUE(dynComp.generalizedBiasForces(biasForces_with_l_sole_base)); + FreeFloatingGeneralizedTorques gravityForces_with_l_sole_base(dynComp.model()); + ASSERT_IS_TRUE(dynComp.generalizedGravityForces(gravityForces_with_l_sole_base)); // Test that computed quantities are consistent regardless of floating base choice ASSERT_EQUAL_TRANSFORM(world_H_l_sole_with_l_foot_base, world_H_l_sole_with_l_sole_base); @@ -1233,8 +1241,31 @@ void testFloatingBaseFrameConsistency(std::string modelFilePath, rootLinkAcceleration_with_l_sole_base); ASSERT_EQUAL_VECTOR(baseForceAndJointTorques_with_l_foot_base.jointTorques(), baseForceAndJointTorques_with_l_sole_base.jointTorques()); + ASSERT_EQUAL_VECTOR(biasForces_with_l_foot_base.jointTorques(), + biasForces_with_l_sole_base.jointTorques()); + ASSERT_EQUAL_VECTOR(gravityForces_with_l_foot_base.jointTorques(), + gravityForces_with_l_sole_base.jointTorques()); + iDynTree::Transform toFrame_H_fromFrame; + if (dynComp.getFrameVelocityRepresentation() == iDynTree::BODY_FIXED_REPRESENTATION) + { + toFrame_H_fromFrame = l_foot_H_l_sole; + } else if (dynComp.getFrameVelocityRepresentation() == iDynTree::INERTIAL_FIXED_REPRESENTATION) + { + toFrame_H_fromFrame = Transform::Identity(); + } else if (dynComp.getFrameVelocityRepresentation() == iDynTree::MIXED_REPRESENTATION) + { + // l_foot[W]_H_l_sole[W] + toFrame_H_fromFrame = Transform(Rotation::Identity(), + world_H_l_sole_with_l_sole_base.getPosition() + - world_H_l_foot_with_l_foot_base.getPosition()); + } ASSERT_EQUAL_VECTOR(baseForceAndJointTorques_with_l_foot_base.baseWrench(), - baseForceAndJointTorques_with_l_sole_base.baseWrench()); + toFrame_H_fromFrame + * baseForceAndJointTorques_with_l_sole_base.baseWrench()); + ASSERT_EQUAL_VECTOR(biasForces_with_l_foot_base.baseWrench(), + toFrame_H_fromFrame * biasForces_with_l_sole_base.baseWrench()); + ASSERT_EQUAL_VECTOR(gravityForces_with_l_foot_base.baseWrench(), + toFrame_H_fromFrame * gravityForces_with_l_sole_base.baseWrench()); // recompute some quantities with getRobotState dynComp.getRobotState(world_H_l_sole_with_l_sole_base, @@ -1397,6 +1428,205 @@ void testCoriolisAndMassMatricesOnRandomModels() } } +void testCoriolisMatrixAlgorithmOnFloatingBaseModel(std::string modelFilePath, + const FrameVelocityRepresentation frameVelRepr) +{ + // It tests getCoriolisAndMassMatrices on a floating base model with + // different frames used as floating base. + + iDynTree::KinDynComputations dynComp; + iDynTree::ModelLoader mdlLoader; + bool ok = mdlLoader.loadModelFromFile(modelFilePath); + ok = ok && dynComp.loadRobotModel(mdlLoader.model()); + ASSERT_IS_TRUE(ok); + + ok = dynComp.setFrameVelocityRepresentation(frameVelRepr); + ASSERT_IS_TRUE(ok); + + const int nrOfDofs = dynComp.getNrOfDegreesOfFreedom(); + + // Test with iCub model using l_foot as base link vs l_sole as base frame + std::string baseLinkName = "l_foot"; + std::string baseFrameName = "l_sole"; + + // Check if both frames exist in the model + int baseLinkIndex = dynComp.model().getFrameIndex(baseLinkName); + int baseFrameIndex = dynComp.model().getFrameIndex(baseFrameName); + + if (baseLinkIndex < 0 || baseFrameIndex < 0) + { + // if frames are not present, test should fail + ASSERT_IS_TRUE(false); + } + + // Set floating base to l_foot and create a state with non-zero velocities + ok = dynComp.setFloatingBase(baseLinkName); + ASSERT_IS_TRUE(ok); + + // Create a non-trivial state with non-zero velocities for both base and joints + Transform world_H_l_foot = getRandomTransform(); + VectorDynSize q_joint(dynComp.model().getNrOfPosCoords()); + getRandomJointPositions(q_joint, dynComp.model()); + + // Generate non-zero base velocity + Twist base_vel_l_foot = getRandomTwist(); + + // Set random joint velocities + VectorDynSize qdot_joint(dynComp.model().getNrOfDOFs()); + getRandomVector(qdot_joint); + + // Gravity + Vector3 gravity; + gravity(0) = 0.0; + gravity(1) = 0.0; + gravity(2) = -9.81; + + // Set state with l_foot as floating base + ok = dynComp.setRobotState(world_H_l_foot, q_joint, base_vel_l_foot, qdot_joint, gravity); + ASSERT_IS_TRUE(ok); + + VectorDynSize nu_l_foot(6 + nrOfDofs); + ok = dynComp.getModelVel(nu_l_foot); + ASSERT_IS_TRUE(ok); + + // Get coriolis matrix, mass matrix, and mass matrix derivative with l_foot as base + MatrixDynSize coriolisMatrix_l_foot(6 + nrOfDofs, 6 + nrOfDofs); + MatrixDynSize massMatrix_l_foot(6 + nrOfDofs, 6 + nrOfDofs); + MatrixDynSize massMatrixDerivative_l_foot(6 + nrOfDofs, 6 + nrOfDofs); + ok = dynComp.getCoriolisAndMassMatrices(coriolisMatrix_l_foot, + massMatrix_l_foot, + massMatrixDerivative_l_foot); + ASSERT_IS_TRUE(ok); + + // Check mass Mdot = C + C^T + ASSERT_EQUAL_MATRIX_TOL(toEigen(massMatrixDerivative_l_foot), + toEigen(coriolisMatrix_l_foot) + + toEigen(coriolisMatrix_l_foot).transpose(), + 1e-8); + + // Check mass matrices consistency + MatrixDynSize massMatrix_l_foot_reference(6 + nrOfDofs, 6 + nrOfDofs); + ok = dynComp.getFreeFloatingMassMatrix(massMatrix_l_foot_reference); + ASSERT_IS_TRUE(ok); + ASSERT_EQUAL_MATRIX_TOL(toEigen(massMatrix_l_foot), toEigen(massMatrix_l_foot_reference), 1e-8); + + // Check C_nu = bias_forces - gravity_forces + FreeFloatingGeneralizedTorques biasForces_l_foot(dynComp.model()); + ok = dynComp.generalizedBiasForces(biasForces_l_foot); + ASSERT_IS_TRUE(ok); + FreeFloatingGeneralizedTorques gravityForces_l_foot(dynComp.model()); + ok = ok && dynComp.generalizedGravityForces(gravityForces_l_foot); + ASSERT_IS_TRUE(ok); + + VectorDynSize C_nu_l_foot(6 + nrOfDofs); + toEigen(C_nu_l_foot) = toEigen(coriolisMatrix_l_foot) * toEigen(nu_l_foot); + + ASSERT_EQUAL_VECTOR_TOL(toEigen(C_nu_l_foot).tail(nrOfDofs), + toEigen(biasForces_l_foot.jointTorques()) + - toEigen(gravityForces_l_foot.jointTorques()), + 1e-8); + + ASSERT_EQUAL_VECTOR_TOL(toEigen(C_nu_l_foot).head(6), + toEigen(biasForces_l_foot.baseWrench()) + - toEigen(gravityForces_l_foot.baseWrench()), + 1e-8); + + // Now set floating base to l_sole + ok = dynComp.setFloatingBase(baseFrameName); + ASSERT_IS_TRUE(ok); + + // Update state with l_sole as floating base + Transform world_H_l_sole = dynComp.getWorldTransform(baseFrameName); + Twist base_vel_l_sole = dynComp.getFrameVel(baseFrameName); + ok = dynComp.setRobotState(world_H_l_sole, q_joint, base_vel_l_sole, qdot_joint, gravity); + ASSERT_IS_TRUE(ok); + + VectorDynSize nu_l_sole(6 + nrOfDofs); + ok = dynComp.getModelVel(nu_l_sole); + ASSERT_IS_TRUE(ok); + + // Get coriolis matrix, mass matrix, and mass matrix derivative with l_sole as base + MatrixDynSize coriolisMatrix_l_sole(6 + nrOfDofs, 6 + nrOfDofs); + MatrixDynSize massMatrix_l_sole(6 + nrOfDofs, 6 + nrOfDofs); + MatrixDynSize massMatrixDerivative_l_sole(6 + nrOfDofs, 6 + nrOfDofs); + ok = dynComp.getCoriolisAndMassMatrices(coriolisMatrix_l_sole, + massMatrix_l_sole, + massMatrixDerivative_l_sole); + ASSERT_IS_TRUE(ok); + + // Check mass Mdot = C + C^T + ASSERT_EQUAL_MATRIX_TOL(toEigen(massMatrixDerivative_l_sole), + toEigen(coriolisMatrix_l_sole) + + toEigen(coriolisMatrix_l_sole).transpose(), + 1e-8); + + // Check mass matrices consistency + MatrixDynSize massMatrix_l_sole_reference(6 + nrOfDofs, 6 + nrOfDofs); + ok = dynComp.getFreeFloatingMassMatrix(massMatrix_l_sole_reference); + ASSERT_IS_TRUE(ok); + ASSERT_EQUAL_MATRIX_TOL(toEigen(massMatrix_l_sole), toEigen(massMatrix_l_sole_reference), 1e-8); + + // Check C_nu = bias_forces - gravity_forces + FreeFloatingGeneralizedTorques biasForces_l_sole(dynComp.model()); + ok = dynComp.generalizedBiasForces(biasForces_l_sole); + ASSERT_IS_TRUE(ok); + FreeFloatingGeneralizedTorques gravityForces_l_sole(dynComp.model()); + ok = ok && dynComp.generalizedGravityForces(gravityForces_l_sole); + ASSERT_IS_TRUE(ok); + + VectorDynSize C_nu_l_sole(6 + nrOfDofs); + toEigen(C_nu_l_sole) = toEigen(coriolisMatrix_l_sole) * toEigen(nu_l_sole); + + ASSERT_EQUAL_VECTOR_TOL(toEigen(C_nu_l_sole).tail(nrOfDofs), + toEigen(biasForces_l_sole.jointTorques()) + - toEigen(gravityForces_l_sole.jointTorques()), + 1e-8); + + ASSERT_EQUAL_VECTOR_TOL(toEigen(C_nu_l_sole).head(6), + toEigen(biasForces_l_sole.baseWrench()) + - toEigen(gravityForces_l_sole.baseWrench()), + 1e-8); + + // check kinetic energy consistency + double kineticEnergy_l_foot + = 0.5 * toEigen(nu_l_foot).transpose() * toEigen(massMatrix_l_foot) * toEigen(nu_l_foot); + double kineticEnergy_l_sole + = 0.5 * toEigen(nu_l_sole).transpose() * toEigen(massMatrix_l_sole) * toEigen(nu_l_sole); + + ASSERT_EQUAL_DOUBLE_TOL(kineticEnergy_l_foot, kineticEnergy_l_sole, 1e-9); +} + +void testCoriolisMatrixAlgorithmOnFloatingBaseModelAllRepresentations(std::string modelFilePath) +{ + + for (auto repr : + {BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION, INERTIAL_FIXED_REPRESENTATION}) + { + + std::string reprStr; + switch (repr) + { + case BODY_FIXED_REPRESENTATION: + reprStr = "BODY_FIXED_REPRESENTATION"; + break; + case INERTIAL_FIXED_REPRESENTATION: + reprStr = "INERTIAL_FIXED_REPRESENTATION"; + break; + case MIXED_REPRESENTATION: + reprStr = "MIXED_REPRESENTATION"; + break; + default: + reprStr = "UNKNOWN_REPRESENTATION"; + break; + } + + std::cout << "Testing getCoriolisAndMassMatrices using different floating bases for " + "representation " + << reprStr << " on model " << modelFilePath << std::endl; + testCoriolisMatrixAlgorithmOnFloatingBaseModel(modelFilePath, repr); + } +} + int main() { // Just run the tests on a handful of models to avoid @@ -1449,5 +1679,9 @@ int main() // Test getCoriolisAndMassMatrices on random models testCoriolisAndMassMatricesOnRandomModels(); + // Test getCoriolisAndMassMatrices on a Floating Base Model using different floating bases + testCoriolisMatrixAlgorithmOnFloatingBaseModelAllRepresentations(getAbsModelPath("iCubGenova02." + "urdf")); + return EXIT_SUCCESS; } diff --git a/src/model/src/Dynamics.cpp b/src/model/src/Dynamics.cpp index ec46897a4f..f3f780fe48 100644 --- a/src/model/src/Dynamics.cpp +++ b/src/model/src/Dynamics.cpp @@ -344,6 +344,8 @@ bool CoriolisMatrixAlgorithm(const Model& model, massMatrixDerivative.rows(), massMatrixDerivative.cols()); + LinkIndex baseLinkIndex = traversal.getBaseLink()->getIndex(); + // Forward pass to initialize the CRBI for (int i = 0; i < traversal.getNrOfVisitedLinks(); i++) { @@ -366,7 +368,6 @@ bool CoriolisMatrixAlgorithm(const Model& model, // get link and parent link LinkConstPtr visitedLink = traversal.getLink(traversalEl); LinkIndex visitedLinkIndex = visitedLink->getIndex(); - LinkConstPtr parentLink = traversal.getParentLinkFromLinkIndex(visitedLinkIndex); // get joint between parent and child IJointConstPtr toParentJoint = traversal.getParentJointFromLinkIndex(visitedLinkIndex); // get link velocity @@ -374,7 +375,7 @@ bool CoriolisMatrixAlgorithm(const Model& model, // get link inertia SpatialInertia inertia = linkCRBIs(visitedLinkIndex); - if (parentLink) + if (visitedLinkIndex != baseLinkIndex) { // the visited link is NOT the base link if (toParentJoint->getNrOfDOFs() == 0) @@ -428,7 +429,7 @@ bool CoriolisMatrixAlgorithm(const Model& model, LinkIndex visitedLinkIndex = visitedLink->getIndex(); LinkConstPtr parentLink = traversal.getParentLinkFromLinkIndex(visitedLinkIndex); - if (visitedLinkIndex) + if (visitedLinkIndex != baseLinkIndex) { // the visited link is NOT the base link IJointConstPtr toParentJoint = traversal.getParentJointFromLinkIndex(visitedLinkIndex); @@ -629,7 +630,7 @@ bool CoriolisMatrixAlgorithm(const Model& model, LinkConstPtr ancestorParentLink = traversal.getParentLinkFromLinkIndex(ancestorLinkIndex); LinkIndex ancestorParentIndex = ancestorParentLink->getIndex(); - assert(ancestorParentIndex == 0); // base link + assert(ancestorParentIndex == baseLinkIndex); Transform otherLink_X_parentLink = linkPos(ancestorLinkIndex).inverse() * linkPos(ancestorParentIndex);