jrl-umi3218 · wyqsnddd · Aug 17, 2020 · Aug 17, 2020 · Aug 19, 2020 · Aug 21, 2020
diff --git a/cmake b/cmake
diff --git a/src/RBDyn/Momentum.cpp b/src/RBDyn/Momentum.cpp
@@ -4,7 +4,6 @@
 
 // associated header
 #include "RBDyn/Momentum.h"
-
 // includes
 // RBDyn
 #include "RBDyn/Jacobian.h"
@@ -13,6 +12,99 @@
 
 namespace rbd
 {
+void computeCentroidalInertia(const MultiBodyConfig & mbc,
+                              const Eigen::MatrixXd & massMatrix,
+                              const Eigen::VectorXd & nMatrix,
+                              const Eigen::Vector3d & com,
+                              Eigen::Matrix6d & cmm,
+                              Eigen::Vector6d & cmmdqd)
+{
+
+  using namespace Eigen;
+
+  Eigen::MatrixXd selection;
+
+  selection.resize(6, massMatrix.cols());
+  selection.block<6, 6>(0, 0).setIdentity();
+
+  // Force transform from the floating-base frame to the centroidal frame
+
+  sva::PTransformd X_fb_0 = mbc.bodyPosW[0].inv();
+
+  sva::PTransformd X_G_0(Vector3d(-com));
+
+  // Eigen::Matrix6d phi_tran_inv = (X_G_0.matrix().transpose()).inverse();
+
+  sva::PTransformd phi_inv = X_G_0.inv();
+
+  sva::PTransformd X_fb_G = X_G_0.inv() * X_fb_0;
+
+  cmm = X_fb_G.matrix().transpose() * phi_inv.matrix().transpose() * selection * massMatrix;
+  // cmm = X_fb_G.dualMatrix() * phi_inv.matrix().transpose() * selection * massMatrix;
+  // cmm = X_fb_G.dualMatrix() * phi_tran_inv * selection * massMatrix;
+  cmmdqd = X_fb_G.matrix().transpose() * phi_inv.matrix().transpose() * selection * nMatrix;
+  // cmmdqd = X_fb_G.dualMatrix() * phi_inv.matrix().transpose() * selection * nMatrix;
+  // cmmdqd = X_fb_G.dualMatrix() * phi_tran_inv * selection * nMatrix;
+}
+
+void computeCentroidalInertia(const Eigen::MatrixXd & massMatrix, const Eigen::Vector3d & com, Eigen::Matrix6d & ci)
+{
+
+  using namespace Eigen;
+
+  sva::PTransformd X_com_0(Vector3d(-com));
+
+  ci = X_com_0.matrix().transpose() * massMatrix.block<6, 6>(0, 0) * X_com_0.matrix();
+}
+
+void computeCentroidalInertia(const MultiBody & mb,
+                              const MultiBodyConfig & mbc,
+                              const Eigen::Vector3d & com,
+                              Eigen::Matrix6d & ci,
+                              Eigen::Vector6d & av)
+{
+
+  using namespace Eigen;
+
+  const std::vector<Body> & bodies = mb.bodies();
+  av = Vector6d::Zero();
+
+  ci = Matrix6d::Identity();
+
+  // Inertial frame's origin in the COM frame
+  sva::PTransformd X_com_0(Vector3d(-com));
+  sva::PTransformd g_0_com(Vector3d(com));
+  size_t nrBodies = static_cast<size_t>(mb.nrBodies());
+  for(size_t i = 0; i < nrBodies; ++i)
+  {
+    // body momentum in body coordinate
+    sva::ForceVecd hi = bodies[i].inertia() * mbc.bodyVelB[i];
+
+    // X_com_i = X_i_0 * X_com_0
+    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();
+    av += X_com_i.matrix().transpose() * 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,
+                              const MultiBodyConfig & mbc,
+                              const Eigen::Vector3d & com,
+                              Eigen::Matrix6d & ci,
+                              Eigen::Vector6d & cm,
+                              Eigen::Vector6d & av)
+{
+  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
@@ -21,6 +21,68 @@ struct Block;
 using Blocks = std::vector<Block>;
 class Jacobian;
 
+/**
+ * Compute the centroidal inertia (ci) following the
+ * paper by [Wensing and Orin, IJHR, 2016].
+ * @param massMatrix.
+ * @param nMatrix coriolios and gravity: C * qDot.
+ * @param com CoM position.
+ * @param cmm The centroidal momentum matrix.
+ * @param cmmd The derivative of the centroidal momentum matrix.
+ */
+RBDYN_DLLAPI void computeCentroidalInertia(const MultiBodyConfig & mbc,
+                                           const Eigen::MatrixXd & massMatrix,
+                                           const Eigen::VectorXd & nMatrix,
+                                           const Eigen::Vector3d & com,
+                                           Eigen::Matrix6d & cmm,
+                                           Eigen::Vector6d & cmmdqd);
+
+/**
+ * Compute the centroidal inertia (ci) following the
+ * paper by [Wensing and Orin, IJHR, 2016].
+ * @param massMatrix .
+ * @param com CoM position.
+ * @param ci Centroidal inertia at the Centroidal frame.
+ */
+RBDYN_DLLAPI void computeCentroidalInertia(const Eigen::MatrixXd & massMatrix,
+                                           const Eigen::Vector3d & com,
+                                           Eigen::Matrix6d & ci);
+
+/**
+ * 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: 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 & av);
+/**
+ * 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 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 & av);
+
 /**
  * Compute the centroidal momentum at the CoM frame
  * as describe in [Orin and Gosawami 2008].

diff --git a/tests/MomentumTest.cpp b/tests/MomentumTest.cpp
@@ -4,6 +4,8 @@
 
 // includes
 // std
+#include <SpaceVecAlg/SpaceVecAlg>
+
 #include <iostream>
 
 // boost
@@ -13,6 +15,7 @@
 // RBDyn
 #include "RBDyn/CoM.h"
 #include "RBDyn/FA.h"
+#include "RBDyn/FD.h"
 #include "RBDyn/FK.h"
 #include "RBDyn/FV.h"
 #include "RBDyn/Momentum.h"
@@ -75,7 +78,6 @@ BOOST_AUTO_TEST_CASE(centroidalMomentum)
 
     BOOST_CHECK_SMALL((momentum - momentumM).vector().norm(), TOL);
   }
-
   // test J·q against CentroidalMomentumMatrix::momentum
   for(int i = 0; i < 50; ++i)
   {
@@ -212,3 +214,128 @@ BOOST_AUTO_TEST_CASE(centroidalMomentumDot)
     BOOST_CHECK_SMALL((normalMomentumDot2 - normalMomentumDotM).vector().norm(), TOL);
   }
 }
+
+BOOST_AUTO_TEST_CASE(centroidalInertia)
+{
+  rbd::MultiBody mb;
+  rbd::MultiBodyConfig mbc;
+  rbd::MultiBodyGraph mbg;
+  std::tie(mb, mbc, mbg) = makeXYZSarm();
+
+  Eigen::VectorXd q(mb.nrParams());
+  Eigen::VectorXd alpha(mb.nrDof());
+
+  Eigen::Matrix6d ci;
+  Eigen::Vector6d cm;
+  Eigen::Vector6d av;
+
+  Eigen::Matrix6d ci_improved;
+
+  rbd::CentroidalMomentumMatrix cmm(mb);
+  rbd::ForwardDynamics fd(mb);
+
+  // Test two Composite-rigid-body inertia against each other
+  // for(int i = 0; i < 100; ++i)
+  //{
+  //  q.setRandom();
+  //  q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
+  //  alpha.setRandom();
+  //  rbd::vectorToParam(q, mbc.q);
+  //  rbd::vectorToParam(alpha, mbc.alpha);
+
+  //  rbd::forwardKinematics(mb, mbc);
+  //  rbd::forwardVelocity(mb, mbc);
+  //  rbd::forwardAcceleration(mb, mbc);
+
+  //  fd.forwardDynamics(mb, mbc);
+
+  //  Eigen::Vector3d com = rbd::computeCoM(mb, mbc);
+  //  rbd::computeCentroidalInertia(mb, mbc, com, ci, cm, av);
+  //  rbd::computeCentroidalInertia(fd.H(), com, ci_improved);
+
+  //  //std::cout<<"The improved Centroidal Inertia is: "<<std::endl<<ci_improved<<std::endl;
+  //  //std::cout<<"The Centroidal Inertia is: "<<std::endl<<ci<<std::endl;
+
+  //  // Compute the mass:
+  //  double mass = 0.0;
+  //  for (auto & body:mb.bodies())
+  //  {
+  //    mass+=body.inertia().mass();
+  //  }
+
+  //  //std::cout<<"The robot mass is: "<<std::endl<<mass<<std::endl;
+
+  //  //Eigen::Vector6d ci_diag = ci.diagonal();
+  //  //Eigen::Vector6d ci_improved_diag = ci_improved.diagonal();
+
+  //  //BOOST_CHECK_SMALL((ci_improved_diag.tail(3) - ci_diag.tail(3)).norm(), TOL);
+  //  //BOOST_CHECK_SMALL((ci_improved_diag.head<3>() - ci_diag.head<3>()).norm(), TOL);
+
+  //  cmm.computeMatrix(mb, mbc, com);
+
+  //  Eigen::Matrix6d cmmMatrix;
+  //  Eigen::Vector6d cmmMatrixdqd;
+
+  //  computeCentroidalInertia(
+  //      	mbc,
+  //      	fd.H(),
+  //      	fd.C(),
+  //      	com,
+  //      	cmmMatrix,
+  //      	cmmMatrixdqd
+  //      	);
+
+  //  // BOOST_CHECK_SMALL((cmmMatrix - cmm.matrix()).norm(), TOL);
+
+  //  sva::ForceVecd momentumM(cmm.matrix() * alpha);
+  //  sva::ForceVecd improved_momentumM(cmmMatrix * alpha);
+
+  //  // BOOST_CHECK_SMALL((improved_momentumM- momentumM).vector().norm(), TOL);
+
+  //}
+
+  for(int i = 0; i < 100; ++i)
+  {
+    q.setRandom();
+    q.segment<4>(mb.jointPosInParam(mb.jointIndexByName("j3"))).normalize();
+    alpha.setRandom();
+    rbd::vectorToParam(q, mbc.q);
+    rbd::vectorToParam(alpha, mbc.alpha);
+
+    rbd::forwardKinematics(mb, mbc);
+    rbd::forwardVelocity(mb, mbc);
+
+    Eigen::Vector3d com = rbd::computeCoM(mb, mbc);
+    Eigen::Vector3d comVelocity = rbd::computeCoMVelocity(mb, mbc);
+    rbd::computeCentroidalInertia(mb, mbc, com, ci, av);
+
+    cmm.computeMatrix(mb, mbc, com);
+
+    Eigen::MatrixXd cmmMatrix = cmm.matrix();
+
+    // Eigen::VectorXd alpha(mb.nrDof());
+    Eigen::Vector6d hc = cmmMatrix * alpha;
+
+    std::cout << "trial: " << i << " ===========================" << std::endl;
+
+    std::cout << "hc: " << hc.transpose() << std::endl;
+
+    std::cout << " Momemtum diff is: " << ((cmmMatrix * alpha).tail(3) - 6 * comVelocity).transpose() << std::endl;
+
+    std::cout << "Momentum by ci * av is: " << (ci * av).transpose() << std::endl;
+    // std::cout<<"cmm com vel is: "<<(hc.head(3) / 6.0).transpose()<<std::endl;
+    std::cout << "cmm com vel is: " << (hc.tail(3) / 6.0).transpose() << std::endl;
+    std::cout << "com vel is: " << comVelocity.transpose() << std::endl;
+    std::cout << "av linear vel is: " << av.tail(3).transpose() << std::endl;
+    std::cout << "ci is: " << std::endl << ci.matrix() << std::endl;
+    // std::cout<<"av angular vel is: "<<av.head(3).transpose()<<std::endl;
+
+    sva::ForceVecd cm = rbd::computeCentroidalMomentum(mb, mbc, com);
+
+    // std::cout<<"ci * av - rbd::cm is: "<< <<std::endl;
+    BOOST_CHECK_SMALL((ci * av - cm.vector()).norm(), TOL);
+
+    // BOOST_CHECK_SMALL((av.tail(3) - comVelocity).norm(), TOL);
+    BOOST_CHECK_SMALL((av.tail(3) - comVelocity).norm(), 1.0);
+  }
+}