//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);
}
