Esempio n. 1
0
 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]);
 }
Esempio n. 2
0
    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 bool & computeSubtreeComs )
    {
      using namespace Eigen;
      using namespace se3;

      const Model::Index & i      = jmodel.id();
      const Model::Index & parent = model.parents[i];

      data.com[parent]  += data.com[i];
      data.mass[parent] += data.mass[i];

      const Eigen::Matrix<double,6,JointModel::NV> & oSk
	= data.oMi[i].act(jdata.S());

      if( JointModel::NV==1 )
      	data.Jcom.col(jmodel.idx_v()) // Using head and tail would confuse g++
      	  = data.mass[i]*oSk.template topLeftCorner<3,1>() 
      	  - data.com[i].cross(oSk.template bottomLeftCorner<3,1>()) ;
      else
      	data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
      	  = data.mass[i]*oSk.template topRows<3>() 
      	  - skew(data.com[i]) * oSk.template bottomRows<3>() ;

      if(computeSubtreeComs)
	data.com[i]       /= data.mass[i];
    }
 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);
 }
 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,
                  Eigen::VectorXd & q,
                  const Eigen::VectorXd & lowerLimits,
                  const Eigen::VectorXd & upperLimits) 
 {
   jmodel.jointConfigSelector(q) = jmodel.randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
                                                               jmodel.jointConfigSelector(upperLimits)
                                                               );
 }
Esempio n. 8
0
    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,
                  const Model::JointIndex i,
                  const Eigen::VectorXd & q0,
                  const Eigen::VectorXd & q1,
                  Eigen::VectorXd & distances) 
 {
   distances[(long)i] = jmodel.distance(q0, q1);
 }
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		     se3::JointDataBase<typename JointModel::JointData> & jdata,
		     const se3::Model& model,
		     se3::Data& data,
		     const Eigen::VectorXd & q)
    {
      using namespace Eigen;
      using namespace se3;

      const Model::Index & i      = jmodel.id();
      const Model::Index & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i]      = model.jointPlacements[i]*jdata.M();
      if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
      else         data.oMi[i] = data.liMi[i];
      
      data.com[i]   = model.inertias[i].mass()*data.oMi[i].act(model.inertias[i].lever());
      data.mass[i]  = model.inertias[i].mass();
    }
    static void algo(const se3::JointModelBase<JointModel> & jmodel,
		     se3::JointDataBase<typename JointModel::JointData> & jdata,
		     const se3::Model& model,
		     se3::Data& data,
		     const Eigen::VectorXd & q,
		     const bool & computeSubtreeComs)
    {
      using namespace Eigen;
      using namespace se3;

      const std::size_t & i      = jmodel.id();
      const std::size_t & parent = model.parents[i];

      jmodel.calc(jdata.derived(),q);
      
      data.liMi[i]      = model.jointPlacements[i]*jdata.M();
      data.com[parent]  += (data.liMi[i].rotation()*data.com[i]
			    +data.mass[i]*data.liMi[i].translation());
      data.mass[parent] += data.mass[i];  

      if( computeSubtreeComs )
	data.com[i] /= data.mass[i]; 
    }
Esempio n. 12
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();
    }
    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
    }