Homepage Demos Overview Downloads Tutorials Reference
Credits

HeadPointerMC.cc

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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:28 2004 by Doxygen 1.3.4