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,41 @@
 
 namespace rbd
 {
+void computeCentroidalInertiaAndVelocity(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 Candidate
+  Vector6d cmc = 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
+    cmc += (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();
+  }
+
+  // Compute the average velocity: inertia.inverse()*momentum
+  av = ci.inverse() * cmc;
+  cm = cmc;
+}
 
 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,23 @@ struct Block;
 using Blocks = std::vector<Block>;
 class Jacobian;
 
+/**
+ * Compute the centroidal momentum 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);
+
 /**
  * Compute the centroidal momentum at the CoM frame
  * as describe in [Orin and Gosawami 2008].