Modules/HeadControl/GT2004HeadControl/GT2004HeadControlBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2004HeadControlBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in "basic-behaviors.xml".
*
* @author Martin Lötzsch
*/

#include "GT2004HeadControl.h"
#include "Tools/Math/Common.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"

void GT2004HeadControlBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
{
  engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionSeen);
  engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionPropagated);
  engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionCommunicated);
  engine.registerBasicBehavior(basicBehaviorLookAroundAtSeenBall);
  engine.registerBasicBehavior(basicBehaviorOtherHeadMovements);
  engine.registerBasicBehavior(basicBehaviorFindBall); 
  engine.registerBasicBehavior(basicBehaviorLookAtBall);
  engine.registerBasicBehavior(basicBehaviorLookAtCloseLandmark);
  engine.registerBasicBehavior(basicBehaviorLookAtBallAndClosestLandmark);
  engine.registerBasicBehavior(basicBehaviorReturnToBall);
  engine.registerBasicBehavior(basicBehaviorDirectedScanForLandmarks);
  engine.registerBasicBehavior(basicBehaviorScanBackToBall);
  engine.registerBasicBehavior(basicBehaviorScanAwayFromBall);
  engine.registerBasicBehavior(basicBehaviorGrabBall);
  engine.registerBasicBehavior(basicBehaviorReleaseBall);
  engine.registerBasicBehavior(basicBehaviorWaitForGrab);
  engine.registerBasicBehavior(basicBehaviorSearchForBallLeft);
  engine.registerBasicBehavior(basicBehaviorSearchForBallRight);
}

void GT2004BasicBehaviorLookAtBall::execute()
{
  // trust the communicated ball if it get lost
  headControl.useCommunicatedBall = true;

  double neckTilt, headPan, headTilt;
  headControl.getLookAtBallAngles(ballModel.seen, neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);
}


void GT2004BasicBehaviorSearchForBallLeft::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    headControl.searchForBallLeft();
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt);
}

void GT2004BasicBehaviorSearchForBallRight::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    headControl.searchForBallRight();
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt);
}

void GT2004BasicBehaviorLookAroundAtSeenBall::execute()
{
  double neckTilt, headPan, headTilt;

  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    Vector3<double> leftDown,
                    rightDown,
                    leftTop,
                    rightTop;

    Vector3<double> ballPosition3d;     
    ballPosition3d.x = (double) ballModel.seen.x;
    ballPosition3d.y = (double) ballModel.seen.y;
    ballPosition3d.z = 0;
    Vector2<double> toBall = ballModel.seen - robotPose.translation;
    
    // center the ball view in the middle of the image
    // if the ball is near, the ball should be seen, if we look halfway down
    // constante definition of distance to ball

    Vector2<int> cameraImageOffset(0,0);
  
    double neckTilt,headPan,headTilt;
    headControl.simpleLookAtPointOnField(ballPosition3d,cameraImageOffset,neckTilt,headPan,headTilt);
    Vector3<double> ballAngles (neckTilt,headPan,headTilt);


    //headControl.getSensorHeadAngles(leftDown);
    leftDown = ballAngles;
    rightDown = leftDown;
    leftTop = leftDown;
    rightDown = leftDown;

    leftDown.y += 0.6;
    //leftDown.x -= 0.2;
    
    rightDown.y -= 0.6;
    //rightDown.x -= 0.2;

    leftTop.y += 0.6;
    leftTop.x += 0.2;

    rightTop.y -= 0.6;
    rightTop.x += 0.2;

    Vector3<double> points[]={leftDown,
                              rightDown,
                              rightTop,
                              leftTop};
    long durations[] =   {100,100,100,100};

    headPathPlanner.init(points,durations,(sizeof(points)/sizeof(Vector3<double>)),false);

  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);
  
}

void GT2004BasicBehaviorLookAtCloseLandmark::execute()
{
  double neckTilt, headPan, headTilt;
  int closest = headControl.calculateClosestLandmark(headPathPlanner.lastHeadPan, 0);
  
  // scan sidewards beginning with side on which the head is actually oriented
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
  panCount = 0;
  lastScanWasLeft = (headPathPlanner.lastHeadPan < 0);
  }

  // Only for testing
  // closest = -1;
  
  if (closest == -1 && panCount < 2)
  {
    OUTPUT (idText, text, "Looking at closest landmark...");
    headPathPlanner.getAngles (neckTilt, headPan, headTilt);

    if (headPan > headControl.headMiddleLeft.y)
    {
      lastScanWasLeft = true;
      panCount++;
      headControl.setJoints (headControl.headRight.x, headControl.headRight.y, headControl.headRight.z);
    }
    else if (headPan > headControl.headMiddleRight.y)
    {
      lastScanWasLeft = false;
      panCount++;
      headControl.setJoints (headControl.headLeft.x, headControl.headLeft.y, headControl.headLeft.z);
    } 
    else if (!lastScanWasLeft)
      headControl.setJoints (headControl.headLeft.x, headControl.headLeft.y, headControl.headLeft.z);
    else
      headControl.setJoints (headControl.headRight.x, headControl.headRight.y, headControl.headRight.z);

    headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    closest = headControl.calculateClosestLandmark (headPan, 0);
  }
  
  headControl.aimAtLandmark(closest, neckTilt, headPan, headTilt);

  headControl.setJoints(neckTilt, headPan, headTilt, 4.0);

  CIRCLE(headControlField, 
    targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 100, 
    2, 0, Drawings::red);
  DEBUG_DRAWING_FINISHED(headControlField);  
}


void GT2004BasicBehaviorDirectedScanForLandmarks::execute()
{
  double neckTilt, headPan, headTilt;

  /** alternate scan direction whenever basic behavior is called for the first time */
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    leftOrRight *= -1;
    lastScanWasLeft = (leftOrRight > 0);
  }
 
 
  /** The following calculates the closest landmark and compares it to the 
  one that the scan is currently aiming at. If the closest is further in the
  direction of the scan, it is used as the new target. If it is in the opposite
  direction, it is not used to guarantee a stable head movement */

  int closest = headControl.calculateClosestLandmark(headPathPlanner.lastHeadPan + leftOrRight*.1, leftOrRight);

  /** reset variables when a new scan is started */
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    currentLandmark = closest;
    nextLandmark = -1;
    waitSomeBeforeLookingAtNextLandmark = 0;
  }

  /** if a landmark is found to be closest that is not the 
  one the robot is currently aiming at, store the ID in nextLandmark
  but don't change the target yet. */ 
  if (closest != nextLandmark)
  {
    if ((nextLandmark != -1) || (closest != -1))
    {
      nextLandmark = closest;
      waitSomeBeforeLookingAtNextLandmark = 1;
    }
//    else 
//    {
//      double angleToClosest = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y));
//      double angleToCurrent = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y));
//      if ((angleToCurrent - angleToClosest)*leftOrRight < 0)
//        nextLandmark = closest;
//      waitSomeBeforeLookingAtNextLandmark = 1;
//    }      
  }

  /** the following functions as a delay so that 
  the robot continues to look at a landmark for a 
  brief period of time before aiming at the next */
  if (waitSomeBeforeLookingAtNextLandmark)
  {
    CIRCLE(headControlField, 
    targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y, 100, 
    2, 0, Drawings::red);
    waitSomeBeforeLookingAtNextLandmark++;  
  }

  /* after the robot has aimed at the previous/old target for 
  more than 8 secs, select new target */
  if (waitSomeBeforeLookingAtNextLandmark > 15)
  {
    currentLandmark = nextLandmark;
    waitSomeBeforeLookingAtNextLandmark = -1;
  }
  
  CIRCLE(headControlField, 
    targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y, 80, 
    2, 0, Drawings::yellow);
  
  DEBUG_DRAWING_FINISHED(headControlField);  

  /** calculate the XABSL input symbol "nextLandmarkIsWithinReach" */ 
  double angleToCurrent = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y));
  if((closest == -1) || 
    (fabs(angleToCurrent) > headControl.jointLimitHeadPanP + cameraInfo.openingAngleWidth/2))
  {
    nextLandmarkIsWithinReach = false;
  }
  else
  {
    nextLandmarkIsWithinReach = true;
  }
  
  if (currentLandmark != -1)
    headControl.aimAtLandmark(currentLandmark, neckTilt, headPan, headTilt);

  headControl.setJoints(neckTilt, headPan, headTilt, 4);
}



void GT2004BasicBehaviorLookAtBallAndClosestLandmark::execute()
{
  double neckTiltBall, headPanBall, headTiltBall;
  headControl.getLookAtBallAngles(ballModel.seen, neckTiltBall, headPanBall, headTiltBall);
  
  double neckTiltLM, headPanLM, headTiltLM;
  int closest = headControl.calculateClosestLandmark(Geometry::angleTo(robotPose, ballModel.seen));
  if (closest != -1)
  {
    headControl.simpleLookAtPointOnField(
      targetPointOnLandmark[closest].position, 
      Vector2<int>(0, 0), 
      neckTiltLM, headPanLM, headTiltLM);
  }
  else
  {
    /** @todo: do this properly!! */ 
  }

  /** calculate the sizes of the ball and the interesting landmark in opening angles */
  double openingAngleOfBall = tan(2*ballRadius/((ballModel.seen-robotPose.translation).abs()));
  Vector2<double> landmarkInXYPlane(targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y);
  double openingAngleWidthOfLandmark = tan(2*targetPointOnLandmark[closest].width/((landmarkInXYPlane-robotPose.translation).abs()));
  
  /** the difference in pan between looking at the ball and looking at the landmark */
  double panDiff = headPanBall - headPanLM;
  double tiltDiff = headTiltBall - headTiltLM;

  /**  limit the panDiffTmp to the range defined here: 
  (this is necessary for the weight calculation to work!)  */
  double minAngle = .2, maxAngle = 1.2;
  Range<double> angleRange(minAngle, maxAngle);
  double panDiffTmp = angleRange.limit(panDiff);
  
  /** caculate a weight that determines how much of the adjustment is 
  actually performed. This is useful when the landmark is far away and
  it is most likely better to just center on the ball */ 
  double adjustmentWeight = 1-(panDiffTmp-minAngle)/(maxAngle-minAngle);
  Range<double> weightRange(0,1);
  adjustmentWeight = weightRange.limit(adjustmentWeight);

  /** only perform adjustments if the ball ist more than 50 cms away */
  double minBClose = 350, maxBClose = 700;
  Range<double> ballCloseRange(minBClose, maxBClose);
  double ballIsClose = (Geometry::fieldCoord2Relative(robotPose, ballModel.seen)).abs();
  ballIsClose = ballCloseRange.limit(ballIsClose);
  ballIsClose = (ballIsClose - minBClose)/(maxBClose-minBClose);
  
  adjustmentWeight *= ballIsClose;

  /** adjust pan direction: gaze between ball and landmark */
  if (fabs(panDiff) + openingAngleWidthOfLandmark > cameraInfo.openingAngleWidth/2)
  {
    double maxPanAdjustment = cameraInfo.openingAngleWidth/2 - 2.5*openingAngleOfBall;
    double panAdjustment = -panDiff+sgn(panDiff)*(cameraInfo.openingAngleWidth/2.5-openingAngleWidthOfLandmark);
    if (fabs(headPanLM) > 0.7*headControl.jointLimitHeadPanP)
      panAdjustment = 0;

    panAdjustment = Range<double>::Range(-maxPanAdjustment,maxPanAdjustment).limit(panAdjustment);

    headPanBall += panAdjustment*adjustmentWeight;
  }

  /** for tilt: only adjust gaze direction upwards (!)
  and only if the landmark is reachable; furthermore, onyl raise the gaze 
  direction as much as necessary to get landmark into view. */

  if (headTiltLM > headTiltBall)
    if (headTiltLM - headTiltBall > cameraInfo.openingAngleHeight/2)
    {
      double maxTiltAdjustment = cameraInfo.openingAngleHeight/2 - openingAngleOfBall;
      if(maxTiltAdjustment < 0)
        maxTiltAdjustment = 0;
      double tiltAdjustment = -tiltDiff-cameraInfo.openingAngleHeight/2.5;
      if (fabs(headPanLM) > 0.7*headControl.jointLimitHeadPanP)
        tiltAdjustment = 0;
      Range<double> tiltClipping(0, maxTiltAdjustment);
      tiltAdjustment = tiltClipping.limit(tiltAdjustment);

      // only do this adjustment if landmark is almost in sight...
      headTiltBall += tiltAdjustment*adjustmentWeight;
    }
   
  headControl.setJoints(neckTiltBall, headPanBall, headTiltBall);

  LINE(headControlField, 
    robotPose.translation.x, robotPose.translation.y, 
    robotPose.translation.x + 2000*cos(headPanBall+robotPose.rotation), robotPose.translation.y + 2000*sin(headPanBall+robotPose.rotation), 
    20, 0, Drawings::red);
  
  CIRCLE(headControlField, 
    targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 100, 
    2, 0, Drawings::red);
  DEBUG_DRAWING_FINISHED(headControlField);  
}

void GT2004BasicBehaviorBeginBallSearchAtBallPositionSeen::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    // now start search and init headpathplaner
    // not very nice...
    headControl.beginBallSearchAt((Vector2<double>) ballModel.seen);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);

}

void GT2004BasicBehaviorBeginBallSearchAtBallPositionPropagated::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    // now start search and init headpathplaner
    // not very nice...
    headControl.beginBallSearchAt((Vector2<double>) ballModel.propagated);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);

}

void GT2004BasicBehaviorBeginBallSearchAtBallPositionCommunicated::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    // now start search and init headpathplaner
    // not very nice...
    headControl.beginBallSearchAt((Vector2<double>) ballModel.communicated);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);

}


void GT2004BasicBehaviorFindBall::execute()
{
  double neckTilt, headPan, headTilt;
  bool headPanSide=headControl.headPanIsLeft();
  int jumped=0;

  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    Vector3<double> leftRightSweepTop,
                    leftRightSweepBottom,
                    halfLeftRightSweep,
                    halfLeftRightSweepBottom;

    if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    {
      if (headControlMode.headControlMode == HeadControlMode::searchForBallLeft)
        lastScanWasLeft = false;
      else if (headControlMode.headControlMode == HeadControlMode::searchForBallRight)
        lastScanWasLeft = true;
      else
        lastScanWasLeft=!headPanSide;
    }
    if (!lastScanWasLeft)
    {
      leftRightSweepTop = headControl.headLeft;
      leftRightSweepBottom = headControl.headLeftDown;
      halfLeftRightSweep = headControl.headMiddleLeft;
      halfLeftRightSweepBottom = headControl.headMiddleLeftDown;
    }
    else
    {
      leftRightSweepTop = headControl.headRight;
      leftRightSweepBottom = headControl.headRightDown;
      halfLeftRightSweep = headControl.headMiddleRight;
      halfLeftRightSweepBottom = headControl.headMiddleRightDown;
    }

    Vector3<double> points[]={headControl.headDown,
                              headControl.headDown,
                              halfLeftRightSweepBottom,
                              leftRightSweepBottom,
                              leftRightSweepTop,
                              halfLeftRightSweep,
                              headControl.headUp};
    long durations[] =   {100,40,80,120,80,200,120};
    
    // jump over positions, which are far away from aktual headposition

    Vector3<double> *ppoints = points;
    long *pdurations = durations;

    while (headControl.headPositionDistanceToActualPosition(points[jumped],headPanSide)<0 
           && jumped<3
           && !basicBehaviorWasActiveDuringLastExecutionOfEngine)
    {
      ppoints++;
      pdurations++;
      jumped++;
    }

    headPathPlanner.init(ppoints,pdurations,(sizeof(points)/sizeof(Vector3<double>))-jumped);
    lastScanWasLeft = !lastScanWasLeft;
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);
}

void GT2004BasicBehaviorReturnToBall::execute()
{
  double neckTilt, headPan, headTilt;
  headControl.getLookAtBallAngles(ballModel.seen, neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt);
}

void GT2004BasicBehaviorScanAwayFromBall::execute()
{
  double neckTilt, headPan, headTilt;
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    if (lastScanWasLeft)
    {
      Vector3<double> centerTop(   0, headPathPlanner.lastHeadPan-0.07, 0.25),
        rightTop(    0, max(-1.2,headPathPlanner.lastHeadPan-1.5),0),
        rightBottom( 0, max(-1.2,headPathPlanner.lastHeadPan-1.5),-0.25);
      Vector3<double>  points[3]={centerTop,rightTop,rightBottom};
      headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    }
    else
    {
      Vector3<double> centerTop(   0, headPathPlanner.lastHeadPan+0.07,min(headPathPlanner.lastHeadTilt+0.2,0.1)),
        leftTop(     0, min(1.2,headPathPlanner.lastHeadPan+1.5),0),
        leftBottom(  0, min(1.2,headPathPlanner.lastHeadPan+1.5),-0.25);
      Vector3<double> points[3]={centerTop,leftTop,leftBottom};
      headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    }
    lastScanWasLeft = !lastScanWasLeft;
  }  

  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);
}

void GT2004BasicBehaviorScanBackToBall::execute()
{
  double neckTilt, headPan, headTilt;
  headControl.getLookAtBallAngles(ballModel.seen, neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt, 4.0);
}

void GT2004BasicBehaviorGrabBall::execute()
{
  double neckTilt, headPan, headTilt;
  if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    // Change PID values for faster head motion
    pidData.setValues(JointData::neckTilt,0x20,0x08,0x0C);
    pidData.setValues(JointData::headPan,0x0A,0x08,0x0C);
    pidData.setValues(JointData::headTilt,0x0A,0x08,0x0C);

    Vector3<double> 
      prepare(-0.5, 0.0, 0.37),
      grab(   -1.0, 0.0, 0.6);
    Vector3<double> points[2]={prepare, grab};
    long durations[2]={50,50} ;
    headPathPlanner.init(points, durations,sizeof(points)/sizeof(Vector3<double>),false);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt); 
}

void GT2004BasicBehaviorReleaseBall::execute()
{
  double neckTilt, headPan, headTilt;
  if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    // Change PID values for faster head motion
    pidData.setValues(JointData::neckTilt,0x20,0x08,0x0C);
    pidData.setValues(JointData::headPan,0x0A,0x08,0x0C);
    pidData.setValues(JointData::headTilt,0x0A,0x08,0x0C);

    Vector3<double> 
      release(0.0, 0.0, 0.0);
    Vector3<double> points[1]={release};
    headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  if(!headPathPlanner.isLastPathFinished())
    headControl.setJointsDirect(neckTilt, headPan, headTilt); 
}

void GT2004BasicBehaviorWaitForGrab::execute()
{
  headControl.setJointsDirect(-0.5, 0.0, 0.37); 
}

void GT2004BasicBehaviorOtherHeadMovements::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }

  switch (headControlMode.headControlMode)
  {
    case HeadControlMode::openChallengeReset:
        pidData.setToDefaults();
      break;
    case HeadControlMode::openChallengeJoysickMode:
        pidData.setToDefaults();
        pidData.setValues(JointData::headPan,0,0,0);
        pidData.setValues(JointData::neckTilt,0,0,0);
        pidData.setValues(JointData::legFR1,0,0,0);
        pidData.setValues(JointData::legFR2,0,0,0);
        pidData.setValues(JointData::legFR3,0,0,0);
        pidData.setValues(JointData::legFL1,0,0,0);
        pidData.setValues(JointData::legFL2,0,0,0);
        pidData.setValues(JointData::legFL3,0,0,0);

        pidData.setValues(JointData::legHR1,0,0,0);
        pidData.setValues(JointData::legHR2,0,0,0);
        pidData.setValues(JointData::legHR3,0,0,0);
        pidData.setValues(JointData::legHL1,0,0,0);
        pidData.setValues(JointData::legHL2,0,0,0);
        pidData.setValues(JointData::legHL3,0,0,0);

        pidData.setValues(JointData::mouth,0,0,0);
        pidData.setValues(JointData::tailPan,0,0,0);
        pidData.setValues(JointData::tailTilt,0,0,0);
      break;
    case HeadControlMode::openChallengePullBridge:
        pidData.setToDefaults();
        pidData.setValues(JointData::headPan,0,0,0);
        pidData.setValues(JointData::neckTilt,0,0,0);
        pidData.setValues(JointData::mouth,0,0,0);
        headMotionRequest.tilt=153256;
        this->headMotionRequest.mouth = 0;
      break;
    case HeadControlMode::openChallengeTest:
        pidData.setToDefaults();
        pidData.setValues(JointData::neckTilt,0,0,0);
        this->headMotionRequest.mouth = -20000000;
      break;
    case HeadControlMode::openChallengeGoToBridge:
      pidData.setToDefaults();
      headControl.setJointsDirect(0.153,0,0.363,-2.0);
      break;
  case HeadControlMode::none:
    // do nothing
    break;
    
  case HeadControlMode::lookLeft:
    {
      Vector3<double> left(-0.55, 1.6, -0.3);
      lastScanWasLeft = true;
      if(headControl.lastHeadControlMode != headControlMode.headControlMode)
      {
        Vector3<double> points[]={headControl.headDown,left};
        headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
      }
      headPathPlanner.getAngles(neckTilt, headPan, headTilt);
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
    }
    break;  
    
  case HeadControlMode::lookRight:
    {
      Vector3<double> right(-0.55, -1.6, -0.3);

      lastScanWasLeft = false;
      if(headControl.lastHeadControlMode != headControlMode.headControlMode)
      {
        Vector3<double> points[]={headControl.headDown,right};
        headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
      }
      headPathPlanner.getAngles(neckTilt, headPan, headTilt);
      headControl.setJointsDirect(neckTilt, headPan, headTilt, 0);
    }
    break;  
    
  case HeadControlMode::searchForLandmarks:
    {
      //lastScanWasLeft = (headPathPlanner.lastHeadPan>0);
      //Vector3<double> left((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0),   1.5, 0.0);
      //Vector3<double> right((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0), -1.5, 0.0);
      //Vector3<double> center((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 2.0), 0.0, 0.0);
      
      if(headControl.lastHeadControlMode != headControlMode.headControlMode)
      {
        if(lastScanWasLeft)
        { 
          Vector3<double> points[2]={headControl.headLeft,headControl.headRight};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[3]={headControl.headRight, headControl.headUp, headControl.headLeft};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      
      if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
      {
        if(lastScanWasLeft)
        {
          Vector3<double> points[2]={headControl.headUp,headControl.headRight};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[2]={headControl.headUp, headControl.headLeft};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
      break;
    }
    
  case HeadControlMode::searchForLandmarksHeadLow:
    {
      Vector3<double> left((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0 - .8),   1.0, 0.5);
      Vector3<double> right((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0 - .8), -1.0, 0.5);
      Vector3<double> center((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 2.0 - .2), 0.0, 0.5);
      
      if(headControl.lastHeadControlMode != headControlMode.headControlMode)
      {
        if(lastScanWasLeft)
        { 
          Vector3<double> points[2]={left, right};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[3]={right, center, left};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      
      if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
      {
        if(lastScanWasLeft)
        {
          Vector3<double> points[2]={center, right};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[2]={center, left};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
      break;
    }
    
  case HeadControlMode::lookStraightAhead:
    headControl.setJointsDirect(0.0, 0.0, 0.0, 0.05); 
    break;  

  case HeadControlMode::lookTowardOpponentGoal:
    headControl.setJointsDirect(0.0, -robotPose.rotation, 0.0, 0.0); 
    break;  
    
  case HeadControlMode::lookBetweenFeet:
    headControl.setJointsDirect(-1.0, 0.0, 0.0); 
    break;  

  case HeadControlMode::lookToStars:
    headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
    break;
  case HeadControlMode::lookParallelToGround:
    if (bodyPosture.bodyTiltCalculatedFromAccelerationSensors>0)
      headControl.setJoints(0.0, 0.0, bodyPosture.bodyTiltCalculatedFromAccelerationSensors); 
    else
      headControl.setJoints(bodyPosture.bodyTiltCalculatedFromAccelerationSensors, 0.0, 0.0); 
    break;
    
  case HeadControlMode::direct:
    if (headPathPlanner.isLastPathFinished())
    {
      Vector3<double> point((double )headControlMode.directTilt/1000000.0,(double )headControlMode.directPan/1000000.0,(double )headControlMode.directRoll/1000000.0);
      Vector3<double> points[1]={point};
      headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 2000);
    }
    headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    headControl.setJointsDirect(neckTilt, headPan, headTilt);
    break;
    
  case HeadControlMode::stayAsForced:
    {  
      const double neckTiltTolerance = 0.06;
      const double headPanTolerance   = 0.06;
      const double headTiltTolerance = 0.05;
      
      double neckTiltDiff = sensorDataBuffer.lastFrame().data[SensorData::neckTilt] - headMotionRequest.tilt;
      double headPanDiff  = sensorDataBuffer.lastFrame().data[SensorData::headPan]  - headMotionRequest.pan;
      double headTiltDiff = sensorDataBuffer.lastFrame().data[SensorData::headTilt] - headMotionRequest.roll;
      
      if (neckTiltDiff > neckTiltTolerance)
      {
        headMotionRequest.tilt += (long)(neckTiltDiff-neckTiltTolerance);
      }
      else if (neckTiltDiff < -neckTiltTolerance)
      {
        headMotionRequest.tilt += (long)(neckTiltDiff+neckTiltTolerance);
      }
      
      if (headPanDiff > headPanTolerance)
      {
        headMotionRequest.pan += (long)(headPanDiff-headPanTolerance);
      }
      else if (headPanDiff < -headPanTolerance)
      {
        headMotionRequest.pan += (long)(headPanDiff+headPanTolerance);
      }
      
      if (headTiltDiff > headTiltTolerance)
      {
        headMotionRequest.roll += (long)(headTiltDiff-headTiltTolerance);
      }
      else if (headTiltDiff < -headTiltTolerance)
      {
        headMotionRequest.roll += (long)(headTiltDiff+headTiltTolerance);
      }
      break;
    }
    case HeadControlMode::watchOrigin:
      {
        Vector3<double> point(0,0,180);
        //its OK to start immediately here, because we scan smooth and in a way that we can always see it again:
        if (robotPose.getValidity()<0.5)
        {
          //if we dont know where to look, we look there smooth:
          if(headPathPlanner.isLastPathFinished())
          {
            unsigned long blindTime=SystemCall::getTimeSince(lastTimeOfGoodSL);
            double odoRot=currentOdometryData.rotation-lastOdometryData.rotation;
            if ((fabs(robotPose.rotation)<1.3)&&
                ((blindTime<500)||
                 ((fabs(odoRot)<0.004)&&(blindTime<1300))
                )
               )
            {
              //scan for origin
              if(headPathPlanner.isLastPathFinished())
              {
                if(lastScanWasLeft)
                {
                  headControl.simpleLookAtPointOnField(point,Vector2<int>(-65,0),neckTilt,headPan,headTilt);
                  Vector3<double> right(neckTilt,headPan,headTilt);
                  Vector3<double> points[1]={right};
                  headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
                }
                else
                {
                  headControl.simpleLookAtPointOnField(point,Vector2<int>(65,0),neckTilt,headPan,headTilt);
                  Vector3<double> left(neckTilt,headPan,headTilt);
                  Vector3<double> points[1]={left};
                  headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
                }
                lastScanWasLeft=!lastScanWasLeft;
              }
            }
            else if (odoRot==0)
            {
              //2do: somethimg usefull
            }
            else if (odoRot>0)
            {
              //if we can/will see the origin, then left
              
              lastScanWasLeft=true;
              static Vector3<double> point7(0.05,1.5 ,0.05);
              Vector3<double> points[1]={point7};
              headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 450);
            }
            else
            {
              //if we can/will see the origin, then right
              lastScanWasLeft=false;
              static Vector3<double> point7(0.05,-1.5 ,0.05);
              Vector3<double> points[1]={point7};
              headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 450);
            }
          }
          headPathPlanner.getAngles(neckTilt,headPan,headTilt);
          headControl.setJointsDirect(neckTilt,headPan,headTilt);
        }
        else
        {
          //if we know where to look, we look there immediately:
          if (robotPose.rotation<-1.45)
          {
            headControl.setJointsDirect(0.05, 1.5, 0.05);
          }
          else if (robotPose.rotation>1.45)
          {
            headControl.setJointsDirect(0.05, -1.5, 0.05);
          }
          else
          {
            double rota=-robotPose.speedbyDistanceToGoal*30;
            rota /= 2;
            headControl.simpleLookAtPointOnField(point,Vector2<int>((int)rota,0),neckTilt,headPan,headTilt);
            headControl.setJointsDirect(neckTilt,headPan,headTilt);
          }
        }
        break;
      }
  case HeadControlMode::calibrateHeadSpeed:
      if (headControl.calibrateHeadSpeedIsReady())
      {
        // look to the stars
        headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
      }
      else
      {
        headControl.calibrateHeadSpeed();
        headPathPlanner.getAngles(neckTilt,headPan,headTilt);
        headControl.setJointsDirect(neckTilt,headPan,headTilt);
      }
      break;

  default:
    errorHandler.error("GT2004BasicBehaviorOtherHeadMovements::execute(): head control mode \"%s\" is not implemented.", 
      HeadControlMode::getHeadControlModeName(headControlMode.headControlMode));
    break;
  }
  lastOdometryData = currentOdometryData;
}

/*
* Change Log
* 
* $Log: GT2004HeadControlBasicBehaviors.cpp,v $
* Revision 1.50  2004/07/02 13:24:23  jhoffman
* - scan for landmarks low (so stuff behind the boarders will not be seen!)
*
* Revision 1.49  2004/07/02 12:51:02  dassler
* searchForBallLeft -Right changed. FindBall will now do the job
*
* Revision 1.48  2004/07/02 10:27:03  jhoffman
* - preparation for compensating robot motions by appropriate head motion (no actual changes)
* - headControl-Mode added for testing
* - look-at-ball-and-closest-landmark uses quicker head movement
*
* Revision 1.47  2004/07/01 18:21:16  dassler
* Added BasicBehavior and HeadControlMode:
* search-for-ball-left
* search-for-ball-right
* Test is needed
*
* Revision 1.46  2004/06/30 16:16:15  jhoffman
* - state removed from "get-to-position-and-avoid-obstacles" because basic behavior now does the turning
* - basic behavior "go-to-point-and-avoide-obstacles" improved (faster, stops when destination is reached)
*
* Revision 1.45  2004/06/29 18:07:23  schmitt
* removed little error of last checkin
*
* Revision 1.44  2004/06/29 17:47:18  schmitt
* lookAtCloseLandmark updated, if no landmark is seen
*
* Revision 1.43  2004/06/29 17:06:53  dassler
* find ball scans first to the side where the head pan angle is smaller
*
* Revision 1.42  2004/06/29 10:17:00  dassler
* find ball choose side to scan dependent of headPan
*
* Revision 1.41  2004/06/28 17:49:04  schmitt
* changed LookAtCloseLandmark if no landmark can be found with current head position
*
* Revision 1.40  2004/06/28 15:21:28  dassler
* Head Positions corrected
*
* Revision 1.39  2004/06/28 14:00:04  jhoffman
* - scan back to ball slower
* - directed scan briefly stops at landmarks
* - input symbol set-joints-is-close-to-destination implemented and added to behavior
* - merged marcs changes into track-ball-strict
*
* Revision 1.38  2004/06/19 12:32:12  jhoffman
* - directed scan briefly stops before looking at the next landmark
* - comments added
*
* Revision 1.37  2004/06/18 18:28:39  dassler
* introduced basic-behavior:
* BeginBallSearchAtBallPositionSeen
* BeginBallSearchAtBallPositionCommunicated
* BeginBallSearchAtBallPositionPropagated
*
* track-ball modified and build in ball-just-lost
*
* Revision 1.36  2004/06/18 12:40:34  jhoffman
* - minor changes to headcontrol
* - debug drawing added
*
* Revision 1.35  2004/06/18 00:04:44  juengel
* Removed unused code.
*
* Revision 1.34  2004/06/17 22:02:12  dassler
* LookLeft/Right set to the the berlin gaze
* searchForBall headPath optimzied.
*
* Revision 1.33  2004/06/17 21:15:34  jhoffman
* *** empty log message ***
*
* Revision 1.32  2004/06/17 21:14:01  jhoffman
* *** empty log message ***
*
* Revision 1.31  2004/06/17 20:48:24  spranger
* - removed clipping in lookatball
* - removed y-clipping in getLookAtBallAngles
*
* Revision 1.30  2004/06/17 15:26:45  jhoffman
* various improvements to headcontrol
*
* Revision 1.29  2004/06/17 15:05:48  dassler
* GrabAndTurnWithBall is set to old headpathplaner
*
* Revision 1.27  2004/06/16 21:01:59  spranger
* reintroduced clipping (to lookatball)
*
* Revision 1.26  2004/06/16 18:14:00  jhoffman
* - search-for-landmarks-bug removed (setjointsdirect now sets "headPathPlanner.last...")
*
* Revision 1.25  2004/06/15 16:29:56  jhoffman
* check in for practice match
*
* Revision 1.24  2004/06/14 20:12:10  jhoffman
* - numerous changes and additions to headcontrol
* - cameraInfo default constructor now creates ERS7 info
* - debug drawing "headcontrolfield" added
*
* Revision 1.23  2004/06/11 16:28:46  juengel
* Added basic behavior release-ball.
*
* Revision 1.22  2004/06/09 20:03:11  juengel
* grab-ball uses headPathPlanner for intermediate state
*
* Revision 1.21  2004/06/08 17:42:50  jhoffman
* - xabsl dialog only requests debug keys to be send every 100 ms
* - added comments
* - look-at-ball misbehavior removed
*
* Revision 1.20  2004/06/05 16:02:49  jhoffman
* - added look-at-ball-and-closest-landmark (basicbahavior)
* - found and removed bug in simple-look-at-point
*
* Revision 1.19  2004/06/04 08:44:43  juengel
* Renamed simpleLookAtPoint to simpleLookAtPointOnField.
* Added simpleLookAtPointRelativeToRobot.
* Added ball clipping for balls inside robot.
*
* Revision 1.18  2004/06/01 15:05:41  jhoffman
* added "simple-look-at-ball" to gt2004headcontrol
*
* Revision 1.17  2004/05/29 18:18:56  dueffert
* walk parameter evolution, measurement and calibration stuff ported to GT2004_WM
*
* Revision 1.16  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.15  2004/05/26 17:21:47  dueffert
* better data types used
*
* Revision 1.14  2004/05/25 20:03:38  jhoffman
* replicated ath-gt-headcontrol
*
* Revision 1.13  2004/05/25 19:50:22  tim
* changed parameters
*
* Revision 1.12  2004/05/25 19:26:38  tim
* renamed basic behavior
*
* Revision 1.11  2004/05/25 18:30:12  jhoffman
* landmarks-array in field dimensions
* headcontrol looks at close landmark
*
* Revision 1.10  2004/05/25 18:10:30  tim
* changed parameters for grab-ball
*
* Revision 1.9  2004/05/25 17:41:27  tim
* added look-straight-ahead BasicBehavior
*
* Revision 1.8  2004/05/25 13:35:27  tim
* added grab-ball BasicBehavior
*
* Revision 1.7  2004/05/25 09:44:57  wachter
* Warnings fixed
*
* Revision 1.6  2004/05/24 21:47:58  dueffert
* someone wanted headpathplanner to use rad
*
* Revision 1.5  2004/05/24 18:19:43  jhoffman
* microrad --> rad
*
* Revision 1.4  2004/05/24 14:12:28  hamerla
* HaedControls für OpenChallenge
*
* Revision 1.3  2004/05/24 12:53:42  juengel
* bug fixed
*
* Revision 1.2  2004/05/23 22:49:14  loetzsch
* simplified basic behaviors
*
* Revision 1.1.1.1  2004/05/22 17:19:24  cvsadm
* created new repository GT2004_WM
*
* Revision 1.1  2004/05/18 13:40:00  loetzsch
* registered symbols and basic behaviors for GT2004HeadControl,
* renamed some states and basic behaviors
*
*/

---