static void algo(const se3::JointModelBase<JointModel> & jmodel, Eigen::VectorXd & q, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits) { jmodel.jointConfigSelector(q) = jmodel.randomConfiguration(jmodel.jointConfigSelector(lowerLimits), jmodel.jointConfigSelector(upperLimits) ); }
static void algo(const se3::JointModelBase<JointModel> & jmodel, const Eigen::VectorXd & q0, const Eigen::VectorXd & q1, const double u, Eigen::VectorXd & result) { jmodel.jointConfigSelector(result) = jmodel.interpolate(q0, q1, u); }
static void algo(const se3::JointModelBase<JointModel> & jmodel, const Eigen::VectorXd & q, const Eigen::VectorXd & v, Eigen::VectorXd & result) { jmodel.jointConfigSelector(result) = jmodel.integrate(q, v); }
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(); }