Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PostureMC.cc

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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4