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