[comment-fix] according to issue: #75.

* Replace matrix inverse with 'llt().solve(m)' * Function parameter overloading * Clean the comments
jrl-umi3218 · Aug 27, 2020 · 0737a19 · 0737a19
1 parent eaad080
commit 0737a19
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 19 deletions.
diff --git a/src/RBDyn/Momentum.cpp b/src/RBDyn/Momentum.cpp
@@ -13,19 +13,17 @@
 
 namespace rbd
 {
-void computeCentroidalInertiaAndVelocity(const MultiBody & mb,
-                                         const MultiBodyConfig & mbc,
-                                         const Eigen::Vector3d & com,
-                                         Eigen::Matrix6d & ci,
-                                         Eigen::Vector6d & cm,
-                                         Eigen::Vector6d & av)
+void computeCentroidalInertia(const MultiBody & mb,
+                              const MultiBodyConfig & mbc,
+                              const Eigen::Vector3d & com,
+                              Eigen::Matrix6d & ci,
+                              Eigen::Vector6d & cm)
 {
 
   using namespace Eigen;
 
   const std::vector<Body> & bodies = mb.bodies();
-  // cm Candidate
-  Vector6d cmc = Vector6d::Zero();
+  cm = Vector6d::Zero();
 
   ci = Matrix6d::Identity();
 
@@ -36,17 +34,25 @@ void computeCentroidalInertiaAndVelocity(const MultiBody & mb,
     sva::ForceVecd hi = bodies[i].inertia() * mbc.bodyVelB[i];
 
     // momentum at CoM for link i : {}^iX_{com}^T {}^iI_i {}^iV_i
-    cmc += (mbc.bodyPosW[i] * X_com_0).transMul(hi).vector();
+    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();
   }
+}
 
+void computeCentroidalInertia(const MultiBody & mb,
+                              const MultiBodyConfig & mbc,
+                              const Eigen::Vector3d & com,
+                              Eigen::Matrix6d & ci,
+                              Eigen::Vector6d & cm,
+                              Eigen::Vector6d & av)
+{
+  computeCentroidalInertia(mb, mbc, com, ci, cm);
   // Compute the average velocity: inertia.inverse()*momentum
-  av = ci.inverse() * cmc;
-  cm = cmc;
+  av = ci.llt().solve(cm);
 }
 
 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
@@ -22,21 +22,38 @@ using Blocks = std::vector<Block>;
 class Jacobian;
 
 /**
- * Compute the centroidal momentum at the CoM frame
- * as describe in [Orin and Gosawami 2008].
+ * Compute the centroidal inertia (ci), and centroidal momentum (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
  */
-RBDYN_DLLAPI void computeCentroidalInertiaAndVelocity(const MultiBody & mb,
-                                                      const MultiBodyConfig & mbc,
-                                                      const Eigen::Vector3d & com,
-                                                      Eigen::Matrix6d & ci,
-                                                      Eigen::Vector6d & cm,
-                                                      Eigen::Vector6d & av);
+RBDYN_DLLAPI void computeCentroidalInertia(const MultiBody & mb,
+                                           const MultiBodyConfig & mbc,
+                                           const Eigen::Vector3d & com,
+                                           Eigen::Matrix6d & ci,
+                                           Eigen::Vector6d & 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
+ */
+RBDYN_DLLAPI void computeCentroidalInertia(const MultiBody & mb,
+                                           const MultiBodyConfig & mbc,
+                                           const Eigen::Vector3d & com,
+                                           Eigen::Matrix6d & ci,
+                                           Eigen::Vector6d & cm,
+                                           Eigen::Vector6d & av);
 
 /**
  * Compute the centroidal momentum at the CoM frame