diff --git a/CMakeLists.txt b/CMakeLists.txt
index d6e5ff92f90119f6feca6ddf1f95065bf04beb8b..5ef5a85e083f1ba5443b4363cc15bf61aa4034c3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,6 +59,7 @@ SET(${PROJECT_NAME}_HEADERS
algorithm/jacobian.hpp
algorithm/cholesky.hpp
algorithm/kinematics.hpp
+ algorithm/center-of-mass.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -111,6 +112,9 @@ PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
ADD_EXECUTABLE(jacobian EXCLUDE_FROM_ALL unittest/jacobian.cpp)
PKG_CONFIG_USE_DEPENDENCY(jacobian eigenpy)
+ADD_EXECUTABLE(com EXCLUDE_FROM_ALL unittest/com.cpp)
+PKG_CONFIG_USE_DEPENDENCY(com eigenpy)
+
ADD_EXECUTABLE(cholesky EXCLUDE_FROM_ALL unittest/cholesky.cpp)
PKG_CONFIG_USE_DEPENDENCY(cholesky eigenpy)
PKG_CONFIG_USE_DEPENDENCY(cholesky urdfdom)
diff --git a/src/algorithm/center-of-mass.hpp b/src/algorithm/center-of-mass.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..41a1a1831e9bf43854c38a84ead5d505e1cb5db9
--- /dev/null
+++ b/src/algorithm/center-of-mass.hpp
@@ -0,0 +1,207 @@
+#ifndef __se3_center_of_mass_hpp__
+#define __se3_center_of_mass_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include <iostream>
+
+namespace se3
+{
+ // inline const Eigen::MatrixXd&
+ // centerOfMass(const Model & model,
+ // Data& data,
+ // const Eigen::VectorXd & q);
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+
+ struct CenterOfMassForwardStep : public fusion::JointVisitor<CenterOfMassForwardStep>
+ {
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const Eigen::VectorXd &,
+ const bool &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(CenterOfMassForwardStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const Eigen::VectorXd & q,
+ const bool & computeSubtreeComs)
+ {
+ using namespace Eigen;
+ using namespace se3;
+
+ const Model::Index & i = jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ jmodel.calc(jdata.derived(),q);
+
+ data.liMi[i] = model.jointPlacements[i]*jdata.M();
+ data.com[parent] += (data.liMi[i].rotation()*data.com[i]
+ +data.mass[i]*data.liMi[i].translation());
+ data.mass[parent] += data.mass[i];
+
+ if( computeSubtreeComs )
+ data.com[i] /= data.mass[i];
+ }
+
+ };
+
+ /* Compute the centerOfMass in the local frame. */
+ const Eigen::Vector3d &
+ centerOfMass(const Model & model, Data& data,
+ const Eigen::VectorXd & q,
+ bool computeSubtreeComs = true)
+ {
+ data.mass[0] = 0;
+ data.com[0] = Eigen::Vector3d::Zero();
+
+ for( int i=1;i<model.nbody;++i )
+ {
+ data.com[i] = model.inertias[i].mass()*model.inertias[i].lever();
+ data.mass[i] = model.inertias[i].mass();
+ }
+
+ for( int i=model.nbody-1;i>0;--i )
+ {
+ CenterOfMassForwardStep
+ ::run(model.joints[i],data.joints[i],
+ CenterOfMassForwardStep::ArgsType(model,data,q,computeSubtreeComs));
+ }
+ data.com[0] /= data.mass[0];
+
+ return data.com[0];
+ }
+
+ const Eigen::Vector3d & getComFromCrba(const Model & , Data& data)
+ {
+ return data.Ycrb[1].lever();
+ }
+
+ /* --- JACOBIAN ---------------------------------------------------------- */
+ /* --- JACOBIAN ---------------------------------------------------------- */
+ /* --- JACOBIAN ---------------------------------------------------------- */
+ struct JacobianCenterOfMassForwardStep
+ : public fusion::JointVisitor<JacobianCenterOfMassForwardStep>
+ {
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const Eigen::VectorXd &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(JacobianCenterOfMassForwardStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const Eigen::VectorXd & q)
+ {
+ using namespace Eigen;
+ using namespace se3;
+
+ const Model::Index & i = jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ jmodel.calc(jdata.derived(),q);
+
+ data.liMi[i] = model.jointPlacements[i]*jdata.M();
+ if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
+ else data.oMi[i] = data.liMi[i];
+
+ data.com[i] = model.inertias[i].mass()*data.oMi[i].act(model.inertias[i].lever());
+ data.mass[i] = model.inertias[i].mass();
+ }
+
+ };
+
+ struct JacobianCenterOfMassBackwardStep
+ : public fusion::JointVisitor<JacobianCenterOfMassBackwardStep>
+ {
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const bool &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(JacobianCenterOfMassBackwardStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const bool & computeSubtreeComs )
+ {
+ using namespace Eigen;
+ using namespace se3;
+
+ const Model::Index & i = jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ data.com[parent] += data.com[i];
+ data.mass[parent] += data.mass[i];
+
+ const Eigen::Matrix<double,6,JointModel::NV> & oSk
+ = data.oMi[i].act(jdata.S());
+
+ if( JointModel::NV==1 )
+ data.Jcom.col(jmodel.idx_v()) // Using head and tail would confuse g++
+ = data.mass[i]*oSk.template topLeftCorner<3,1>()
+ - data.com[i].cross(oSk.template bottomLeftCorner<3,1>()) ;
+ else
+ data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
+ = data.mass[i]*oSk.template topRows<3>()
+ - skew(data.com[i]) * oSk.template bottomRows<3>() ;
+
+ if(computeSubtreeComs)
+ data.com[i] /= data.mass[i];
+ }
+
+ };
+
+ /* Compute the centerOfMass in the local frame. */
+ const Eigen::Vector3d &
+ jacobianCenterOfMass(const Model & model, Data& data,
+ const Eigen::VectorXd & q,
+ const bool & computeSubtreeComs = true)
+ {
+ data.com[0] = Eigen::Vector3d::Zero();
+ data.mass[0] = 0;
+ for( int i=1;i<model.nbody;++i )
+ {
+ JacobianCenterOfMassForwardStep
+ ::run(model.joints[i],data.joints[i],
+ JacobianCenterOfMassForwardStep::ArgsType(model,data,q));
+ }
+ for( int i=model.nbody-1;i>0;--i )
+ {
+ JacobianCenterOfMassBackwardStep
+ ::run(model.joints[i],data.joints[i],
+ JacobianCenterOfMassBackwardStep::ArgsType(model,data,computeSubtreeComs));
+ }
+
+ data.com[0] /= data.mass[0];
+ data.Jcom /= data.mass[0];
+ return data.com[0];
+ }
+
+ const Eigen::Vector3d & getJacobianComFromCrba(const Model & , Data& data)
+ {
+ return data.Jcom = data.M.topRows<3>()/data.M(0,0);
+ }
+
+
+
+} // namespace se3
+
+#endif // ifndef __se3_center_of_mass_hpp__
+
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index d6d3eb12e3730ae6f4a184dd2fb1ecca196e6c8b..ea09740b82d6425e0f4edf0125b960555cd34c6a 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -90,6 +90,11 @@ namespace se3
std::vector<int> nvSubtree_fromRow; //
Eigen::MatrixXd J; // Jacobian of joint placement
+ std::vector<SE3> iMf; // Body placement wrt to algorithm end effector.
+
+ std::vector<Eigen::Vector3d> com; // Subtree com position.
+ std::vector<double> mass; // Subtree total mass.
+ Eigen::Matrix<double,3,Eigen::Dynamic> Jcom; // Jacobian of center of mass.
Data( const Model& ref );
@@ -195,6 +200,10 @@ namespace se3
,parents_fromRow(ref.nv)
,nvSubtree_fromRow(ref.nv)
,J(6,ref.nv)
+ ,iMf(ref.nbody)
+ ,com(ref.nbody)
+ ,mass(ref.nbody)
+ ,Jcom(3,ref.nv)
{
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
diff --git a/unittest/com.cpp b/unittest/com.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..79e0e801efbee8ea4a630f8cbde11aef031ac48f
--- /dev/null
+++ b/unittest/com.cpp
@@ -0,0 +1,108 @@
+#ifdef NDEBUG
+#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
+#endif
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/tools/timer.hpp"
+#include "pinocchio/multibody/parser/sample-models.hpp"
+
+#include <iostream>
+#include <boost/utility/binary.hpp>
+
+void timings(const se3::Model & model, se3::Data& data, long flag)
+{
+ using namespace se3;
+ StackTicToc timer(StackTicToc::US);
+#ifdef NDEBUG
+ const int NBT = 1000*1000;
+#else
+ const int NBT = 1;
+#endif
+
+ bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
+ if(verbose) std::cout <<"--" << std::endl;
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+
+ if( flag >> 0 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ centerOfMass(model,data,q);
+ }
+ if(verbose) std::cout << "COM =\t";
+ timer.toc(std::cout,NBT);
+ }
+ if( flag >> 1 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ centerOfMass(model,data,q,false);
+ }
+ if(verbose) std::cout << "Wo stree =\t";
+ timer.toc(std::cout,NBT);
+ }
+ if( flag >> 2 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ jacobianCenterOfMass(model,data,q);
+ }
+ if(verbose) std::cout << "Jcom =\t";
+ timer.toc(std::cout,NBT);
+ }
+}
+
+void assertValues(const se3::Model & model, se3::Data& data)
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ VectorXd q = VectorXd::Zero(model.nq);
+ crba(model,data,q);
+
+ { /* Test COM against CRBA*/
+ centerOfMass(model,data,q);
+ assert( data.com[0].isApprox( getComFromCrba(model,data) ));
+
+ }
+
+ { /* Test COM against Jcom (both use different way of compute the COM. */
+ Vector3d com = data.com[0];
+ data.M.fill(0); crba(model,data,q);
+ jacobianCenterOfMass(model,data,q);
+ assert(com.isApprox(jacobianCenterOfMass(model,data,q)));
+ }
+
+ { /* Test Jcom against CRBA */
+ assert(data.Jcom.isApprox(data.M.block(0,0,3,model.nv)/data.mass[0] ));
+ }
+
+ std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
+ std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
+ std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
+ std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
+}
+
+int main()
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ se3::Model model;
+ se3::buildModels::humanoidSimple(model);
+ se3::Data data(model);
+
+#ifndef NDEBUG
+ assertValues(model,data);
+#else
+ timings(model,data,BOOST_BINARY(101));
+#endif
+
+ return 0;
+}