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];
}
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();
}
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 std::size_t & i = jmodel.id();
const std::size_t & 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];
}