jrl-umi3218 · wyqsnddd · Aug 17, 2020 · Aug 17, 2020 · Aug 19, 2020 · Aug 21, 2020
diff --git a/src/RBDyn/Momentum.cpp b/src/RBDyn/Momentum.cpp
@@ -13,6 +13,47 @@
 
 namespace rbd
 {
+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 = Vector6d::Zero();
+
+  ci = Matrix6d::Identity();
+
+  sva::PTransformd X_com_0(Vector3d(-com));
+  for(int i = 0; i < mb.nrBodies(); ++i)
+  {
+    // 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();
+  }
+}
+
+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.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
@@ -21,6 +21,40 @@ struct Block;
 using Blocks = std::vector<Block>;
 class Jacobian;
 
+/**
+ * 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 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
  * as describe in [Orin and Gosawami 2008].