diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h index 03ac49ade14..f9ce3238bdd 100644 --- a/src/high-level/include/iDynTree/KinDynComputations.h +++ b/src/high-level/include/iDynTree/KinDynComputations.h @@ -79,6 +79,7 @@ class KinDynComputations { // Resize internal data structures after a model has been successfully loaded void resizeInternalDataStructures(); + public: /** @@ -407,6 +408,24 @@ class KinDynComputations { */ iDynTree::Twist getFrameVel(const FrameIndex frameIdx); + /** + * Return the frame acceleration, with the convention specified by getFrameVelocityRepresentation . + * + * @warning As this method recomputes the accelerations of all links for each call, it may be computationally expensive. + */ + Vector6 getFrameAcc(const std::string & frameName, + const Vector6& baseAcc, + const VectorDynSize& s_ddot); + + /** + * Return the frame acceleration, with the convention specified by getFrameVelocityRepresentation . + * + * @warning As this method recomputes the accelerations of all links for each call, it may be computationally expensive. + */ + Vector6 getFrameAcc(const FrameIndex frameIdx, + const Vector6& baseAcc, + const VectorDynSize& s_ddot); + bool getFrameFreeFloatingJacobian(const std::string & frameName, iDynTree::MatrixDynSize & outJacobian); @@ -414,6 +433,8 @@ class KinDynComputations { iDynTree::MatrixDynSize & outJacobian); + + /** * Return the relative Jacobian between the two frames * diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp index 53e3f6838d8..93e47473cb3 100644 --- a/src/high-level/src/KinDynComputations.cpp +++ b/src/high-level/src/KinDynComputations.cpp @@ -142,9 +142,21 @@ struct KinDynComputations::KinDynComputationsPrivateAttributes // Bias accelerations buffers bool m_areBiasAccelerationsUpdated; + // Storate of base bias acceleration + SpatialAcc m_baseBiasAcc; + // storage of bias accelerations buffers (contains the bias acceleration for the given link in body-fixed) LinkAccArray m_linkBiasAcc; + /** Base acceleration, in body-fixed representation */ + SpatialAcc m_baseAcc; + + /** Generalized acceleration, base part in body-fixed representation */ + FreeFloatingAcc m_generalizedAccs; + + /** Acceleration of each link, in body-fixed representation, i.e. \f$ {}^L \mathrm{v}_{A,L} \f$ */ + LinkAccArray m_linkAccs; + // Inverse dynamics buffers /** Base acceleration, in body-fixed representation */ @@ -178,6 +190,93 @@ struct KinDynComputations::KinDynComputationsPrivateAttributes } }; + +typedef Eigen::Matrix Matrix3dRowMajor; +/** + * Function to convert a body fixed acceleration to a mixed acceleration. + * + * TODO refactor in a more general handling of conversion between the three + * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors + * acceleration. + * + * @return The mixed acceleration + */ +Vector6 convertBodyFixedAccelerationToMixedAcceleration(const SpatialAcc & bodyFixedAcc, + const Twist & bodyFixedVel, + const Rotation & inertial_R_body) +{ + Vector6 mixedAcceleration; + + Eigen::Map linBodyFixedAcc(bodyFixedAcc.getLinearVec3().data()); + Eigen::Map angBodyFixedAcc(bodyFixedAcc.getAngularVec3().data()); + + Eigen::Map linBodyFixedTwist(bodyFixedVel.getLinearVec3().data()); + Eigen::Map angBodyFixedTwist(bodyFixedVel.getAngularVec3().data()); + + Eigen::Map linMixedAcc(mixedAcceleration.data()); + Eigen::Map angMixedAcc(mixedAcceleration.data()+3); + + Eigen::Map inertial_R_body_eig(inertial_R_body.data()); + + // First we account for the effect of linear/angular velocity + linMixedAcc = inertial_R_body_eig*(linBodyFixedAcc + angBodyFixedTwist.cross(linBodyFixedTwist)); + + // Angular acceleration can be copied + angMixedAcc = inertial_R_body_eig*angBodyFixedAcc; + + return mixedAcceleration; +} + +/** + * Function to convert mixed acceleration to body fixed acceleration + * + * TODO refactor in a more general handling of conversion between the three + * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors + * acceleration. + * + * @return The body fixed acceleration + */ +SpatialAcc convertMixedAccelerationToBodyFixedAcceleration(const Vector6 & mixedAcc, + const Twist & bodyFixedVel, + const Rotation & inertial_R_body) +{ + SpatialAcc bodyFixedAcc; + + Eigen::Map linMixedAcc(mixedAcc.data()); + Eigen::Map angMixedAcc(mixedAcc.data()+3); + + Eigen::Map linBodyFixedTwist(bodyFixedVel.getLinearVec3().data()); + Eigen::Map angBodyFixedTwist(bodyFixedVel.getAngularVec3().data()); + + Eigen::Map inertial_R_body_eig(inertial_R_body.data()); + + Eigen::Map linBodyFixedAcc(bodyFixedAcc.getLinearVec3().data()); + Eigen::Map angBodyFixedAcc(bodyFixedAcc.getAngularVec3().data()); + + linBodyFixedAcc = inertial_R_body_eig.transpose()*linMixedAcc - angBodyFixedTwist.cross(linBodyFixedTwist); + angBodyFixedAcc = inertial_R_body_eig.transpose()*angMixedAcc; + + return bodyFixedAcc; +} + +/** + * Function to convert inertial acceleration to body acceleration. + * + * TODO refactor in a more general handling of conversion between the three + * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors + * acceleration. + * + * @return The body fixed acceleration + */ +SpatialAcc convertInertialAccelerationToBodyFixedAcceleration(const Vector6 & inertialAcc, + const Transform & inertial_H_body) +{ + SpatialAcc inertialAccProperForm; + fromEigen(inertialAccProperForm,toEigen(inertialAcc)); + return inertial_H_body.inverse()*inertialAccProperForm; +} + + KinDynComputations::KinDynComputations(): pimpl(new KinDynComputationsPrivateAttributes) { @@ -217,6 +316,7 @@ void KinDynComputations::invalidateCache() { this->pimpl->m_isFwdKinematicsUpdated = false; this->pimpl->m_isRawMassMatrixUpdated = false; + this->pimpl->m_areBiasAccelerationsUpdated = false; } void KinDynComputations::resizeInternalDataStructures() @@ -232,7 +332,11 @@ void KinDynComputations::resizeInternalDataStructures() this->pimpl->m_rawMassMatrix.zero(); this->pimpl->m_jacBuffer.resize(6,6+this->pimpl->m_robot_model.getNrOfDOFs()); this->pimpl->m_jacBuffer.zero(); + this->pimpl->m_baseBiasAcc.zero(); this->pimpl->m_linkBiasAcc.resize(this->pimpl->m_robot_model); + this->pimpl->m_baseAcc.zero(); + this->pimpl->m_generalizedAccs.resize(this->pimpl->m_robot_model); + this->pimpl->m_linkAccs.resize(this->pimpl->m_robot_model); this->pimpl->m_invDynBaseAcc.zero(); this->pimpl->m_invDynGeneralizedProperAccs.resize(this->pimpl->m_robot_model); this->pimpl->m_invDynNetExtWrenches.resize(this->pimpl->m_robot_model); @@ -319,11 +423,31 @@ void KinDynComputations::computeBiasAccFwdKinematics() return; } + // Convert input base acceleration, that in this case is zero + Vector6 zeroBaseAcc; + zeroBaseAcc.zero(); + if( pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION ) + { + fromEigen(pimpl->m_baseBiasAcc,toEigen(zeroBaseAcc)); + } + else if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ) + { + pimpl->m_baseBiasAcc = convertInertialAccelerationToBodyFixedAcceleration(zeroBaseAcc, pimpl->m_pos.worldBasePos()); + } + else + { + assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION); + pimpl->m_baseBiasAcc = convertMixedAccelerationToBodyFixedAcceleration(zeroBaseAcc, + pimpl->m_vel.baseVel(), + pimpl->m_pos.worldBasePos().getRotation()); + } + // Compute body-fixed bias accelerations bool ok = ForwardBiasAccKinematics(pimpl->m_robot_model, pimpl->m_traversal, pimpl->m_pos, pimpl->m_vel, + pimpl->m_baseBiasAcc, pimpl->m_linkVel, pimpl->m_linkBiasAcc); @@ -410,6 +534,15 @@ bool KinDynComputations::setFrameVelocityRepresentation(const FrameVelocityRepre return false; } + // If there is a change in FrameVelocityRepresentation, we should also invalidate the bias acceleration cache, as + // the bias acceleration depends on the frameVelRepr even if it is always expressed in body fixed representation. + // All the other cache are fine because they are always stored in BODY_FIXED, and they do not depend on the frameVelRepr, + // as they are converted on the fly when the relative retrieval method is called. + if (frameVelRepr != pimpl->m_frameVelRepr) + { + this->pimpl->m_areBiasAccelerationsUpdated = false; + } + pimpl->m_frameVelRepr = frameVelRepr; return true; } @@ -878,6 +1011,90 @@ Twist KinDynComputations::getFrameVel(const FrameIndex frameIdx) } +Vector6 KinDynComputations::getFrameAcc(const std::string & frameName, + const Vector6& baseAcc, + const VectorDynSize& s_ddot) +{ + return getFrameAcc(getFrameIndex(frameName), baseAcc, s_ddot); +} + +Vector6 KinDynComputations::getFrameAcc(const FrameIndex frameIdx, + const Vector6& baseAcc, + const VectorDynSize& s_ddot) +{ + if (!pimpl->m_robot_model.isValidFrameIndex(frameIdx)) + { + reportError("KinDynComputations","getFrameAcc","Frame index out of bounds"); + Vector6 ret; + ret.zero(); + return ret; + } + + // compute fwd kinematics (if necessary) + this->computeFwdKinematics(); + + // Convert input base acceleration + if( pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION ) + { + fromEigen(pimpl->m_baseAcc,toEigen(baseAcc)); + } + else if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ) + { + pimpl->m_baseAcc = convertInertialAccelerationToBodyFixedAcceleration(baseAcc,pimpl->m_pos.worldBasePos()); + } + else + { + assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION); + pimpl->m_baseAcc = convertMixedAccelerationToBodyFixedAcceleration(baseAcc, + pimpl->m_vel.baseVel(), + pimpl->m_pos.worldBasePos().getRotation()); + } + + // Prepare the vector of generalized accs (note: w.r.t. to inverseDynamics + // here we do not include the gravity in the acceleration of the base! + pimpl->m_generalizedAccs.baseAcc() = pimpl->m_baseAcc; + toEigen(pimpl->m_generalizedAccs.jointAcc()) = toEigen(s_ddot); + + // Run acceleration kinematics + ForwardAccKinematics(pimpl->m_robot_model, + pimpl->m_traversal, + pimpl->m_pos, + pimpl->m_vel, + pimpl->m_generalizedAccs, + pimpl->m_linkVel, + pimpl->m_linkAccs); + + // Convert the link body fixed kinematics to the required rappresentation + Transform frame_X_link = pimpl->m_robot_model.getFrameTransform(frameIdx).inverse(); + + SpatialAcc acc_frame_body_fixed = frame_X_link*pimpl->m_linkAccs(pimpl->m_robot_model.getFrameLink(frameIdx)); + Twist vel_frame_body_fixed = frame_X_link*pimpl->m_linkVel(pimpl->m_robot_model.getFrameLink(frameIdx)); + + // In body fixed and inertial representation, we can transform the bias acceleration with just a adjoint + if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION) + { + return acc_frame_body_fixed.asVector(); + } + else + { + // To convert the twist to a mixed or inertial representation, we need world_H_frame + Transform world_H_frame = getWorldTransform(frameIdx); + + if (pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ) + { + return (world_H_frame*acc_frame_body_fixed).asVector(); + } + else + { + // In the mixed case, we need to account for the non-vanishing term related to the + // derivative of the transform between mixed and body representation + assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION); + return convertBodyFixedAccelerationToMixedAcceleration(acc_frame_body_fixed, vel_frame_body_fixed, world_H_frame.getRotation()); + } + } + +} + bool KinDynComputations::getFrameFreeFloatingJacobian(const std::string& frameName, MatrixDynSize& outJacobian) @@ -1070,91 +1287,6 @@ Vector6 KinDynComputations::getFrameBiasAcc(const std::string & frameName) } -typedef Eigen::Matrix Matrix3dRowMajor; -/** - * Function to convert a body fixed acceleration to a mixed acceleration. - * - * TODO refactor in a more general handling of conversion between the three - * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors - * acceleration. - * - * @return The mixed acceleration - */ -Vector6 convertBodyFixedAccelerationToMixedAcceleration(const SpatialAcc & bodyFixedAcc, - const Twist & bodyFixedVel, - const Rotation & inertial_R_body) -{ - Vector6 mixedAcceleration; - - Eigen::Map linBodyFixedAcc(bodyFixedAcc.getLinearVec3().data()); - Eigen::Map angBodyFixedAcc(bodyFixedAcc.getAngularVec3().data()); - - Eigen::Map linBodyFixedTwist(bodyFixedVel.getLinearVec3().data()); - Eigen::Map angBodyFixedTwist(bodyFixedVel.getAngularVec3().data()); - - Eigen::Map linMixedAcc(mixedAcceleration.data()); - Eigen::Map angMixedAcc(mixedAcceleration.data()+3); - - Eigen::Map inertial_R_body_eig(inertial_R_body.data()); - - // First we account for the effect of linear/angular velocity - linMixedAcc = inertial_R_body_eig*(linBodyFixedAcc + angBodyFixedTwist.cross(linBodyFixedTwist)); - - // Angular acceleration can be copied - angMixedAcc = inertial_R_body_eig*angBodyFixedAcc; - - return mixedAcceleration; -} - -/** - * Function to convert mixed acceleration to body fixed acceleration - * - * TODO refactor in a more general handling of conversion between the three - * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors - * acceleration. - * - * @return The body fixed acceleration - */ -SpatialAcc convertMixedAccelerationToBodyFixedAcceleration(const Vector6 & mixedAcc, - const Twist & bodyFixedVel, - const Rotation & inertial_R_body) -{ - SpatialAcc bodyFixedAcc; - - Eigen::Map linMixedAcc(mixedAcc.data()); - Eigen::Map angMixedAcc(mixedAcc.data()+3); - - Eigen::Map linBodyFixedTwist(bodyFixedVel.getLinearVec3().data()); - Eigen::Map angBodyFixedTwist(bodyFixedVel.getAngularVec3().data()); - - Eigen::Map inertial_R_body_eig(inertial_R_body.data()); - - Eigen::Map linBodyFixedAcc(bodyFixedAcc.getLinearVec3().data()); - Eigen::Map angBodyFixedAcc(bodyFixedAcc.getAngularVec3().data()); - - linBodyFixedAcc = inertial_R_body_eig.transpose()*linMixedAcc - angBodyFixedTwist.cross(linBodyFixedTwist); - angBodyFixedAcc = inertial_R_body_eig.transpose()*angMixedAcc; - - return bodyFixedAcc; -} - -/** - * Function to convert inertial acceleration to body acceleration. - * - * TODO refactor in a more general handling of conversion between the three - * derivative of frame velocities (inertial, body-fixed, mixed) and the sensors - * acceleration. - * - * @return The body fixed acceleration - */ -SpatialAcc convertInertialAccelerationToBodyFixedAcceleration(const Vector6 & inertialAcc, - const Transform & inertial_H_body) -{ - SpatialAcc inertialAccProperForm; - fromEigen(inertialAccProperForm,toEigen(inertialAcc)); - return inertial_H_body.inverse()*inertialAccProperForm; -} - Vector6 KinDynComputations::getFrameBiasAcc(const FrameIndex frameIdx) { diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp index 0e443eee93d..1ec9ed605f3 100644 --- a/src/high-level/tests/KinDynComputationsUnitTest.cpp +++ b/src/high-level/tests/KinDynComputationsUnitTest.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -61,7 +62,7 @@ void setRandomState(iDynTree::KinDynComputations & dynComp) for(int i=0; i < 6; i++) { - baseVel(i) = i; //real_random_double(); + baseVel(i) = real_random_double(); } for(size_t dof=0; dof < dofs; dof++) @@ -303,12 +304,53 @@ void testRelativeJacobians(KinDynComputations & dynComp) // Dummy test: for now it just prints the frameBiasAcc, to check there is no // usage of not initialized memory -void testFrameBiasAcc(KinDynComputations & dynComp) +void testAbsoluteJacobiansAndFrameBiasAcc(KinDynComputations & dynComp) { FrameIndex frame = real_random_int(0, dynComp.getNrOfFrames()); - // Compute and print frameBiasAcc - std::cerr << "Computed frameBiasAcc " << dynComp.getFrameBiasAcc(frame).toString() << std::endl; + // Test Jacobian consistency + + // Get robot velocity + iDynTree::VectorDynSize nu(6+dynComp.getNrOfDegreesOfFreedom()); + dynComp.getModelVel(nu); + + // Compute frame velocity and jacobian + Twist frameVel = dynComp.getFrameVel(frame); + Vector6 frameVelJac; + FrameFreeFloatingJacobian jac(dynComp.model()); + dynComp.getFrameFreeFloatingJacobian(frame, jac); + toEigen(frameVelJac) = toEigen(jac)*toEigen(nu); + + ASSERT_EQUAL_VECTOR(frameVel.asVector(), frameVelJac); + + std::cerr << "nu: " << nu.toString() << std::endl; + + // Compute frame acceleration, and bias acceleration + Vector6 baseAcc; + baseAcc.zero(); + baseAcc(0) = 1; + baseAcc(1) = 0; + baseAcc(2) = 0; + baseAcc(3) = 0; + baseAcc(4) = 0; + baseAcc(5) = 0; + + VectorDynSize sddot(dynComp.model().getNrOfDOFs()); + for(int i=0; i < sddot.size(); i++) + { + sddot(i) = i*0.1; + } + + iDynTree::VectorDynSize nuDot(6+dynComp.getNrOfDegreesOfFreedom()); + toEigen(nuDot) = toEigen(baseAcc, sddot); + + + Vector6 frameAcc = dynComp.getFrameAcc(frame, baseAcc, sddot); + Vector6 biasAcc = dynComp.getFrameBiasAcc(frame); + Vector6 frameAccJac; + toEigen(frameAccJac) = toEigen(jac)*toEigen(nuDot) + toEigen(biasAcc); + + ASSERT_EQUAL_VECTOR(frameAcc, frameAccJac); } void testModelConsistency(std::string modelFilePath, const FrameVelocityRepresentation frameVelRepr) @@ -328,7 +370,7 @@ void testModelConsistency(std::string modelFilePath, const FrameVelocityRepresen testAverageVelocityAndTotalMomentumJacobian(dynComp); testInverseDynamics(dynComp); testRelativeJacobians(dynComp); - testFrameBiasAcc(dynComp); + testAbsoluteJacobiansAndFrameBiasAcc(dynComp); } } @@ -337,8 +379,11 @@ void testModelConsistencyAllRepresentations(std::string modelName) { std::string urdfFileName = getAbsModelPath(modelName); std::cout << "Testing file " << urdfFileName << std::endl; + std::cout << "Testing MIXED_REPRESENTATION " << urdfFileName << std::endl; testModelConsistency(urdfFileName,iDynTree::MIXED_REPRESENTATION); + std::cout << "Testing BODY_FIXED_REPRESENTATION " << urdfFileName << std::endl; testModelConsistency(urdfFileName,iDynTree::BODY_FIXED_REPRESENTATION); + std::cout << "Testing INERTIAL_FIXED_REPRESENTATION " << urdfFileName << std::endl; testModelConsistency(urdfFileName,iDynTree::INERTIAL_FIXED_REPRESENTATION); } diff --git a/src/model/include/iDynTree/Model/ForwardKinematics.h b/src/model/include/iDynTree/Model/ForwardKinematics.h index 75014de0812..7576db7cc55 100644 --- a/src/model/include/iDynTree/Model/ForwardKinematics.h +++ b/src/model/include/iDynTree/Model/ForwardKinematics.h @@ -22,6 +22,7 @@ namespace iDynTree class LinkVelArray; class LinkAccArray; class JointPosDoubleArray; + class SpatialAcc; class VectorDynSize; /** @@ -114,9 +115,36 @@ namespace iDynTree LinkAccArray & linkAcc); /** - * Function that computes the links bias accelerations - * given the free floating robot velocities.. + * Function that computes the links bias accelerations given the free floating robot velocities. * + * @note This function can also consider a non-zero base acceleration, because when computing + * the bias acc for the MIXED representation, a zero mixed base acceleration is a equivalent + * to a non-zero BODY_FIXED base acceleration. + * + * @param[in] model the used model, + * @param[in] traversal the used traversal, + * @param[in] worldHbase the world_H_base transform, + * @param[in] robotPos the position of the robot, i.e. \f$ ({}^A H_B, s)\f$, + * @param[in] robotVel the velocity of the robot, with the base velocity expressed in BODY_FIXED + * representation i.e. \f$ \nu = \begin{bmatrix} {}^B \mathrm{v}_{A,B} \newline \dot{s} \end{bmatrix} \f$, + * @param[in] baseBiasAcc base bias acceleration with BODY_FIXED rapresentation, useful when the bias acceleration + * is considering the MIXED base acceleration to be zero, + * @param[in] linkVel the velocity of each link of the robot, with velocity expressed with BODY_FIXED + * representation i.e. for each link \f$ \f$ we have \f$ {}^L \mathrm{v}_{A,L} \f$, + * @param[out] linkBiasAcc the bias acceleration of each link of the robot, with the acceleration expressed with BODY_FIXED + * representation. + * + */ + bool ForwardBiasAccKinematics(const Model & model, + const Traversal & traversal, + const FreeFloatingPos & robotPos, + const FreeFloatingVel & robotVel, + const SpatialAcc & baseBiasAcc, + const LinkVelArray & linkVel, + LinkAccArray & linkBiasAcc); + + /** + * Legacy function, will be deprecated, use the variant with an explicit baseBiasAcc value. */ bool ForwardBiasAccKinematics(const Model & model, const Traversal & traversal, diff --git a/src/model/src/ForwardKinematics.cpp b/src/model/src/ForwardKinematics.cpp index 796f770ba7a..a394be2e6d3 100644 --- a/src/model/src/ForwardKinematics.cpp +++ b/src/model/src/ForwardKinematics.cpp @@ -233,6 +233,7 @@ bool ForwardBiasAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos & robotPos, const FreeFloatingVel & robotVel, + const SpatialAcc& baseBiasAcc, const LinkVelArray & linkVel, LinkAccArray & linkBiasAcc) { @@ -247,8 +248,9 @@ bool ForwardBiasAccKinematics(const Model& model, if( parentLink == 0 ) { // If the visited link is the base, the base has no parent. - // In this case the bias acceleration is (by definition) zero - linkBiasAcc(visitedLink->getIndex()).zero(); + // In this case the bias acceleration is tipically zero, + // or in strange cases the one specified by the baseBiasAcc argument + linkBiasAcc(visitedLink->getIndex()) = baseBiasAcc; } else { @@ -265,4 +267,15 @@ bool ForwardBiasAccKinematics(const Model& model, return retValue; } +bool ForwardBiasAccKinematics(const Model& model, + const Traversal& traversal, + const FreeFloatingPos & robotPos, + const FreeFloatingVel & robotVel, + const LinkVelArray & linkVel, + LinkAccArray & linkBiasAcc) +{ + return ForwardBiasAccKinematics(model, traversal, robotPos, robotVel, + SpatialAcc::Zero(), linkVel, linkBiasAcc); +} + }