//Method called during the 'SET' Mode void HeadProvider::setMode() { //Maximum head movement is Rad/motion frame (6 deg/20ms from AL docs) const float MAX_HEAD_VEL = 6.0f*TO_RAD;/* ** *///we don't use this... //Calculate how much we can move toward the goal const float yawChangeTarget = NBMath::clip(yawDest - lastYawDest, - yawMaxSpeed, yawMaxSpeed); const float pitchChangeTarget = NBMath::clip(pitchDest - lastPitchDest, -pitchMaxSpeed, pitchMaxSpeed); //update memory for next run lastYawDest = lastYawDest+yawChangeTarget; lastPitchDest = lastPitchDest +pitchChangeTarget; //update the chain angles std::vector<float> newChainAngles; newChainAngles.push_back(lastYawDest); newChainAngles.push_back(lastPitchDest); setNextChainJoints(HEAD_CHAIN,newChainAngles); std::vector<float> head_gains(HEAD_JOINTS, headSetStiffness); //Return the stiffnesses for each joint setNextChainStiffnesses(HEAD_CHAIN, head_gains); }
void HeadProvider::scriptedMode(std::vector<float>& sensorAngles) { if ( currChoppedCommand->isDone() ) setNextHeadCommand(sensorAngles); if (!currChoppedCommand->isDone() ) { currChoppedCommand->nextFrame(); setNextChainJoints( HEAD_CHAIN, currChoppedCommand->getNextJoints(HEAD_CHAIN) ); setNextChainStiffnesses( Kinematics::HEAD_CHAIN, currChoppedCommand->getStiffness( Kinematics::HEAD_CHAIN) ); } else { setNextChainJoints( HEAD_CHAIN, getCurrentHeads(sensorAngles) ); setNextChainStiffnesses( Kinematics::HEAD_CHAIN, std::vector<float>(HEAD_JOINTS, 0.0f) ); } }
void ScriptedProvider::calculateNextJointsAndStiffnesses( std::vector<float>& sensorAngles, std::vector<float>& sensorCurrents, const messages::InertialState& sensorInertials, const messages::FSR& sensorFSRs) { PROF_ENTER(P_SCRIPTED); if (currCommandEmpty()) setNextBodyCommand(sensorAngles); // Go through the chains and enqueue the next // joints from the ChoppedCommand. boost::shared_ptr<std::vector <std::vector <float> > > currentChains(getCurrentChains(sensorAngles)); currCommand->nextFrame(); // so Python can keep track of progress for (unsigned int id=0; id< Kinematics::NUM_CHAINS; ++id ) { Kinematics::ChainID cid = static_cast<Kinematics::ChainID>(id); if ( currCommand->isDone() ){ setNextChainJoints( cid, currentChains->at(cid) ); }else{ setNextChainJoints( cid, currCommand->getNextJoints(cid) ); } // Curr command will allways provide the current stiffnesses // even if it is finished providing new joint angles. setNextChainStiffnesses( cid, currCommand->getStiffness(cid) ); } setActive(); PROF_EXIT(P_SCRIPTED); }