jrl-umi3218
diff --git a/‎src/RBDyn/Momentum.cpp‎
Lines changed: 14 additions & 11 deletions b/‎src/RBDyn/Momentum.cpp‎
Lines changed: 14 additions & 11 deletions
diff --git a/‎src/RBDyn/RBDyn/Momentum.h‎
Lines changed: 7 additions & 6 deletions b/‎src/RBDyn/RBDyn/Momentum.h‎
Lines changed: 7 additions & 6 deletions
@@ -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<Body> & 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)
 
@@ -22,21 +22,22 @@ using Blocks = std::vector<Block>;
 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);
 
 /**