From e63923967829d1a2fa8bea2eb13b6264ceae8bc5 Mon Sep 17 00:00:00 2001 From: Yuquan WANG Date: Mon, 24 Aug 2020 10:18:43 +0200 Subject: [PATCH] [Improvement] matrix inversion and the for-loop for computing centroidal inertia. * Improve the average velocity computation: ci.llt().solveInPlace(av) * Simplify the for loop computation with the intermediate term: X_com_i --- src/RBDyn/Momentum.cpp | 25 ++++++++++++++----------- src/RBDyn/RBDyn/Momentum.h | 13 +++++++------ 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/src/RBDyn/Momentum.cpp b/src/RBDyn/Momentum.cpp index d4c5ecdc..c63b888a 100644 --- a/src/RBDyn/Momentum.cpp +++ b/src/RBDyn/Momentum.cpp @@ -17,13 +17,13 @@ void computeCentroidalInertia(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com, Eigen::Matrix6d & ci, - Eigen::Vector6d & cm) + Eigen::Vector6d & av) { using namespace Eigen; const std::vector & bodies = mb.bodies(); - cm = Vector6d::Zero(); + av = Vector6d::Zero(); ci = Matrix6d::Identity(); @@ -33,14 +33,17 @@ void computeCentroidalInertia(const MultiBody & mb, // body momentum in body coordinate sva::ForceVecd hi = bodies[i].inertia() * mbc.bodyVelB[i]; - // momentum at CoM for link i : {}^iX_{com}^T {}^iI_i {}^iV_i - cm += (mbc.bodyPosW[i] * X_com_0).transMul(hi).vector(); - - // sum: X^T_com_i*I_i*X_com_i // X_com_i = X_i_0 * X_com_0 - ci += ((mbc.bodyPosW[i] * X_com_0).matrix().transpose()) * (bodies[i].inertia().matrix()) - * (mbc.bodyPosW[i] * X_com_0).matrix(); + auto X_com_i = mbc.bodyPosW[i] * X_com_0; + + // Momentum at CoM for link i : {}^iX_{com}^T {}^iI_i {}^iV_i + av += X_com_i.transMul(hi).vector(); + + // Sum: X^T_com_i*I_i*X_com_i + ci += X_com_i.matrix().transpose() * bodies[i].inertia().matrix() * X_com_i.matrix(); } + + ci.llt().solveInPlace(av); } void computeCentroidalInertia(const MultiBody & mb, @@ -50,9 +53,9 @@ void computeCentroidalInertia(const MultiBody & mb, Eigen::Vector6d & cm, Eigen::Vector6d & av) { - computeCentroidalInertia(mb, mbc, com, ci, cm); - // Compute the average velocity: inertia.inverse()*momentum - av = ci.llt().solve(cm); + computeCentroidalInertia(mb, mbc, com, ci, av); + // Compute the centroidal momentum := inertia * average velocity + cm = ci * av; } sva::ForceVecd computeCentroidalMomentum(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com) diff --git a/src/RBDyn/RBDyn/Momentum.h b/src/RBDyn/RBDyn/Momentum.h index 43794635..b982da92 100644 --- a/src/RBDyn/RBDyn/Momentum.h +++ b/src/RBDyn/RBDyn/Momentum.h @@ -22,21 +22,22 @@ using Blocks = std::vector; class Jacobian; /** - * Compute the centroidal inertia (ci), and centroidal momentum (cm) + * Compute the centroidal inertia (ci), centroidal momentum (cm), + * and the average velocity (av = ci^-1*cm) * at the CoM frame as describe in [Orin and Gosawami 2008]. * @param mb MultiBody used has model. * @param mbc Use bodyPosW, bodyVelB. * @param com CoM position. * @param ci Centroidal inertia at the Centroidal frame. * @param cm centroidal momentum at the Centroidal frame. - * @param av Average velocity is inverse(centroidalInertia)*centroidalMomentum + * @param av Average velocity is: av = ci.inverse()*cm */ RBDYN_DLLAPI void computeCentroidalInertia(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com, Eigen::Matrix6d & ci, - Eigen::Vector6d & cm); - + Eigen::Vector6d & cm, + Eigen::Vector6d & av); /** * Compute the centroidal inertia (ci), centroidal momentum (cm), * and the average velocity (av = ci^-1*cm) @@ -45,14 +46,14 @@ RBDYN_DLLAPI void computeCentroidalInertia(const MultiBody & mb, * @param mbc Use bodyPosW, bodyVelB. * @param com CoM position. * @param ci Centroidal inertia at the Centroidal frame. - * @param cm centroidal momentum at the Centroidal frame. * @param av Average velocity is inverse(centroidalInertia)*centroidalMomentum + * + * If the Centroidal Momentum is needed, we can easily compute it as: cm = ci*av. */ RBDYN_DLLAPI void computeCentroidalInertia(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com, Eigen::Matrix6d & ci, - Eigen::Vector6d & cm, Eigen::Vector6d & av); /**