static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model& model,
se3::Data& data,
const int &i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
using namespace Eigen;
using namespace se3;
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[(Model::Index)i];
data.liMi[(Model::Index)i] = model.jointPlacements[(Model::Index)i]*jdata.M();
data.v[(Model::Index)i] = jdata.v();
if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
data.a_gf[(Model::Index)i] = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
data.a_gf[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a_gf[parent]);
data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a_gf[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
}
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> &,
const se3::Model &,
se3::Data & data,
const Eigen::VectorXd &) // TODO: make joint limits depend explicitely on the current state (q,v)
{
using namespace Eigen;
using namespace se3;
// TODO: as limits are static, no need of q, nor computations
jmodel.jointVelocitySelector(data.effortLimit) = jmodel.maxEffortLimit();
jmodel.jointVelocitySelector(data.velocityLimit) = jmodel.maxVelocityLimit();
jmodel.jointConfigSelector(data.lowerPositionLimit) = jmodel.lowerPosLimit(); // if computation needed, do it here, or may be in lowerPosLimit
jmodel.jointConfigSelector(data.upperPositionLimit) = jmodel.upperPosLimit();
}
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointData> & jdata,
const se3::Model & model,
se3::Data & data,
const Model::Index i,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
data.v[i] = jdata.v();
data.liMi[i] = model.jointPlacements[i] * jdata.M();
if(parent>0)
{
data.oMi[i] = data.oMi[parent] * data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[parent]);
}
else
data.oMi[i] = data.liMi[i];
data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
static void algo(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
Eigen::VectorXd & result)
{
jmodel.jointVelocitySelector(result) = jmodel.difference(q0, q1);
}
