Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

HeadPointerMC.cc

Go to the documentation of this file.

#include "HeadPointerMC.h"
#include "Shared/debuget.h"
#include "Shared/WorldState.h"
#include "MotionManager.h"
#include <math.h>
#include "Shared/Config.h"

HeadPointerMC::HeadPointerMC() :
  MotionCommand(), dirty(true), active(true) {
  setWeight(1);
  defaultMaxSpeed();
  for(unsigned int i=0; i<NumHeadJoints; i++) {
    headModes[i]=BodyRelative;
    headValues[i]=0;
    headJoints[i].value=state->outputs[HeadOffset+i];
  }
}

void HeadPointerMC::setJoints(double tilt, double pan, double roll) {
  headValues[TiltOffset]=tilt;
  headValues[PanOffset]=pan;
  headValues[RollOffset]=roll;
  dirty=true;
}

void HeadPointerMC::setWeight(double w) {
  for(unsigned int x=0; x<NumHeadJoints; x++)
    headJoints[x].weight=w;
  dirty=true;
}

void HeadPointerMC::defaultMaxSpeed() {
  maxSpeed[TiltOffset]=config->motion.max_head_tilt_speed*FrameTime/1000;
  maxSpeed[PanOffset]=config->motion.max_head_pan_speed*FrameTime/1000;
  maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime/1000;
}

void HeadPointerMC::setMode(CoordFrame_t m, bool conv) {
  for(unsigned int x=0; x<NumHeadJoints; x++)
    setJointMode((RobotInfo::TPROffset_t)x,m,conv);
}

void HeadPointerMC::setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool conv) {
  if(conv)
    headValues[i]=HeadPointerMC::convert(i,headValues[i],headModes[i],m);
  headModes[i]=m;
  dirty=true;
}

int HeadPointerMC::updateOutputs() {
  int tmp=isDirty();
  if(tmp) {
    dirty=false;
    for(unsigned int i=0; i<NumHeadJoints; i++) {
      float value;
      if(headModes[i]!=BodyRelative) {
        value=convToBodyRelative((RobotInfo::TPROffset_t)i,headValues[i],headModes[i]);
        dirty=true;
      } else
        value=headValues[i];
      if(maxSpeed[i]<=0)
        headJoints[i].value=value;
      if(value==headJoints[i].value) {
        motman->setOutput(this,i+HeadOffset,headJoints[i]);
      } else { // we're trying to exceeded maxSpeed
        unsigned int f=0;
        while(value>headJoints[i].value+maxSpeed[i] && f<NumFrames) {
          headJoints[i].value+=maxSpeed[i];
          motman->setOutput(this,i+HeadOffset,headJoints[i],f);
          f++;
        }
        while(value<headJoints[i].value-maxSpeed[i] && f<NumFrames) {
          headJoints[i].value-=maxSpeed[i];
          motman->setOutput(this,i+HeadOffset,headJoints[i],f);
          f++;
        }
        if(f<NumFrames) { //we reached target value, fill in rest of frames
          headJoints[i].value=value;
          for(;f<NumFrames;f++)
            motman->setOutput(this,i+HeadOffset,headJoints[i],f);
        } else // we didn't reach target value, still dirty
          dirty=true;
      }
    }
  }
  return tmp;
}

const OutputCmd& HeadPointerMC::getOutputCmd(unsigned int i) {
  i-=HeadOffset;
  if(i<NumHeadJoints && getActive())
    return headJoints[i];
  else
    return OutputCmd::unused;
}

/*! @todo this is perhaps a bit amateurish - could be more accurate */
double HeadPointerMC::convToBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
  switch(mode) {
  default:
    ASSERT(false,"Passed bad mode "<<mode);
  case BodyRelative:
    return v;
  case GravityRelative: {
    double bacc=state->sensors[BAccelOffset];
    double lacc=state->sensors[LAccelOffset];
    double dacc=state->sensors[DAccelOffset];
    switch(i) {
    case TiltOffset:
      return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
    case PanOffset:
      return v;
    case RollOffset:
      return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
    default:
      ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
      return v;
    }
  }
  }
}

/*! @todo this is perhaps a bit amateurish - could be more accurate */
double HeadPointerMC::convFromBodyRelative(TPROffset_t i, double v, CoordFrame_t mode) const {
  switch(mode) {
  default:
    ASSERT(false,"Passed bad mode "<<mode);
  case BodyRelative:
    return v;
  case GravityRelative: {
    double bacc=state->sensors[BAccelOffset];
    double lacc=state->sensors[LAccelOffset];
    double dacc=state->sensors[DAccelOffset];
    switch(i) {
    case TiltOffset:
      return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
    case PanOffset:
      return v;
    case RollOffset:
      return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
    default:
      ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
      return v;
    }
  }
  }
}


/*! @file
 * @brief Implements HeadPointerMC, a class for various ways to control where the head is looking
 * @author ejt (Creator)
 *
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.8 $
 * $State: Rel $
 * $Date: 2003/09/07 22:14:01 $
 */

Tekkotsu v1.5

Generated Fri Oct 10 15:51:59 2003 by Doxygen 1.3.4