Homepage Demos Overview Downloads Tutorials Reference
Credits

OldHeadPointerMC.cc

Go to the documentation of this file.
00001 #include "OldHeadPointerMC.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "MotionManager.h"
00005 #include <math.h>
00006 #include "Shared/Config.h"
00007 
00008 OldHeadPointerMC::OldHeadPointerMC() :
00009   MotionCommand(), dirty(true), active(true), targetReached(false) {
00010   setWeight(1);
00011   defaultMaxSpeed();
00012   for(unsigned int i=0; i<NumHeadJoints; i++) {
00013     headModes[i]=BodyRelative;
00014     headTargets[i]=0;
00015     headCmds[i].value=state->outputs[HeadOffset+i];
00016   }
00017 }
00018 
00019 void OldHeadPointerMC::setJoints(double tilt, double pan, double roll) {
00020   setJointValue(TiltOffset,tilt);
00021   setJointValue(PanOffset,pan);
00022   setJointValue(RollOffset,roll);
00023 }
00024 
00025 void OldHeadPointerMC::setWeight(double w) {
00026   for(unsigned int x=0; x<NumHeadJoints; x++)
00027     headCmds[x].weight=w;
00028   dirty=true; targetReached=false;
00029 }
00030 
00031 void OldHeadPointerMC::defaultMaxSpeed() {
00032   maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime/1000;
00033   maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime/1000;
00034   maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime/1000;
00035 }
00036 
00037 void OldHeadPointerMC::setMode(CoordFrame_t m, bool conv) {
00038   for(unsigned int x=0; x<NumHeadJoints; x++)
00039     setJointMode((RobotInfo::TPROffset_t)x,m,conv);
00040 }
00041 
00042 void OldHeadPointerMC::setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool conv) {
00043   if(conv)
00044     headTargets[i]=OldHeadPointerMC::convert(i,headTargets[i],headModes[i],m);
00045   headModes[i]=m;
00046   dirty=true; targetReached=false;
00047 }
00048 
00049 int OldHeadPointerMC::updateOutputs() {
00050   int tmp=isDirty();
00051   if(tmp) {
00052     dirty=false;
00053     for(unsigned int i=0; i<NumHeadJoints; i++) {
00054       float value;
00055       if(headModes[i]!=BodyRelative) {
00056   value=convToBodyRelative((RobotInfo::TPROffset_t)i,headTargets[i],headModes[i]);
00057   dirty=true;
00058       } else
00059   value=headTargets[i];
00060       if(maxSpeed[i]<=0)
00061   headCmds[i].value=value;
00062       if(value==headCmds[i].value) {
00063   motman->setOutput(this,i+HeadOffset,headCmds[i]);
00064       } else { // we may be trying to exceeded maxSpeed
00065   unsigned int f=0;
00066   while(value>headCmds[i].value+maxSpeed[i] && f<NumFrames) {
00067     headCmds[i].value+=maxSpeed[i];
00068     motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00069     f++;
00070   }
00071   while(value<headCmds[i].value-maxSpeed[i] && f<NumFrames) {
00072     headCmds[i].value-=maxSpeed[i];
00073     motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00074     f++;
00075   }
00076   if(f<NumFrames) { //we reached target value, fill in rest of frames
00077     headCmds[i].value=value;
00078     for(;f<NumFrames;f++)
00079       motman->setOutput(this,i+HeadOffset,headCmds[i],f);
00080   } else // we didn't reach target value, still dirty
00081     dirty=true;
00082       }
00083     }
00084     float dist=0;
00085     for(unsigned int i=0; i<NumHeadJoints; i++) {
00086       float diff=(state->outputs[HeadOffset+i]-headTargets[i]);
00087       dist+=diff*diff;
00088     }
00089     if(dist<0.05*0.05 && !targetReached) {
00090       postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID));
00091       targetReached=true;
00092     }
00093   }
00094   return tmp;
00095 }
00096 
00097 const OutputCmd& OldHeadPointerMC::getOutputCmd(unsigned int i) {
00098   i-=HeadOffset;
00099   if(i<NumHeadJoints && getActive())
00100     return headCmds[i];
00101   else
00102     return OutputCmd::unused;
00103 }
00104 
00105 /*! @todo this is perhaps a bit amateurish - could be more accurate */
00106 double OldHeadPointerMC::convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
00107   switch(mode) {
00108   default:
00109     ASSERT(false,"Passed bad mode "<<mode);
00110   case BodyRelative:
00111     return v;
00112   case GravityRelative: {
00113     double bacc=state->sensors[BAccelOffset];
00114     double lacc=state->sensors[LAccelOffset];
00115     double dacc=state->sensors[DAccelOffset];
00116     switch(i) {
00117     case TiltOffset:
00118       return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00119     case PanOffset:
00120       return v;
00121     case RollOffset: //==NodOffset
00122       if(state->robotDesign&WorldState::ERS7Mask) {
00123   float pans=sin(state->outputs[HeadOffset+PanOffset]);
00124   float panc=cos(state->outputs[HeadOffset+PanOffset]);
00125   float ans=v;
00126   ans+=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans));
00127   ans-=panc*state->outputs[HeadOffset+TiltOffset];
00128   return ans;
00129       } else
00130   return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00131     default:
00132       ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00133       return v;
00134     }
00135   }
00136   }
00137 }
00138 
00139 /*! @todo this is perhaps a bit amateurish - could be more accurate */
00140 double OldHeadPointerMC::convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
00141   switch(mode) {
00142   default:
00143     ASSERT(false,"Passed bad mode "<<mode);
00144   case BodyRelative:
00145     return v;
00146   case GravityRelative: {
00147     double bacc=state->sensors[BAccelOffset];
00148     double lacc=state->sensors[LAccelOffset];
00149     double dacc=state->sensors[DAccelOffset];
00150     switch(i) {
00151     case TiltOffset:
00152       return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00153     case PanOffset:
00154       return v;
00155     case RollOffset: //==NodOffset
00156       if(state->robotDesign&WorldState::ERS7Mask) {
00157   float pans=sin(state->outputs[HeadOffset+PanOffset]);
00158   float panc=cos(state->outputs[HeadOffset+PanOffset]);
00159   float ans=v;
00160   ans-=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans));
00161   ans+=panc*state->outputs[HeadOffset+TiltOffset];
00162   return ans;
00163       } else
00164   return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00165     default:
00166       ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00167       return v;
00168     }
00169   }
00170   }
00171 }
00172 
00173 
00174 /*! @file
00175  * @brief Implements OldHeadPointerMC, a class for various ways to control where the head is looking
00176  * @author ejt (Creator)
00177  *
00178  * $Author: ejt $
00179  * $Name: tekkotsu-2_2_1 $
00180  * $Revision: 1.1 $
00181  * $State: Exp $
00182  * $Date: 2004/10/14 20:23:50 $
00183  */
00184 
00185 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1