Exemple #1
0
static inline void MersenneReload(state_t *state)
{
  state_t *s = state;
  int j;

  for( j = MERSENNE_N - MERSENNE_M + 1; --j; ++s )
    *s = s[MERSENNE_M] ^ Twist(s[0], s[1]);
  for( j = MERSENNE_M; --j; ++s )
    *s = s[MERSENNE_M - MERSENNE_N] ^ Twist(s[0], s[1]);
  *s = s[MERSENNE_M - MERSENNE_N] ^ Twist(s[0], state[0]);
}
    ChainIdSolver_RNE_FB::ChainIdSolver_RNE_FB(const Chain& chain_,Vector grav):
        chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
        X(ns),S(ns),v(ns),a(ns),f(ns)
    {
        ag=-Twist(grav,Vector::Zero());
        assert(v.size() == ns);

    }
bool MissionControlSpeedFilter::advance(double dt) {
  const double maxHeadingVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().x();
  filteredVelocities_[0].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().x());
  double headingVel = filteredVelocities_[0].val();
//  boundToRange(&headingVel, -maxHeadingVel, maxHeadingVel);
  headingVel = mapInRange(headingVel, -1.0, 1.0, -maxHeadingVel, maxHeadingVel);

  const double maxLateralVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().y();
  filteredVelocities_[1].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().y());

  double lateralVel = filteredVelocities_[1].val();
//  boundToRange(&lateralVel, -maxLateralVel, maxLateralVel);
  lateralVel = mapInRange(lateralVel, -1.0, 1.0, -maxLateralVel, maxLateralVel);


  LinearVelocity linearVelocity(headingVel, lateralVel, 0.0);

  const double maxTurningVel = maximumBaseTwistInHeadingFrame_.getRotationalVelocity().z();
  filteredVelocities_[2].update(unfilteredBaseTwistInHeadingFrame_.getRotationalVelocity().z());
  double turningVel = filteredVelocities_[2].val();
  //boundToRange(&turningVel, -maxTurningVel, maxTurningVel);
  turningVel = mapInRange(turningVel, -1.0, 1.0, -maxTurningVel, maxTurningVel);

  LocalAngularVelocity angularVelocity(0.0, 0.0, turningVel);

  baseTwistInHeadingFrame_ = Twist(linearVelocity, angularVelocity);


  /* -------- Position -------- */


//  desiredPositionOffsetInWorldFrame_.z() = 0.5*(maxHeight-minHeight)*(joyStick->getVertical()-minHeight) + maxHeight;
  boundToRange(&desiredPositionOffsetInWorldFrame_.x(), minimalDesiredPositionOffsetInWorldFrame_.x(), maximalDesiredPositionOffsetInWorldFrame_.x());
  boundToRange(&desiredPositionOffsetInWorldFrame_.y(), minimalDesiredPositionOffsetInWorldFrame_.y(), maximalDesiredPositionOffsetInWorldFrame_.y());
  boundToRange(&desiredPositionOffsetInWorldFrame_.z(), minimalDesiredPositionOffsetInWorldFrame_.z(), maximalDesiredPositionOffsetInWorldFrame_.z());

  /* -------- Orientation -------- */

  EulerAnglesZyx desEulerAnglesZyx(desiredOrientationHeadingToBase_);
  desEulerAnglesZyx.setUnique();
  EulerAnglesZyx minEulerAnglesZyx(minimalOrientationHeadingToBase_);
  minEulerAnglesZyx.setUnique();
  EulerAnglesZyx maxEulerAnglesZyx(maximalOrientationHeadingToBase_);
  maxEulerAnglesZyx.setUnique();
  double value = desEulerAnglesZyx.roll();
  boundToRange(&value, minEulerAnglesZyx.roll(), maxEulerAnglesZyx.roll());
  desEulerAnglesZyx.setRoll(value);
  value = desEulerAnglesZyx.pitch();
  boundToRange(&value, minEulerAnglesZyx.pitch(), maxEulerAnglesZyx.pitch());
  desEulerAnglesZyx.setPitch(value);
  value = desEulerAnglesZyx.yaw();
  boundToRange(&value, minEulerAnglesZyx.yaw(), maxEulerAnglesZyx.yaw());
  desEulerAnglesZyx.setYaw(value);
  desiredOrientationHeadingToBase_(desEulerAnglesZyx.getUnique());

  return true;
}
 TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree, Vector grav, const TreeSerialization & serialization)
 : UndirectedTreeSolver(tree,serialization)
 {
   
     //Initializing gravitational acceleration (if any)
     ag=-Twist(grav,Vector::Zero());
     
     //allocate vectors
     v.resize(tree.getNrOfSegments(),Twist::Zero());
     a.resize(tree.getNrOfSegments(),Twist::Zero());
     f.resize(tree.getNrOfSegments(),Wrench::Zero());
     
 }
 ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
         chain(_chain),
         nr(0),
         nj(chain.getNrOfJoints()),
         ns(chain.getNrOfSegments()),
         grav(_grav),
         jntarraynull(nj),
         chainidsolver_coriolis( chain, Vector::Zero()),
         chainidsolver_gravity( chain, grav),
         wrenchnull(ns,Wrench::Zero()),
         X(ns),
         S(ns),
         Ic(ns)
 {
     ag=-Twist(grav,Vector::Zero());
 }
    TreeInertialParameters::TreeInertialParameters(Tree & tree,Vector grav,const TreeSerialization & serialization) : ref_tree(tree), 
                                                                      nj(ref_tree.getNrOfJoints()),
                                                                      ns(ref_tree.getNrOfSegments()),
                                                                      X(ns),
                                                                      S(ns),
                                                                      v(ns),
                                                                      a(ns),
                                                                      f(ns),
                                                                      X_b(ns),
                                                                      serial(serialization),
                                                                      indicator_function(ns)
    {
        //assert(X.size() == ns);
        assert(S.size() == ns);
        //should compile only in debug mode
        if(!serial.is_consistent(ref_tree)) {
            serial = TreeSerialization(ref_tree);
        }
        
        //Get root name
		root_name = tree.getRootSegment()->first;
		
        //Initializing gravitational acceleration (if any)
        ag=-Twist(grav,Vector::Zero());
        
        //the deprecated method is more efficient
        const SegmentMap& sm = tree.getSegments();
        
        mu_root.resize(0);
        mu.resize(ns,std::vector<unsigned int>(0));
        lambda.resize(ns);
        
        link2joint.resize(tree.getNrOfSegments(),tree.getNrOfSegments());
                
        seg_vector.resize(tree.getNrOfSegments());
        
        
        //create necessary vectors
        SegmentMap::const_iterator root, i;
        
        tree.getRootSegment(root);
        for( unsigned int j=0; j < root->second.children.size(); j++ ) {
            mu_root.push_back(serial.getLinkId(root->second.children[j]->first));
        }
        
        for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ) {
            if( i != root ) {
                unsigned int i_index = serial.getLinkId(i->first);
                seg_vector[i_index] = i;
                
                for( unsigned int j=0; j < i->second.children.size(); j++ ) {
                    mu[i_index].push_back(serial.getLinkId(i->second.children[j]->first));
                }
                
                if( i->second.segment.getJoint().getType() != Joint::None ) {
                    link2joint[i_index] = serial.getJointId(i->first);
                }
                
                if( i->second.parent == root ) {
                    lambda[i_index] = -1;
                } else {
                    lambda[i_index] = serial.getLinkId(i->second.parent->first);
                }
                
            }
		}
        
        //The serialization is necessary before serializing the parameters        
        tree_param = Eigen::VectorXd(10*ns);
        updateParams();
        
        
        //As the order of the recursion is the same, it is calculated only at configuration
        std::vector<unsigned int> index_stack;
        
        index_stack.reserve(tree.getNrOfSegments());
        recursion_order.reserve(tree.getNrOfSegments());
        
        index_stack.clear();
        recursion_order.clear();
        
        for( unsigned int j=0; j < mu_root.size(); j++ ) {
            index_stack.push_back(mu_root[j]);
        }
        
        while( !index_stack.empty() ) {
            
            unsigned int curr_index = index_stack.back();
            index_stack.pop_back();
            
            recursion_order.push_back(curr_index);
            
            //Doing the recursion on the children
            for( unsigned int j=0; j < mu[curr_index].size(); j++ ) {
                index_stack.push_back(mu[curr_index][j]);
            }
        }
        
        assert(recursion_order.size() == tree.getNrOfSegments());
        
        //Compiling indicator function;
        for(unsigned int j=0; j < ns; j++ ) {
            indicator_function[j] = std::vector<bool>(nj,false);
        }
        
        for(unsigned int j=0; j < ns; j++ ) {
            unsigned int jnt_index = link2joint[j];
            if( seg_vector[j]->second.segment.getJoint().getType() != Joint::None ) {
                
                index_stack.clear();
                index_stack.push_back(j);
                while( !index_stack.empty() ) {
                    unsigned int curr_index = index_stack.back();
                    index_stack.pop_back();
                    
                    indicator_function[curr_index][jnt_index] = true;
                
                    for( unsigned int k=0; k < mu[curr_index].size(); k++ ) {
                        index_stack.push_back(mu[curr_index][k]);
                    }
                }
            }
        }
        
        
        for( unsigned int j=0; j < mu_root.size(); j++ ) {
            index_stack.push_back(mu_root[j]);
        }
        
        while( !index_stack.empty() ) {
            
            unsigned int curr_index = index_stack.back();
            index_stack.pop_back();
            
            recursion_order.push_back(curr_index);
            
            //Doing the recursion on the children
            for( unsigned int j=0; j < mu[curr_index].size(); j++ ) {
                index_stack.push_back(mu[curr_index][j]);
            }
        }

        
        
        
    }
Exemple #7
0
Twist Path_Line::Acc(double s,double sd,double sdd) const  {
	return Twist( V_start_end*sdd*scalelin, orient->Acc(s*scalerot,sd*scalerot,sdd*scalerot) );
}
Exemple #8
0
Twist Path_Line::Vel(double s,double sd) const  {
	return Twist( V_start_end*sd*scalelin, orient->Vel(s*scalerot,sd*scalerot) );
}
 Twist Jacobian::getColumn(unsigned int i) const{
     return Twist(Vector(data(0,i),data(1,i),data(2,i)),Vector(data(3,i),data(4,i),data(5,i)));
 }
Exemple #10
0
 Twist deriv() const {
     return Twist(vel.deriv(),rot.deriv());
 }
Exemple #11
0
 Twist value() const {
     return Twist(vel.value(),rot.value());
 }
Exemple #12
0
 Twist deriv() const { return Twist(p.deriv(),M.deriv());}
 ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
     chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
     X(ns),S(ns),v(ns),a(ns),f(ns)
 {
     ag=-Twist(grav,Vector::Zero());
 }
 Twist twistvw( const Vector& trans, const Vector& rot )
 {
         return Twist( trans, rot );
 };