Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 
00007 const OutputCmd HeadPointerMC::unused;
00008 
00009 HeadPointerMC::HeadPointerMC() :
00010   MotionCommand(), dirty(true), active(true) {
00011   setWeight(1);
00012   for(unsigned int i=0; i<NumHeadJoints; i++) {
00013     headModes[i]=BodyRelative;
00014     headValues[i]=0;
00015   }
00016 }
00017 
00018 void HeadPointerMC::setJoints(double tilt, double pan, double roll) {
00019   headValues[TiltOffset]=tilt;
00020   headValues[PanOffset]=pan;
00021   headValues[RollOffset]=roll;
00022   dirty=true;
00023 }
00024 
00025 void HeadPointerMC::setWeight(double w) {
00026   for(unsigned int x=0; x<NumHeadJoints; x++)
00027     headJoints[x].weight=w;
00028   dirty=true;
00029 }
00030 
00031 void HeadPointerMC::setMode(CoordFrame_t m, bool conv) {
00032   for(unsigned int x=0; x<NumHeadJoints; x++)
00033     setJointMode((RobotInfo::TPROffset_t)x,m,conv);
00034 }
00035 
00036 void HeadPointerMC::setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool conv) {
00037   if(conv)
00038     headValues[i]=HeadPointerMC::convert(i,headValues[i],headModes[i],m);
00039   headModes[i]=m;
00040   dirty=true;
00041 }
00042 
00043 int HeadPointerMC::updateOutputs() {
00044   int tmp=isDirty();
00045   if(tmp) {
00046     dirty=false;
00047     for(unsigned int i=0; i<NumHeadJoints; i++) {
00048       if(headModes[i]!=BodyRelative) {
00049         headJoints[i].value=convToBodyRelative((RobotInfo::TPROffset_t)i,headValues[i],headModes[i]);
00050         dirty=true;
00051       } else
00052         headJoints[i].value=headValues[i];
00053       for(unsigned int f=0; f<NumFrames; f++)
00054         motman->setOutput(this,i+HeadOffset,headJoints[i]);
00055     }
00056   }
00057   return tmp;
00058 }
00059 
00060 const OutputCmd& HeadPointerMC::getOutputCmd(unsigned int i) {
00061   i-=HeadOffset;
00062   if(i<NumHeadJoints && getActive())
00063     return headJoints[i];
00064   else
00065     return unused;
00066 }
00067 
00068 /*! @todo this is perhaps a bit amateurish - could be more accurate */
00069 double HeadPointerMC::convToBodyRelative(RobotInfo::TPROffset_t i, double v, CoordFrame_t mode) const {
00070   switch(mode) {
00071   default:
00072     ASSERT(false,"Passed bad mode "<<mode);
00073   case BodyRelative:
00074     return v;
00075   case GravityRelative: {
00076     double bacc=state->sensors[BAccelOffset];
00077     double lacc=state->sensors[LAccelOffset];
00078     double dacc=state->sensors[DAccelOffset];
00079     switch(i) {
00080     case TiltOffset:
00081       return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00082     case PanOffset:
00083       return v;
00084     case RollOffset:
00085       return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00086     default:
00087       ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00088       return v;
00089     }
00090   }
00091   }
00092 }
00093 
00094 /*! @todo this is perhaps a bit amateurish - could be more accurate */
00095 double HeadPointerMC::convFromBodyRelative(RobotInfo::TPROffset_t i, double v, CoordFrame_t mode) const {
00096   switch(mode) {
00097   default:
00098     ASSERT(false,"Passed bad mode "<<mode);
00099   case BodyRelative:
00100     return v;
00101   case GravityRelative: {
00102     double bacc=state->sensors[BAccelOffset];
00103     double lacc=state->sensors[LAccelOffset];
00104     double dacc=state->sensors[DAccelOffset];
00105     switch(i) {
00106     case TiltOffset:
00107       return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00108     case PanOffset:
00109       return v;
00110     case RollOffset:
00111       return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00112     default:
00113       ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00114       return v;
00115     }
00116   }
00117   }
00118 }
00119 
00120 
00121 /*! @file
00122  * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking
00123  * @author ejt (Creator)
00124  *
00125  * $Author: ejt $
00126  * $Name: tekkotsu-1_4_1 $
00127  * $Revision: 1.6 $
00128  * $State: Exp $
00129  * $Date: 2003/06/12 23:41:40 $
00130  */
00131 
00132 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:30 2003 by Doxygen 1.3.2