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

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4