From 1e11137e39203bfd23e638dab23a680aa906371c Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 10 Sep 2018 18:43:34 +0200 Subject: [PATCH 1/5] Add inertial parameters regressors method to KinDynComputations In particular, add the inverseDynamicsInertialParametersRegressor that computes the (6+nrOfDofs) \times (10*nrOfLinks) matrix that maps the inertial parameters to the part of inverse dynamics that depends on inertial parameters, and the generalizedExternalForces that computes the contribution of external forces to inverse dynamics (useful because those are not included in the torques computed by the regressor). --- doc/dcTutorialCpp.md | 56 +++++---- .../include/iDynTree/KinDynComputations.h | 33 +++++ src/high-level/src/KinDynComputations.cpp | 116 ++++++++++++++++++ .../tests/KinDynComputationsUnitTest.cpp | 34 ++++- src/model/include/iDynTree/Model/Dynamics.h | 54 +++++++- src/model/src/Dynamics.cpp | 56 +++++++++ .../integration/DynamicsIntegrationTest.cpp | 57 +++++++++ 7 files changed, 377 insertions(+), 29 deletions(-) diff --git a/doc/dcTutorialCpp.md b/doc/dcTutorialCpp.md index 124d136354e..ca1f9cb2edd 100644 --- a/doc/dcTutorialCpp.md +++ b/doc/dcTutorialCpp.md @@ -1,29 +1,36 @@ -# DynamicsComputations tutorial +# KinDynComputations tutorial -To use the DynamicsComputations class, you have to include its header. +To use the KinDynComputations class, you have to include its header. For this tutorial we also use the appropriate `using` definitions to -avoid having to type the namespace of the class. +avoid having to type the `iDynTree` namespace. ~~~cpp #include -#include +#include +#include using namespace iDynTree; -using namespace iDynTree::HighLevel; ~~~ The first thing that we have to do is to initialize the class by providing a model of the robot. Currently iDynTree supports only URDF models to specify the model. +All the logic for loading models, including loading reduced models, +is contained in the iDynTree::ModelLoader class. ~~~cpp int main() { - DynamicsComputations dynComp; - dynComp.loadRobotModelFromFile("model.urdf"); - std::cout << "The loaded model has " << dynComp.getNrOfDegreesOfFreedom() - << "internal degrees of freedom and " << dynComp.getNrOfLinks() + ModelLoader mdlLoader; + bool ok = mdlLoader.loadModelFromFile(urdf_filename); + + // Check that ok is actually true + + KinDynComputations kinDynComp; + kinDynComp.loadRobotModel(mdlLoader.model()); + std::cout << "The loaded model has " << kinDynComp.model().getNrOfDOFs() + << "internal degrees of freedom and " << kinDynComp.model().getNrOfLinks() << "links." << std::endl; ~~~ @@ -34,37 +41,37 @@ to specify the joint position, velocity and accelerations and the gravity in the base frame. ~~~cpp - unsigned int dofs = dynComp.getNrOfDegreesOfFreedom(); - VectorDynSize q(dofs), dq(dofs), ddq(dofs); + unsigned int dofs = kinDynComp.model(); + VectorDynSize s(dofs), ds(dofs), dds(dofs); for(unsigned int dof = 0; dof < dofs; dof++ ) { // For the sake of the example, we fill the joints // vector with gibberish data (remember in any case // that all quantities are expressed in radians-based // units - q(dof) = 1.0; - dq(dof) = 0.4; - ddq(dof) = 0.3; + s(dof) = 1.0; + ds(dof) = 0.4; + dds(dof) = 0.3; } - // The spatial acceleration is a 6d acceleration vector. + // The gravity acceleration is a 3d vector. + // // For all 6d quantities, we use the linear-angular serialization // (the first three value are for the linear quantity, the // the last three values are for the angular quantity) - SpatialAcc gravity; - gravity(3) = -9.81; - dynComp.setRobotState(q,dq,ddq,gravity); + Vector3 gravity; + gravity.zero(); + gravity(2) = -9.81; + kinDynComp.setRobotState(s,ds,gravity); ~~~ Once you set the state, you can access the computed dynamical quantities. For example you can get the jacobian for a given frame with name "frameName". Note that "frameName" should be the name of a link in the URDF model. -Note that the jacobian is the floating base jacobian as described in -http://wiki.icub.org/codyco/dox/html/dynamics_notation.html . ~~~cpp MatrixDynSize jac(6,6+dofs); - bool ok = dynComp.getFrameJacobian("frameName",jac); + bool ok = kinDynComp.getFrameFreeFloatingJacobian("frameName", jac); if( !ok ) { @@ -79,8 +86,7 @@ http://wiki.icub.org/codyco/dox/html/dynamics_notation.html . You can also get the dynamics regressor. The dynamics regressor is a (6+dofs) \* (10 \* nrOfLinks) Y matrix such that: -Y pi = M(q) d(v)/dt + C(q,v)v + g(q) -with M(q), C(q,v) and g(q) defined in http://wiki.icub.org/codyco/dox/html/dynamics_notation.html . +Y pi = M(q) d(v)/dt + C(q,v)v + g(q) . The pi vector is a 10 \* nrOfLinks inertial parameters vector, such that the elements of the vector from the ((i-1) \* 10)-th to the (i \* 10-1)-th belong to the i-th link. For more details on the inertial parameters vector, please check https://hal.archives-ouvertes.fr/hal-01137215/document . @@ -88,7 +94,9 @@ parameters vector, please check https://hal.archives-ouvertes.fr/hal-01137215/do ~~~cpp unsigned int links = dynComp.getNrOfLinks(); MatrixDynSize regr(6+dofs,10*links); - ok = dynComp.getDynamicsRegressor(regr); + Vector6 baseAcc; + baseAcc.zero(); + ok = kinDynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr); if( !ok ) { diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index 1b4bd231be9..6324c2714b7 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/src/high-level/include/iDynTree/KinDynComputations.h @@ -697,6 +697,39 @@ class KinDynComputations { */ bool generalizedGravityForces(FreeFloatingGeneralizedTorques & generalizedGravityForces); + /** + * Compute the getNrOfDOFS()+6 vector of generalized external forces. + * + * The semantics of baseAcc, the base part of baseForceAndJointTorques + * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . + * + * The state is the one given set by the setRobotState method. + * + * @param[out] generalizedExternalForces the output external generalized forces + * @return true if all went well, false otherwise + */ + bool generalizedExternalForces(const LinkNetExternalWrenches & linkExtForces, + FreeFloatingGeneralizedTorques & generalizedExternalForces); + + /** + * @brief Compute the free floating inverse dynamics as a linear function of inertial parameters. + * + * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorques + * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . + * + * The state is the one given set by the setRobotState method. + * + * @see iDynTree::InverseDynamicsInertialParametersRegressor for more info on the underlying algorithm. + * + * @param[in] baseAcc the acceleration of the base link + * @param[in] s_ddot the accelerations of the joints + * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor. + * @return true if all went well, false otherwise + */ + bool inverseDynamicsInertialParametersRegressor(const Vector6& baseAcc, + const VectorDynSize& s_ddot, + MatrixDynSize& baseForceAndJointTorquesRegressor); + //@} diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index a15d7ed6f34..1b360b0ca41 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -168,6 +168,9 @@ struct KinDynComputations::KinDynComputationsPrivateAttributes /** Buffer of link velocities, always set to zero for gravity computations */ LinkVelArray m_invDynZeroLinkVel; + /** Buffer of link proper accelerations, always set to zero for external forces */ + LinkAccArray m_invDynZeroLinkProperAcc; + KinDynComputationsPrivateAttributes() { m_isModelValid = false; @@ -242,6 +245,7 @@ void KinDynComputations::resizeInternalDataStructures() this->pimpl->m_invDynZeroVel.baseVel().zero(); this->pimpl->m_invDynZeroVel.jointVel().zero(); this->pimpl->m_invDynZeroLinkVel.resize(this->pimpl->m_robot_model); + this->pimpl->m_invDynZeroLinkProperAcc.resize(this->pimpl->m_robot_model); this->pimpl->m_traversalCache.resize(this->pimpl->m_robot_model); for(LinkIndex lnkIdx = 0; lnkIdx < static_cast(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++) @@ -1909,5 +1913,117 @@ bool KinDynComputations::generalizedGravityForces(FreeFloatingGeneralizedTorques return true; } +bool KinDynComputations::generalizedExternalForces(const LinkNetExternalWrenches & linkExtForces, + FreeFloatingGeneralizedTorques & generalizedExternalForces) +{ + // Convert input external forces + if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION || + pimpl->m_frameVelRepr == MIXED_REPRESENTATION ) + { + this->computeFwdKinematics(); + + for(LinkIndex lnkIdx = 0; lnkIdx < static_cast(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++) + { + const Transform & inertialFrame_X_link = pimpl->m_linkPos(lnkIdx); + pimpl->m_invDynNetExtWrenches(lnkIdx) = pimpl->fromUsedRepresentationToBodyFixed(linkExtForces(lnkIdx),inertialFrame_X_link); + } + } + else + { + for(LinkIndex lnkIdx = 0; lnkIdx < static_cast(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++) + { + pimpl->m_invDynNetExtWrenches(lnkIdx) = linkExtForces(lnkIdx); + } + } + + // Call usual RNEA, but with both velocity and **proper acceleration** set to zero buffers + RNEADynamicPhase(pimpl->m_robot_model, + pimpl->m_traversal, + pimpl->m_pos.jointPos(), + pimpl->m_invDynZeroLinkVel, + pimpl->m_invDynZeroLinkProperAcc, + pimpl->m_invDynNetExtWrenches, + pimpl->m_invDynInternalWrenches, + generalizedExternalForces); + + // Convert output base force + generalizedExternalForces.baseWrench() = pimpl->fromBodyFixedToUsedRepresentation(generalizedExternalForces.baseWrench(), + pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex())); + return true; +} + +bool KinDynComputations::inverseDynamicsInertialParametersRegressor(const Vector6& baseAcc, + const VectorDynSize& s_ddot, + MatrixDynSize& regressor) +{ + // Needed for using pimpl->m_linkVel + this->computeFwdKinematics(); + + // Convert input base acceleration + if( pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION ) + { + fromEigen(pimpl->m_invDynBaseAcc,toEigen(baseAcc)); + } + else if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ) + { + pimpl->m_invDynBaseAcc = convertInertialAccelerationToBodyFixedAcceleration(baseAcc,pimpl->m_pos.worldBasePos()); + } + else + { + assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION); + pimpl->m_invDynBaseAcc = convertMixedAccelerationToBodyFixedAcceleration(baseAcc, + pimpl->m_vel.baseVel(), + pimpl->m_pos.worldBasePos().getRotation()); + } + + // Prepare the vector of generalized proper accs + pimpl->m_invDynGeneralizedProperAccs.baseAcc() = pimpl->m_invDynBaseAcc; + toEigen(pimpl->m_invDynGeneralizedProperAccs.baseAcc().getLinearVec3()) = + toEigen(pimpl->m_invDynBaseAcc.getLinearVec3()) - toEigen(pimpl->m_gravityAccInBaseLinkFrame); + toEigen(pimpl->m_invDynGeneralizedProperAccs.jointAcc()) = toEigen(s_ddot); + + // Run inverse dynamics + ForwardAccKinematics(pimpl->m_robot_model, + pimpl->m_traversal, + pimpl->m_pos, + pimpl->m_vel, + pimpl->m_invDynGeneralizedProperAccs, + pimpl->m_linkVel, + pimpl->m_invDynLinkProperAccs); + + // Compute the inverse dynamics regressor, using the absolute frame A as the reference frame in which the base dynamics is expressed + // (this is done out of convenience because the pimpl->m_linkPos (that contains for each link L the transform A_H_L) is already available + InverseDynamicsInertialParametersRegressor(pimpl->m_robot_model, + pimpl->m_traversal, + pimpl->m_linkPos, + pimpl->m_linkVel, + pimpl->m_invDynLinkProperAccs, + regressor); + + // Transform the first six rows of the regressor according to the choosen frame velocity representation + if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) + { + int cols = regressor.cols(); + Matrix6x6 B_X_A = pimpl->m_pos.worldBasePos().inverse().asAdjointTransformWrench(); + toEigen(regressor).block(0, 0, 6, cols) = + toEigen(B_X_A)*toEigen(regressor).block(0, 0, 6, cols); + + } + else if (pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION) + { + // In this case, the first six rows are already with the correct value + } + else + { + assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION); + int cols = regressor.cols(); + Matrix6x6 B_A_X_A = Transform(Rotation::Identity(), pimpl->m_pos.worldBasePos().getPosition()).inverse().asAdjointTransformWrench(); + toEigen(regressor).block(0, 0, 6, cols) = + toEigen(B_A_X_A)*toEigen(regressor).block(0, 0, 6, cols); + } + + return true; +} + } diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp index b9ae465a7de..dd68d30eeb6 100644 --- a/src/high-level/tests/KinDynComputationsUnitTest.cpp +++ b/src/high-level/tests/KinDynComputationsUnitTest.cpp @@ -21,6 +21,8 @@ #include #include + +#include #include #include @@ -180,7 +182,10 @@ void testInverseDynamics(KinDynComputations & dynComp) iDynTree::JointDOFsDoubleArray shapeAccs(dynComp.model()); iDynTree::LinkNetExternalWrenches netExternalWrenches(dynComp.model()); - netExternalWrenches.zero(); + for(unsigned int link=0; link < dynComp.model().getNrOfLinks(); link++ ) + { + netExternalWrenches(link) = getRandomWrench(); + } // Go component for component, for simplifyng debugging for(int i=0; i < 6+dofs; i++) @@ -198,6 +203,7 @@ void testInverseDynamics(KinDynComputations & dynComp) FreeFloatingGeneralizedTorques invDynForces(dynComp.model()); FreeFloatingGeneralizedTorques massMatrixInvDynForces(dynComp.model()); + FreeFloatingGeneralizedTorques regressorInvDynForces(dynComp.model()); // Run classical inverse dynamics bool ok = dynComp.inverseDynamics(baseAcc,shapeAccs,netExternalWrenches,invDynForces); @@ -212,8 +218,12 @@ void testInverseDynamics(KinDynComputations & dynComp) ok = dynComp.generalizedBiasForces(invDynBiasForces); ASSERT_IS_TRUE(ok); + FreeFloatingGeneralizedTorques invDynExtForces(dynComp.model()); + ok = dynComp.generalizedExternalForces(netExternalWrenches, invDynExtForces); + ASSERT_IS_TRUE(ok); + VectorDynSize massMatrixInvDynForcesContinuous(6+dofs); - toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces); + toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces) + toEigen(invDynExtForces); toEigen(massMatrixInvDynForces.baseWrench().getLinearVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(0); toEigen(massMatrixInvDynForces.baseWrench().getAngularVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(3); toEigen(massMatrixInvDynForces.jointTorques()) = toEigen(massMatrixInvDynForcesContinuous).segment(6,dofs); @@ -221,6 +231,26 @@ void testInverseDynamics(KinDynComputations & dynComp) ASSERT_EQUAL_SPATIAL_FORCE(massMatrixInvDynForces.baseWrench(),invDynForces.baseWrench()); ASSERT_EQUAL_VECTOR(massMatrixInvDynForces.jointTorques(),invDynForces.jointTorques()); + // Run inverse dynamics with regressor matrix + MatrixDynSize regressor(dynComp.model().getNrOfDOFs()+6, 10*dynComp.model().getNrOfLinks()); + ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, shapeAccs, regressor); + ASSERT_IS_TRUE(ok); + + + + VectorDynSize inertialParams(10*dynComp.model().getNrOfLinks()); + ok = dynComp.model().getInertialParameters(inertialParams); + ASSERT_IS_TRUE(ok); + + VectorDynSize regressorInvDynForcesContinuous(6+dofs); + toEigen(regressorInvDynForcesContinuous) = toEigen(regressor)*toEigen(inertialParams) + toEigen(invDynExtForces); + toEigen(regressorInvDynForces.baseWrench().getLinearVec3()) = toEigen(regressorInvDynForcesContinuous).segment<3>(0); + toEigen(regressorInvDynForces.baseWrench().getAngularVec3()) = toEigen(regressorInvDynForcesContinuous).segment<3>(3); + toEigen(regressorInvDynForces.jointTorques()) = toEigen(regressorInvDynForcesContinuous).segment(6,dofs); + + ASSERT_EQUAL_SPATIAL_FORCE(regressorInvDynForces.baseWrench(),invDynForces.baseWrench()); + ASSERT_EQUAL_VECTOR(regressorInvDynForces.jointTorques(),invDynForces.jointTorques()); + } } diff --git a/src/model/include/iDynTree/Model/Dynamics.h b/src/model/include/iDynTree/Model/Dynamics.h index 233b2f93af5..e467c987129 100644 --- a/src/model/include/iDynTree/Model/Dynamics.h +++ b/src/model/include/iDynTree/Model/Dynamics.h @@ -11,8 +11,9 @@ #ifndef IDYNTREE_INVERSE_DYNAMICS_H #define IDYNTREE_INVERSE_DYNAMICS_H -#include +#include +#include #include #include @@ -60,12 +61,26 @@ namespace iDynTree const LinkAccArray & linkBiasAcc, Wrench& totalMomentumBias); - + /** + * \ingroup iDynTreeModel + * + * @brief Compute the inverse dynamics, i.e. the generalized torques corresponding to a given set of robot accelerations and external force/torques. + * + * @param[in] model The model used for the computation. + * @param[in] traversal The traversal used for the computation, it defines the used base link. + * @param[in] jointPos The (internal) joint position of the model. + * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). + * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model + * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . See iDynTree::LinkNetExternalWrenches . + * @param[in] linkExtForces Vector of external 6D force/torques applied to the links. For each link \f$L\f$, the corresponding external force is \f${}_L \mathrm{f}^x_L\f$, i.e. the force that the enviroment applies on the on the link \f$L\f$, expressed in the link frame \f$L\f$. + * @param[out] linkIntWrenches Vector of internal joint force/torques. See iDynTree::LinkInternalWrenches . + * @param[out] baseForceAndJointTorques Generalized torques output. The base element is the residual force on the base (that is equal to zero if the robot acceleration and the external forces provided in LinkNetExternalWrenches were consistent), while the joint part is composed by the joint torques. + */ bool RNEADynamicPhase(const iDynTree::Model & model, const iDynTree::Traversal & traversal, const iDynTree::JointPosDoubleArray & jointPos, const iDynTree::LinkVelArray & linksVel, - const iDynTree::LinkAccArray & linksAcc, + const iDynTree::LinkAccArray & linksProperAcc, const iDynTree::LinkNetExternalWrenches & linkExtForces, iDynTree::LinkInternalWrenches & linkIntWrenches, iDynTree::FreeFloatingGeneralizedTorques & baseForceAndJointTorques); @@ -128,6 +143,8 @@ namespace iDynTree }; /** + * \ingroup iDynTreeModel + * * Compute the floating base acceleration of an unconstrianed * robot, using as input the external forces and the joint torques. * We follow the algorithm described in Featherstone 2008, modified @@ -143,6 +160,37 @@ namespace iDynTree ArticulatedBodyAlgorithmInternalBuffers & buffers, FreeFloatingAcc & robotAcc); + /** + * \ingroup iDynTreeModel + * + * + * @brief Compute the inverse dynamics of the model as linear function of the inertial parameters. + * + * This function computes the matrix that multiplied by the vector of inertial parameters of a model (see iDynTree::Model::getInertialParameters) + * returns the inverse dynamics generalized torques. In particular it is consistent with the result of the iDynTree::RNEADynamicPhase function, i.e. + * the first six rows of the regressor correspond to the sum of all external force/torques acting on the robot, expressed in the origin + * and with the orientation of the specified referenceFrame, as defined by the referenceFrame_H_link argument. + * + * + * @note The regressor only computes the inverse dynamics generalized torques assuming that the external forces are equal to zero, + * as the contribution of the external forces to the inverse dynamics is indipendent from inertial parameters. + * + * @param[in] model The model used for the computation. + * @param[in] traversal The traversal used for the computation, it defines the used base link. + * @param[in] referenceFrame_H_link Position of each link w.r.t. to given frame D (tipically an inertial frame A, the base frame B or the mixed frame B[A]). For each link \f$L\f$, the corresponding transform is \f${}^D H_L\f$. + * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$). + * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model + * (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . + * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor. + * + */ + bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::LinkPositions& referenceFrame_H_link, + const iDynTree::LinkVelArray & linksVel, + const iDynTree::LinkAccArray & linksAcc, + iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor); + } diff --git a/src/model/src/Dynamics.cpp b/src/model/src/Dynamics.cpp index d9d0d1fa081..53cacb98075 100644 --- a/src/model/src/Dynamics.cpp +++ b/src/model/src/Dynamics.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -476,6 +477,61 @@ bool ArticulatedBodyAlgorithm(const Model& model, return true; } +bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model, + const iDynTree::Traversal & traversal, + const iDynTree::LinkPositions& referenceFrame_H_link, + const iDynTree::LinkVelArray & linksVel, + const iDynTree::LinkAccArray & linksProperAcc, + iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor) +{ + baseForceAndJointTorquesRegressor.resize(6+model.getNrOfDOFs(), 10*model.getNrOfLinks()); + baseForceAndJointTorquesRegressor.zero(); + + // Eigen map of the input matrix + iDynTreeEigenMatrixMap regressor = iDynTree::toEigen(baseForceAndJointTorquesRegressor); + + Matrix6x10 netForceTorqueRegressor_i; + + for (TraversalIndex l =(TraversalIndex)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) { + + // In this cycle, we compute the contribution of the inertial parameters of link to the inverse dynamics results + LinkConstPtr link = traversal.getLink(l); + LinkIndex lnkIdx = link->getIndex(); + + // Each link affects the dynamics of the joints from itself to the base + netForceTorqueRegressor_i = SpatialInertia::momentumDerivativeRegressor(linksVel(lnkIdx), + linksProperAcc(lnkIdx)); + + iDynTreeEigenMatrix linkRegressor = iDynTree::toEigen(netForceTorqueRegressor_i); + + // Base dynamics + // The base dynamics is expressed with the orientation of the base and with respect to the base origin + regressor.block(0,(int)(10*lnkIdx),6,10) = + toEigen(referenceFrame_H_link(lnkIdx).asAdjointTransformWrench())*linkRegressor; + + LinkIndex visitedLinkIdx = lnkIdx; + + while (visitedLinkIdx != traversal.getBaseLink()->getIndex()) + { + LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex(); + IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx); + + size_t dofOffset = joint->getDOFsOffset(); + for(int i=0; i < joint->getNrOfDOFs(); i++) + { + iDynTree::SpatialMotionVector S = joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx); + Transform visitedLink_H_link = referenceFrame_H_link(visitedLinkIdx).inverse()*referenceFrame_H_link(lnkIdx); + regressor.block(6+dofOffset+i,10*lnkIdx,1,10) = + toEigen(S).transpose()*(toEigen(visitedLink_H_link.asAdjointTransformWrench())*linkRegressor); + } + + visitedLinkIdx = parentLinkIdx; + } + + } + + return true; +} } diff --git a/src/tests/integration/DynamicsIntegrationTest.cpp b/src/tests/integration/DynamicsIntegrationTest.cpp index dc52f9155d8..f6a0d8774a8 100644 --- a/src/tests/integration/DynamicsIntegrationTest.cpp +++ b/src/tests/integration/DynamicsIntegrationTest.cpp @@ -9,6 +9,7 @@ */ +#include #include #include @@ -109,6 +110,62 @@ void checkInverseAndForwardDynamicsAreIdempotent(const Model & model, zeroVec,1e-6); std::cout << "Check joint torques" << std::endl; ASSERT_EQUAL_VECTOR_TOL(RNEA_baseForceAndJointTorques.jointTorques(),ABA_jntTorques,1e-07); + + // Also check that the inverse dynamics as computed by regressor*inertial parameters is consistent + MatrixDynSize regressor(6+model.getNrOfDOFs(), 10*model.getNrOfLinks()); + VectorDynSize inertialParams(10*model.getNrOfLinks()); + VectorDynSize invDynResults(6+model.getNrOfDOFs()); + + bool ok = model.getInertialParameters(inertialParams); + ASSERT_IS_TRUE(ok); + + // For computing inertial params we need the transform between baseLink and each link + LinkPositions baseLink_H_link(model); + ok = ForwardPositionKinematics(model, + traversal, + Transform::Identity(), + robotPos.jointPos(), + baseLink_H_link); + ASSERT_IS_TRUE(ok); + + ok = InverseDynamicsInertialParametersRegressor(model, + traversal, + baseLink_H_link, + RNEA_linksVel, + RNEA_linksAcc, + regressor); + + // The regressor just accounts for the gravitational and "inertial" forces, but we should + // take into account also the effect of external forces + // Allocate temporary data structure for RNEA for computing ext forces + LinkVelArray RNEA_EXT_linksVel(model); // Zero + LinkAccArray RNEA_EXT_linksAcc(model); // Zero + LinkInternalWrenches RNEA_EXT_linkIntWrenches(model); + FreeFloatingGeneralizedTorques RNEA_EXT_baseForceAndJointTorques(model); + + RNEADynamicPhase(model, + traversal, + robotPos.jointPos(), + RNEA_EXT_linksVel, + RNEA_EXT_linksAcc, + linkExtWrenches, + RNEA_EXT_linkIntWrenches, + RNEA_EXT_baseForceAndJointTorques); + + ASSERT_IS_TRUE(ok); + + toEigen(invDynResults) = + toEigen(regressor)*toEigen(inertialParams); + + Vector6 REGR_baseWrench; + VectorDynSize REGR_jointTorques(model.getNrOfDOFs()); + + toEigen(REGR_baseWrench) = toEigen(invDynResults).segment<6>(0) + toEigen(RNEA_EXT_baseForceAndJointTorques.baseWrench()); + toEigen(REGR_jointTorques) = toEigen(invDynResults).segment(6, model.getNrOfDOFs()) + toEigen(RNEA_EXT_baseForceAndJointTorques.jointTorques()); + + double tolRegr = 1e-6; + ASSERT_EQUAL_VECTOR_TOL(REGR_baseWrench, RNEA_baseForceAndJointTorques.baseWrench().asVector(), tolRegr); + ASSERT_EQUAL_VECTOR_TOL(REGR_jointTorques, RNEA_baseForceAndJointTorques.jointTorques(), tolRegr); } int main() From 9cb82d60bfe712107b7f50e036ec937241053545 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 11 Sep 2018 13:13:37 +0200 Subject: [PATCH 2/5] Add documentation for inverseDynamicsRegressorInertialParameters Cleanup also documentation for KinDynComputations, to simplify the process of adding docs for the new method. --- .../include/iDynTree/KinDynComputations.h | 127 +++++++++++------- .../iDynTree/Model/FreeFloatingMatrices.h | 10 +- 2 files changed, 85 insertions(+), 52 deletions(-) diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index 6324c2714b7..83ad6bb2b1b 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/src/high-level/include/iDynTree/KinDynComputations.h @@ -108,6 +108,7 @@ class KinDynComputations { /** * @name Model loading and definition methods * This methods are used to load the structure of your model. + * */ //@{ @@ -148,11 +149,14 @@ class KinDynComputations { /** * Set the used FrameVelocityRepresentation. + * + * @see FrameVelocityRepresentation */ bool setFrameVelocityRepresentation(const FrameVelocityRepresentation frameVelRepr) const; /** - * Get the used FrameVelocityRepresentation. + * @brief Get the used FrameVelocityRepresentation. + * @see setFrameVelocityRepresentation */ FrameVelocityRepresentation getFrameVelocityRepresentation() const; //@} @@ -188,20 +192,6 @@ class KinDynComputations { */ unsigned int getNrOfLinks() const; - /** - * Get a human readable description of a given link in the model. - * - * @return a human readable description of a given link in the model. - */ - //std::string getDescriptionOfLink(int link_index); - - /** - * Get a human readable description of all links considered in the model. - * - * @return a std::string containing the description of all the links. - */ - //std::string getDescriptionOfLinks(); - /** * Get the number of frames contained in the model. * @@ -212,20 +202,6 @@ class KinDynComputations { */ unsigned int getNrOfFrames() const; - /** - * Get a human readable description of a given frame considered in the model. - * - * @return a human readable description of a given frame considered in the model. - */ - //std::string getDescriptionOfFrame(int frame_index); - - /** - * Get a human readable description of all frames considered in the model. - * - * @return a std::string containing the description of all the frame considered in the model. - */ - //std::string getDescriptionOfLinks(); - /** * Get the name of the link considered as the floating base. * @@ -368,8 +344,7 @@ class KinDynComputations { //@} /** - * @name Methods to get transform information between frames in the model, - * given the current state. + * @name Methods to get transform information between frames in the model, given the current state. */ //@{ @@ -623,12 +598,54 @@ class KinDynComputations { /** - * @name Methods to get quantities related to dynamics matrices. + * @name Methods to get quantities related to unconstrained free floating equation of motions. + * + * This methods permits to compute several quantities related to free floating equation of methods. + * Note that this equations needs to be coupled with a description of the interaction between the model + * and the enviroment (such as a contant model, a bilateral constraint on some links or by considering + * some external forces as inputs) to actually obtain a dynamical system description of the mechanical model evolution. + * + * The equations of motion of a free floating mechanical system under the effect of a uniform gravitational field are: + * \f[ + * M(q) \dot{\nu} + + * C(q, \nu) \nu + + * G(q) + * = + * \begin{bmatrix} + * 0_{6\times1} \newline + * \tau + * \end{bmatrix} + * + + * \sum_{L \in \mathcal{L}} + * J_L^T \mathrm{f}_L^x + * \f] + * + * where: + * + * * \f$n_{PC}\f$ is the value returned by Model::getNrOfPosCoords, + * * \f$n_{DOF}\f$ is the value returned by Model::getNrOfDOFs, + * * \f$n_{L}\f$ is the value returned by Model::getNrOfLinks, + * * \f$q \in \mathbb{R}^3 \times \textrm{SO}(3) \times \mathbb{R}^{n_{PC}}\f$ is the robot position, + * * \f$\nu \in \mathbb{R}^{6+n_{DOF}}\f$ is the robot velocity, + * * \f$\dot{\nu} \in \mathbb{R}^{6+n_{DOF}}\f$ is the robot acceleration, + * * \f$M(q) \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$ is the free floating mass matrix, + * * \f$C(q, \nu) \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$ is the coriolis matrix, + * * \f$G(q) \in \mathbb{R}^{6+n_{DOF}}\f$ is the vector of gravity generalized forces, + * * \f$\tau \in \mathbb{R}^6\f$ is the vector of torques applied on the joint of the multibody model, + * * \f$\mathcal{L}\f$ is the set of all the links contained in the multibody model, + * * \f$J_L \in \mathbb{R}^{6+n_{DOF}}\f$ is the free floating jacobian of link \f$L\f$ as obtained by KinDynComputations::getFrameFreeFloatingJacobian, + * * \f$\mathrm{f}_L^x\f$ is the 6D force/torque applied by the enviroment on link \f$L\f$. + * + * The precise definition of each quantity (in particular the part related to the base) actually depends on the + * choice of FrameVelocityRepresentation, specified with the setFrameVelocityRepresentation method. + * */ //@{ /** - * Get the free floating mass matrix of the system. + * @brief Get the free floating mass matrix of the system. + * + * This method computes \f$M(q) \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$. * * The mass matrix depends on the joint positions, specified by the setRobotState methods. * If the chosen FrameVelocityRepresentation is MIXED_REPRESENTATION or INERTIAL_FIXED_REPRESENTATION, @@ -645,15 +662,11 @@ class KinDynComputations { */ bool getFreeFloatingMassMatrix(MatrixDynSize & freeFloatingMassMatrix); - //@} /** - * @name Methods to unconstrained free floating dynamics. - */ - //@{ - - /** - * Compute the free floating inverse dynamics. + * @brief Compute the free floating inverse dynamics. + * + * This method computes \f$M(q) \dot{\nu} + C(q, \nu) \nu + G(q) - \sum_{L \in \mathcal{L}} J_L^T \mathrm{f}_L^x \in \mathbb{R}^{6+n_{DOF}}\f$. * * The semantics of baseAcc, the base part of baseForceAndJointTorques * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . @@ -672,10 +685,11 @@ class KinDynComputations { FreeFloatingGeneralizedTorques & baseForceAndJointTorques); /** - * Compute the getNrOfDOFS()+6 vector of generalized bias (gravity+coriolis) forces. + * @brief Compute the getNrOfDOFS()+6 vector of generalized bias (gravity+coriolis) forces. * - * The semantics of baseAcc, the base part of baseForceAndJointTorques - * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . + * This method computes \f$C(q, \nu) \nu + G(q) \in \mathbb{R}^{6+n_{DOF}}\f$. + * + * The semantics of the base part of generalizedBiasForces depend of the chosen FrameVelocityRepresentation . * * The state is the one given set by the setRobotState method. * @@ -685,10 +699,11 @@ class KinDynComputations { bool generalizedBiasForces(FreeFloatingGeneralizedTorques & generalizedBiasForces); /** - * Compute the getNrOfDOFS()+6 vector of generalized gravity forces. + * @brief Compute the getNrOfDOFS()+6 vector of generalized gravity forces. * - * The semantics of baseAcc, the base part of baseForceAndJointTorques - * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . + * This method computes \f$G(q) \in \mathbb{R}^{6+n_{DOF}}\f$. + * + * The semantics of the base part of generalizedGravityForces depend of the chosen FrameVelocityRepresentation . * * The state is the one given set by the setRobotState method. * @@ -698,9 +713,14 @@ class KinDynComputations { bool generalizedGravityForces(FreeFloatingGeneralizedTorques & generalizedGravityForces); /** - * Compute the getNrOfDOFS()+6 vector of generalized external forces. + * @brief Compute the getNrOfDOFS()+6 vector of generalized external forces. * - * The semantics of baseAcc, the base part of baseForceAndJointTorques + * This method computes \f$ -\sum_{L \in \mathcal{L}} J_L^T \mathrm{f}_L^x \in \mathbb{R}^{6+n_{DOF}} \f$. + * + * @warning Note that this method returns the **negated** sum of the product of jacobian and the external force, + * consistently with how the generalized external forces are computed in the KinDynComputations::inverseDynamics method. + * + * The semantics of the base part of generalizedExternalForces * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . * * The state is the one given set by the setRobotState method. @@ -714,8 +734,15 @@ class KinDynComputations { /** * @brief Compute the free floating inverse dynamics as a linear function of inertial parameters. * - * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorques - * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation . + * This methods computes the \f$ Y(\dot{\nu}, \nu, q) \in \mathbb{R}^{ (6+n_{DOF}) \times (10n_{L}) } \f$ matrix such that: + * \f[ + * Y(\dot{\nu}, \nu, q) \phi = M(q) \dot{\nu} + C(q, \nu) \nu + G(q) + * \f] + * + * where \f$\phi \in \mathbb{R}^{10n_{L}}\f$ is the vector of inertial parameters returned by the Model::getInertialParameters . + * + * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorquesRegressor + * depend of the chosen FrameVelocityRepresentation . * * The state is the one given set by the setRobotState method. * diff --git a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h index 6404629b140..d51c1ac507e 100644 --- a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h +++ b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h @@ -19,9 +19,15 @@ namespace iDynTree class Model; /** - * Enum describing the possible frame velocity representation convention. + * @brief Possible frame velocity representation convention. * - * See KinDynComputations documentation for more details. + * Given a link \f$L\f$ and an absolute frame \f$A\f$, the + * the possible frame velocity representation are the following: + * * `INERTIAL_FIXED_REPRESENTATION` : Velocity representation is \f${}^{A} \mathrm{v}_{A,B}\f$, + * * `BODY_FIXED_REPRESENTATION` : Velocity representation is \f${}^{B} \mathrm{v}_{A,B}\f$, + * * `MIXED_REPRESENTATION` : Velocity representation is \f${}^{B[A]} \mathrm{v}_{A,B}\f$. + * + * See iDynTree::KinDynComputations documentation for more details. */ enum FrameVelocityRepresentation { From 2d6a48d6c11163ebed7bfde413928d773855cd74 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 11 Sep 2018 14:02:22 +0200 Subject: [PATCH 3/5] Migrate python from DynamicsComputations to KinDynComputations --- .../python/dynamicsComputationTutorial.py | 50 +++++++++++-------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/examples/python/dynamicsComputationTutorial.py b/examples/python/dynamicsComputationTutorial.py index 7b6719bc773..dcfbcdb7990 100644 --- a/examples/python/dynamicsComputationTutorial.py +++ b/examples/python/dynamicsComputationTutorial.py @@ -6,46 +6,52 @@ """ import iDynTree -from iDynTree import DynamicsComputations +from iDynTree import KinDynComputations +from iDynTree import ModelLoader + URDF_FILE = '/home/username/path/robot.urdf'; -dynComp = DynamicsComputations(); -dynComp.loadRobotModelFromFile(URDF_FILE); -print "The loaded model has", dynComp.getNrOfDegreesOfFreedom(), \ - "internal degrees of freedom and",dynComp.getNrOfLinks(),"links." +dynComp = KinDynComputations(); +mdlLoader = ModelLoader(); +mdlLoader.loadModelFromFile(URDF_FILE); + +dynComp.loadRobotModel(mdlLoader.model()); + +print "The loaded model has", dynComp.model().getNrOfDOFs(), \ + "internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links." -dofs = dynComp.getNrOfDegreesOfFreedom(); -q = iDynTree.VectorDynSize(dofs); -dq = iDynTree.VectorDynSize(dofs); -ddq = iDynTree.VectorDynSize(dofs); +dofs = dynComp.model().getNrOfDOFs(); +s = iDynTree.VectorDynSize(dofs); +ds = iDynTree.VectorDynSize(dofs); +dds = iDynTree.VectorDynSize(dofs); for dof in range(dofs): # For the sake of the example, we fill the joints vector with gibberish data (remember in any case # that all quantities are expressed in radians-based units - q.setVal(dof, 1.0); - dq.setVal(dof, 0.4); - ddq.setVal(dof, 0.3); + s.setVal(dof, 1.0); + ds.setVal(dof, 0.4); + dds.setVal(dof, 0.3); -# The spatial acceleration is a 6d acceleration vector. -# For all 6d quantities, we use the linear-angular serialization -# (the first three value are for the linear quantity, the -# the last three values are for the angular quantity) -gravity = iDynTree.SpatialAcc(); +# The gravity acceleration is a 3d acceleration vector. +gravity = iDynTree.Vector3(); gravity.zero(); gravity.setVal(2, -9.81); -dynComp.setRobotState(q,dq,ddq,gravity); +dynComp.setRobotState(s,ds,gravity); jac = iDynTree.MatrixDynSize(6,6+dofs); -ok = dynComp.getFrameJacobian("lf_foot", jac); +ok = dynComp.getFreeFloatingJacobian("lf_foot", jac); if( not ok ): print "Error in computing jacobian of frame " + "lf_foot"; else: print "Jacobian of lf_foot is\n" + jac.toString(); - -links = dynComp.getNrOfLinks(); + + +baseAcc = iDynTree.Vector6(); +baseAcc.zero(); +links = dynComp.model().getNrOfLinks(); regr = iDynTree.MatrixDynSize(6+dofs,10*links); -ok = dynComp.getDynamicsRegressor(regr); +ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr); if( not ok ): print "Error in computing the dynamics regressor"; else : From c0403ead4a3b471f336a5d26d457089caaf0f339 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 11 Sep 2018 14:02:39 +0200 Subject: [PATCH 4/5] Cleanup of deprecated classes --- doc/main.dox | 13 +++++++++++++ .../iDynTree/Estimation/simpleLeggedOdometryKDL.h | 2 +- .../iDynTree/HighLevel/DynamicsComputations.h | 2 +- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/doc/main.dox b/doc/main.dox index 81ee58e68bb..b37d9695ce3 100644 --- a/doc/main.dox +++ b/doc/main.dox @@ -83,6 +83,13 @@ */ + /** + * \defgroup iDynTreeDeprecated Deprecated functions and classes + * + * \warning The functions and classes in this component should not be used for new code. + * + */ + @@ -121,5 +128,11 @@ Until this components are ready to be integrated in a proper part of iDynTree, w \li \ref iDynTreeExperimental "Experimental" : Experimental data structures and algorithms, whose interface is not guaranteed to be stable. +iDynTree started as a tiny wrapper of code between [YARP](http://yarp.it/) and [KDL](https://github.com/orocos/orocos_kinematics_dynamics). +For this reason, in the repository there is still some legacy code, that should not be used by users, as it is going to be removed from iDynTree. + +\li \ref iDynTreeDeprecated "Deprecated" : Deprecated data structures and algorithms, that are going to be removed from iDynTree. + + * */ diff --git a/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h b/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h index 5391ccf78f6..0933233da5b 100644 --- a/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h +++ b/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h @@ -15,7 +15,7 @@ namespace iDynTree { /** - * \ingroup iDynTreeEstimation + * \ingroup iDynTreeDeprecated * * A simple legged odometry for a legged robot. * diff --git a/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h b/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h index cc4625e173f..a4378c4ee07 100644 --- a/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h +++ b/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h @@ -30,7 +30,7 @@ class Wrench; namespace HighLevel { /** - * \ingroup iDynTreeHighLevel + * \ingroup iDynTreeDeprecated * * The dynamics computations class is an high level class stateful to access * several algorithms related to kinematics and dynamics of free floating From 122beec6b9f7d06c976065262f36dee32490b505 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 11 Sep 2018 14:40:43 +0200 Subject: [PATCH 5/5] Update changelog --- doc/releases/v0_12.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md index a7e54f1d9a0..5447674a41e 100644 --- a/doc/releases/v0_12.md +++ b/doc/releases/v0_12.md @@ -13,6 +13,10 @@ Summary Important Changes ----------------- +#### `high-level` +* Added method to compute the inverse dynamics inertial parameters regressor in KinDynComputations ( https://github.com/robotology/idyntree/pull/480 ). +KinDynComputations finally reached feature parity with respect to DynamicsComputations, that will finally be removed in one of the future iDynTree feature releases. + #### `inverse-kinematics` * Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).