Tekkotsu: PostureMC.cc Source File

00001 #include "PostureMC.h"
00002 #include "Shared/Config.h"
00003 #include "Shared/WorldState.h"
00004 
00005 PostureMC& PostureMC::setDirty(bool d/*=true*/) {
00006   dirty=d;
00007   targetReached=false;
00008   for(unsigned int i=0; i<NumOutputs; i++)
00009     curPositions[i]=motman->getOutputCmd(i).value; //not state->outputs[i]; - see function documentation
00010   return *this;
00011 } 
00012 
00013 void PostureMC::defaultMaxSpeed(float x/*=1*/) {
00014   for(unsigned int i=0; i<NumOutputs; i++)
00015     maxSpeeds[i]=MaxOutputSpeed[i]*FrameTime*x; //MaxOutputsSpeed is rad/ms
00016   //respect the config values for the neck joints (which are stored as rad/sec)
00017   maxSpeeds[HeadOffset+TiltOffset]=config->motion.max_head_tilt_speed*FrameTime*x/1000; 
00018   maxSpeeds[HeadOffset+PanOffset]=config->motion.max_head_pan_speed*FrameTime*x/1000;
00019   maxSpeeds[HeadOffset+RollOffset]=config->motion.max_head_roll_speed*FrameTime*x/1000;
00020 }
00021 
00022 int PostureMC::updateOutputs() {
00023   int tmp=isDirty();
00024   if(tmp || hold) {
00025     dirty=false;
00026     for(unsigned int i=0; i<NumOutputs; i++) {
00027       if(maxSpeeds[i]<=0) {
00028         curPositions[i]=cmds[i].value;
00029         motman->setOutput(this,i,cmds[i]);
00030       } else { // we may be trying to exceeded maxSpeed
00031         unsigned int f=0;
00032         while(cmds[i].value>curPositions[i]+maxSpeeds[i] && f<NumFrames) {
00033           curPositions[i]+=maxSpeeds[i];
00034           motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00035           f++;
00036         }
00037         while(cmds[i].value<curPositions[i]-maxSpeeds[i] && f<NumFrames) {
00038           curPositions[i]-=maxSpeeds[i];
00039           motman->setOutput(this,i,OutputCmd(curPositions[i],cmds[i].weight),f);
00040           f++;
00041         }
00042         if(f<NumFrames) { //we reached target value, fill in rest of frames
00043           curPositions[i]=cmds[i].value;
00044           for(;f<NumFrames;f++)
00045             motman->setOutput(this,i,cmds[i],f);
00046         } else // we didn't reach target value, still dirty
00047           dirty=true;
00048       }
00049     }
00050     if(!dirty && !targetReached) {
00051       postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00052       targetReached=true;
00053       targetTimestamp=get_time();
00054     }
00055   }
00056   return tmp;
00057 }
00058 
00059 int PostureMC::isAlive() {
00060   if(dirty || !targetReached)
00061     return true;
00062   if(targetReached && (!hold || get_time()-targetTimestamp>timeout)) { //prevents a conflicted PostureMC's from fighting forever
00063     if(get_time()-targetTimestamp>timeout && getAutoPrune())
00064       serr->printf("WARNING: posture timed out - possible joint conflict or out-of-range target\n");
00065     return false;
00066   }
00067   float max=0;
00068   for(unsigned int i=0; i<NumOutputs; i++)
00069     if(cmds[i].weight>0) {
00070       float dif=cmds[i].value-state->outputs[i];
00071       if(dif>max)
00072         max=dif;
00073     }
00074   return (max>tolerance);
00075 }
00076 
00077 void PostureMC::init() {
00078   defaultMaxSpeed();
00079   setDirty();
00080 }
00081 
00082 /*! @file
00083  * @brief Implements PostureMC, a MotionCommand shell for PostureEngine
00084  * @author ejt (Creator)
00085  *
00086  * $Author: ejt $
00087  * $Name: tekkotsu-3_0 $
00088  * $Revision: 1.5 $
00089  * $State: Exp $
00090  * $Date: 2006/08/31 21:48:00 $
00091  */