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]); } } }
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) ); }
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))); }
Twist deriv() const { return Twist(vel.deriv(),rot.deriv()); }
Twist value() const { return Twist(vel.value(),rot.value()); }
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 ); };