From acaadfacd030d991e715b235e8a1b5685ae1c8ac Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 27 Jun 2020 15:23:11 +0200 Subject: [PATCH 1/7] Implement the KinDynComputations::getCentroidalTotalMomentumJacobian() method --- .../include/iDynTree/KinDynComputations.h | 13 ++++ src/high-level/src/KinDynComputations.cpp | 59 +++++++++++++++++++ 2 files changed, 72 insertions(+) diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index d532acd61ec..990f82b6aaa 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/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); + //@} diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index dd703eb55da..8d43d9bfb32 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -1798,6 +1798,65 @@ SpatialMomentum KinDynComputations::getCentroidalTotalMomentum() return newOutputFrame_X_oldOutputFrame*base_momentum; } +bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centroidalMomentumJacobian) +{ + if (!getLinearAngularMomentumJacobian(centroidalMomentumJacobian)) + return false; + + // Get the base link index + LinkIndex baseLink = this->pimpl->m_traversal.getBaseLink()->getIndex(); + + Position com = getCenterOfMassPosition(); + const Position& basePosition = getWorldTransform(baseLink).getPosition(); + const Rotation& A_R_B = getWorldTransform(baseLink).getRotation(); + + if (pimpl->m_frameVelRepr == 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)); + + toEigen(centroidalMomentumJacobian) = toEigen(com_T_base_in_base.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + + return true; + } + else if(pimpl->m_frameVelRepr == 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); + + toEigen(centroidalMomentumJacobian) = toEigen(com_T_base_in_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + + return true; + } + else + { + // 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); + + toEigen(centroidalMomentumJacobian) = toEigen(com_T_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + + return true; + } + + assert(false); + + return false; +} + bool KinDynComputations::getFreeFloatingMassMatrix(MatrixDynSize& freeFloatingMassMatrix) { // Compute the body-fixed-body-fixed mass matrix, if necessary From b4a1243759063daa6a3e28149fb2f91f9044684e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 27 Jun 2020 15:23:49 +0200 Subject: [PATCH 2/7] Implement a simple test for the centroidal momentum jacobian --- src/high-level/tests/KinDynComputationsUnitTest.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp index c527e03358b..710a15fce93 100644 --- a/src/high-level/tests/KinDynComputationsUnitTest.cpp +++ b/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()); } From bc0fe512d6ed1590d1ac4a65c149c7602ab57c41 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 27 Jun 2020 15:55:40 +0200 Subject: [PATCH 3/7] Attempt to solve macOS KinDynComputations test failure --- src/high-level/src/KinDynComputations.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 8d43d9bfb32..5c1e4e6d311 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -1817,10 +1817,10 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr // 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)); - toEigen(centroidalMomentumJacobian) = toEigen(com_T_base_in_base.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + // The eval() solves the Eigen aliasing problem + toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_base.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); return true; } @@ -1833,7 +1833,8 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr Transform com_T_base_in_inertial(Rotation::Identity(), basePosition - com); - toEigen(centroidalMomentumJacobian) = toEigen(com_T_base_in_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + // The eval() solves the Eigen aliasing problem + toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); return true; } @@ -1847,7 +1848,8 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr iDynTree::Transform com_T_inertial(Rotation::Identity(), -com); - toEigen(centroidalMomentumJacobian) = toEigen(com_T_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian); + // The eval() solves the Eigen aliasing problem + toEigen(centroidalMomentumJacobian) = (toEigen(com_T_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); return true; } From f9df8a0ce7e8679c1eb847e58297c08efbd23e7b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Sat, 27 Jun 2020 22:53:42 +0200 Subject: [PATCH 4/7] Avoid to use getWorldTransform() function in KinDynComputations::getCentroidalTotalMomentumJacobian --- src/high-level/src/KinDynComputations.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 5c1e4e6d311..54526d08fd7 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -1803,12 +1803,10 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr if (!getLinearAngularMomentumJacobian(centroidalMomentumJacobian)) return false; - // Get the base link index - LinkIndex baseLink = this->pimpl->m_traversal.getBaseLink()->getIndex(); - + // get CoM and base pose Position com = getCenterOfMassPosition(); - const Position& basePosition = getWorldTransform(baseLink).getPosition(); - const Rotation& A_R_B = getWorldTransform(baseLink).getRotation(); + const Position& basePosition = pimpl->m_pos.worldBasePos().getPosition(); + const Rotation& A_R_B = pimpl->m_pos.worldBasePos().getRotation(); if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) { From f8bab02af15e80275f4f2c9bad59e6f1d766a458 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Jun 2020 09:50:17 +0200 Subject: [PATCH 5/7] Apply @traversaro suggestions --- src/high-level/src/KinDynComputations.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 54526d08fd7..8e5c159e474 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -1801,7 +1801,9 @@ SpatialMomentum KinDynComputations::getCentroidalTotalMomentum() bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centroidalMomentumJacobian) { if (!getLinearAngularMomentumJacobian(centroidalMomentumJacobian)) + { return false; + } // get CoM and base pose Position com = getCenterOfMassPosition(); From 3feae00a6cd2fc3d7a71018f86a56fca44cf1c62 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Jun 2020 09:53:20 +0200 Subject: [PATCH 6/7] Apply @diegoferigo suggestions --- src/high-level/src/KinDynComputations.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 8e5c159e474..c714274a31b 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -1810,7 +1810,9 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr const Position& basePosition = pimpl->m_pos.worldBasePos().getPosition(); const Rotation& A_R_B = pimpl->m_pos.worldBasePos().getRotation(); - if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) + 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]). @@ -1822,9 +1824,10 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr // The eval() solves the Eigen aliasing problem toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_base.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); - return true; + break; } - else if(pimpl->m_frameVelRepr == MIXED_REPRESENTATION) + + case MIXED_REPRESENTATION: { // The getLinearAngularMomentumJacobian returns a quantity expressed in (B[A]). Here we want // to express the quantity in (G[A]). @@ -1836,9 +1839,10 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr // The eval() solves the Eigen aliasing problem toEigen(centroidalMomentumJacobian) = (toEigen(com_T_base_in_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); - return true; + break; } - else + + case INERTIAL_FIXED_REPRESENTATION: { // The getLinearAngularMomentumJacobian returns a quantity expressed in (A). Here we want // to express the quantity in (G[A]). @@ -1851,12 +1855,15 @@ bool KinDynComputations::getCentroidalTotalMomentumJacobian(MatrixDynSize& centr // The eval() solves the Eigen aliasing problem toEigen(centroidalMomentumJacobian) = (toEigen(com_T_inertial.asAdjointTransformWrench()) * toEigen(centroidalMomentumJacobian)).eval(); - return true; + break; } - assert(false); + default: + assert(false); + return false; + } - return false; + return true; } bool KinDynComputations::getFreeFloatingMassMatrix(MatrixDynSize& freeFloatingMassMatrix) From 3a9c0e33666fa138ebc3d02fb59bdab6362eff35 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 29 Jun 2020 10:11:35 +0200 Subject: [PATCH 7/7] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 771fa4ea3dc..2ca46548cce 100644 --- a/CHANGELOG.md +++ b/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 .