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);
 }
예제 #4
0
    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();
    }