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> & 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, 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) { using namespace Eigen; using namespace se3; 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]; }
static void algo(const se3::JointModelBase<JointModel> & jmodel, se3::JointDataBase<typename JointModel::JointData> & jdata, const se3::Model & model, se3::Data & data, const size_t i, const Eigen::VectorXd & q, const Eigen::VectorXd & v) { using namespace Eigen; using namespace se3; jmodel.calc(jdata.derived(),q,v); const Model::Index & parent = model.parents[i]; data.liMi[i] = model.jointPlacements[i]*jdata.M(); data.v[i] = jdata.v(); if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[(size_t) parent]); data.a[i] = jdata.c() + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[(size_t) parent]); data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext }