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]; }