Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file  Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in "basic-behaviors.xml".
*
* @author Martin L�tzsch
*/

#include "GT2005HeadControl.h"
#include "Tools/Math/Common.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"
#include "Representations/Perception/LandmarksPercept.h"

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

  engine.registerBasicBehavior(basicBehaviorHoldBall);
  engine.registerBasicBehavior(basicBehaviorOpenChallenge);
  engine.registerBasicBehavior(basicBehaviorNone);
  engine.registerBasicBehavior(basicBehaviorLookLeft);
  engine.registerBasicBehavior(basicBehaviorLookRight);
  engine.registerBasicBehavior(basicBehaviorScanForObstacles);
  engine.registerBasicBehavior(basicBehaviorSearchForLandmarks);
  engine.registerBasicBehavior(basicBehaviorSearchForLandmarksHeadLow);
  engine.registerBasicBehavior(basicBehaviorLookAtBluePinkLandmark);
  engine.registerBasicBehavior(basicBehaviorLookStraightAhead);
  engine.registerBasicBehavior(basicBehaviorLookTowardOpponentGoal);
  engine.registerBasicBehavior(basicBehaviorLookBetweenFeet);
  engine.registerBasicBehavior(basicBehaviorLookToStars);
  engine.registerBasicBehavior(basicBehaviorSnapAtFinger);
  engine.registerBasicBehavior(basicBehaviorLookParallelToGround);
  engine.registerBasicBehavior(basicBehaviorDirect);
  engine.registerBasicBehavior(basicBehaviorStayAsForced);
  engine.registerBasicBehavior(basicBehaviorWatchOrigin);
  engine.registerBasicBehavior(basicBehaviorCalibrateHeadSpeed);

  engine.registerBasicBehavior(basicBehaviorRealSlowScan);
  engine.registerBasicBehavior(basicBehaviorLookBetweenFeetForCarriedBall);
}

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

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


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

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


void GT2005BasicBehaviorLookAtMostInformativeLandmark::execute()
{
  double neckTilt, headPan, headTilt;
  double neckTilt180, headPan180, headTilt180;
  int closest, closest180, debugCircleColor = Drawings::red;

  if (robotPose.directionOfGreatestUncertaintyExists)
  {
    Pose2D uncertainty = robotPose.greatestUncertainty;
    Pose2D uncertainty180 = uncertainty;
    uncertainty180.rotation = normalize(uncertainty.rotation + pi);

    closest = headControl.calculateClosestLandmark(uncertainty, 0, 0, true);
    closest180 = headControl.calculateClosestLandmark(uncertainty180, 0, 0, true);
  
    headControl.aimAtLandmark(closest, neckTilt, headPan, headTilt);
    headControl.aimAtLandmark(closest180, neckTilt180, headPan180, headTilt180);
  
    if(fabs(headPan) > fabs(headPan180))
    {
      closest = closest180;
    }
  }
  else
  {
    debugCircleColor = Drawings::blue;
    closest = headControl.calculateClosestLandmark();
  }

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

  //NDECLARE_DEBUGDRAWING("gt05headcontrol:mostInformativeLandmark", "drawingOnField", "marks the landmark that the robot is looking at when using head control mode *look at most informative landmark*");
  CIRCLE(headControlField,
    targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 200, 
    2, 0, debugCircleColor);
  DEBUG_DRAWING_FINISHED(headControlField);  

}

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

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

    Vector3<double> ballPosition3d;     
    ballPosition3d.x = (double) ballModel.seen.positionField.x;
    ballPosition3d.y = (double) ballModel.seen.positionField.y;
    ballPosition3d.z = 0;
    Vector2<double> toBall = ballModel.seen.positionField - 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 GT2005BasicBehaviorLookAtCloseLandmark::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);

  //NDECLARE_DEBUGDRAWING("gt05headcontrol:closeLandmark", "drawingOnField", "marks the close(st) landmark that the robot is looking at *look at close landmark*");

  CIRCLE(headControlField, 
    targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 150, 
    2, 0, 2);

  DEBUG_DRAWING_FINISHED(headControlField);  
}


void GT2005BasicBehaviorDirectedScanForLandmarks::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 GT2005BasicBehaviorDirectedScanForObstacles::execute()
{
  double neckTilt = 0, headPan = 0, headPanDummy, headTilt = 0;
  
  /*
  Outline/Ideas/To-do's

  - gaze direction (tilt): closest obstacle in visible sector
  - inherit scan direction from directed scan for landmarks
  - scan around center which is determined by the current walk request
  - left/right scan amplitude is determined by the proximity of obstacles (near: only scan where the robot is headed, far: wide scan)

  BUGS:

  - motionRequest does not contain valid data (not filled?)

  */
 
  // init
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    leftOrRight *= -1;
    lastScanWasLeft = (leftOrRight > 0);
    lastPan = headPathPlanner.lastHeadPan;
  }
  
  // first determine the direction (center) of the scan
  Pose2D predictedMotionRequestDirection,
    startingPoint(robotPose), 
    walkParams(motionRequest.walkRequest.walkParams);

  walkParams.translation /= 5;
  walkParams.rotation /= 5;

  if(walkParams.translation.x == 0) 
    walkParams.translation.x = 0.1;
  
  int i;
  for(i = 0; i < 5; i++) // predict where the robot will be in the near future
  {
    predictedMotionRequestDirection = startingPoint + walkParams;

    LINE(headControlField, 
      startingPoint.translation.x, startingPoint.translation.y, 
      predictedMotionRequestDirection.translation.x, predictedMotionRequestDirection.translation.y,     
      2, 0, 3);

    startingPoint = predictedMotionRequestDirection;
  }
  double centerOfScan = Geometry::angleTo(robotPose, predictedMotionRequestDirection.translation);

  double directionOfClosestObstacle = -1.5, 
    currentDirection,
    currentFreeSpace,
    minFreeSpace = 10000;

  for(currentDirection = -1.4; currentDirection <= 1.4; currentDirection += .2)
  {
    currentFreeSpace = obstaclesModel.getDistanceInDirection(currentDirection, .2);
    if (currentFreeSpace < minFreeSpace)
    {
      minFreeSpace = currentFreeSpace;
      directionOfClosestObstacle = currentDirection;
    }
  }
  if (fabs(directionOfClosestObstacle) == 1.4)
    directionOfClosestObstacle = 0;

  centerOfScan += directionOfClosestObstacle;
  centerOfScan /= 2;

  // scan amplitude as a function of the free space
  double scanAmplitude = .7, stepSize = .025, scanAmplitudeRatio = 1000;
  double freeSpaceInWalkDirection = obstaclesModel.getDistanceInDirection(centerOfScan, 1.2);

  // debug output
    Pose2D lineEndPoint(0, freeSpaceInWalkDirection*cos(centerOfScan), freeSpaceInWalkDirection*sin(centerOfScan));
    lineEndPoint = robotPose + lineEndPoint;

    LINE(headControlField, 
      robotPose.translation.x, robotPose.translation.y, 
      lineEndPoint.translation.x, lineEndPoint.translation.y,     
      2, 0, 5);
    

  MODIFY("headControl:directedScanForObstacles:scanAmplitude", scanAmplitude);
  MODIFY("headControl:directedScanForObstacles:stepAmplitudeRatio", scanAmplitudeRatio);
  MODIFY("headControl:directedScanForObstacles:stepSize", stepSize);

  double ratio = freeSpaceInWalkDirection/scanAmplitudeRatio;
  if (ratio > 1) ratio = 1;
  if ((walkParams.translation.x < 10) && (walkParams.translation.y < 10) && (walkParams.rotation < 10))
  {
    ratio = 1;
    centerOfScan = 0;
    scanAmplitude = 1.5;
  }
  scanAmplitude *= ratio;
  
  Range<double> panRange(-1.5, 1.5);
  
  headPan = lastPan + leftOrRight * stepSize;
  lastPan = headPan;
  
  if (headPan > panRange.limit(centerOfScan + scanAmplitude))
    leftOrRight = -1;
  else if (headPan < panRange.limit(centerOfScan - scanAmplitude))
    leftOrRight = +1;

  double length = obstaclesModel.getDistanceInDirection(headPan, .1);
  Pose2D pointOfClosestObstacle(0, length*cos(headPan), length*sin(headPan));
  pointOfClosestObstacle = robotPose + pointOfClosestObstacle;
  
    LINE(headControlField, 
      robotPose.translation.x, robotPose.translation.y, 
      pointOfClosestObstacle.translation.x, pointOfClosestObstacle.translation.y,     
      2, 0, 1);
    DEBUG_DRAWING_FINISHED(headControlField);  

  headControl.simpleLookAtPointOnField(
      Vector3<double>(pointOfClosestObstacle.translation.x, pointOfClosestObstacle.translation.y, 0), 
      Vector2<int>(0, 0), 
      neckTilt, headPanDummy, headTilt);  
  headControl.setJoints(neckTilt, headPan, headTilt, 4);
}


void GT2005BasicBehaviorLookAtBallAndClosestLandmark::execute()
{
  double neckTiltBall, headPanBall, headTiltBall;
  headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTiltBall, headPanBall, headTiltBall);
  
  double neckTiltLM, headPanLM, headTiltLM;
  int closest = headControl.calculateClosestLandmark(Geometry::angleTo(robotPose, ballModel.seen.positionField));
  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.positionField-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.positionField)).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 GT2005BasicBehaviorBeginBallSearchAtBallPositionSeen::execute()
{
  double neckTilt, headPan, headTilt;
  if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
  {
    // now start search and init headpathplaner
    // not very nice...
    headControl.beginBallSearchAt((Vector2<double>) ballModel.seen.positionField);
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt);

}

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

}

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

}


void GT2005BasicBehaviorFindBall::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 GT2005BasicBehaviorReturnToBall::execute()
{
  double neckTilt, headPan, headTilt;
  headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt);
}

void GT2005BasicBehaviorScanAwayFromBall::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 GT2005BasicBehaviorScanBackToBall::execute()
{
  double neckTilt, headPan, headTilt;
  headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTilt, headPan, headTilt);
  headControl.setJoints(neckTilt, headPan, headTilt, 4.0);
}

void GT2005BasicBehaviorGrabBall::execute()
{
  double neckTilt, headPan, headTilt;
    // 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);
  if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {

    Vector3<double> 
      prepare(-0.5, 0.0, 0.37),
      grab(   -1.1, 0.0, 0.7);
    
    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); 
  this->headMotionRequest.mouth = 0;

}


void GT2005BasicBehaviorGrabBallHigh::execute()
{
  double neckTilt, headPan, headTilt;
    // 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);
  if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
  this->headMotionRequest.mouth = 0;
    Vector3<double> 
      prepare(   -0.5, 0.0, 0.37),
      grab(      -1.1, 0.0, 0.7),
    grabhigh ( -1.0, 0.0, 1.1);
    Vector3<double> points[3]={prepare, grab, grabhigh};
    long durations[3]={130,130,100} ;
    headPathPlanner.init (points, durations,sizeof(points)/sizeof(Vector3<double>),false);
  timeOfLastExecution = SystemCall::getCurrentSystemTime();
  }
  headPathPlanner.getAngles(neckTilt, headPan, headTilt);
  headControl.setJointsDirect(neckTilt, headPan, headTilt); 
  
  if(SystemCall::getCurrentSystemTime() - timeOfLastExecution > 360)
  this->headMotionRequest.mouth = -1000000;
  else  this->headMotionRequest.mouth = 0;

}


void GT2005BasicBehaviorReleaseBall::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 GT2005BasicBehaviorWaitForGrab::execute()
{
  headControl.setJointsDirect(-0.5, 0.0, 0.37); 
}

void GT2005BasicBehaviorHoldBall::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
          pidData.setToDefaults();
//        pidData.setValues(JointData::mouth,0x02,0x01,0x02);
        pidData.setValues(JointData::neckTilt,0x00,0x00,0x00);
        this->headMotionRequest.tilt  = -1400000;
        this->headMotionRequest.pan   =        0;
        this->headMotionRequest.roll  =   880000;
        this->headMotionRequest.mouth = -1500000;
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorOpenChallenge::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
  switch (headControlMode.headControlMode)
  {     
case HeadControlMode::openChallengeReset:
        pidData.setToDefaults();
//        pidData.setValues(JointData::mouth,0x02,0x01,0x02);
        pidData.setValues(JointData::mouth,0x01,0x08,0x01);
        this->headMotionRequest.mouth = -1500000;
      break;
    case HeadControlMode::openChallengeJoysickMode:
        pidData.setToDefaults();
        pidData.setValues(JointData::mouth,0x01,0x00,0x00);
        this->headMotionRequest.mouth = -1500000;
      break;
    case HeadControlMode::openChallengePullBridge:
        pidData.setToDefaults();
        pidData.setValues(JointData::mouth,0x01,0x01,0x08);
        this->headMotionRequest.mouth = -1500000;
      break;
    case HeadControlMode::openChallengeTest:
        //sensorDataBuffer.frame[0]
        pidData.setToDefaults();
        //pidData.setValues(JointData::neckTilt,0,0,0);
        pidData.setValues(JointData::mouth,0,0,0);
        pidData.setValues(JointData::mouth,0x02,0x01,0x02);
        this->headMotionRequest.mouth = -1500000;
      break;
    case HeadControlMode::openChallengeGoToBridge:
        pidData.setToDefaults();
        pidData.setValues(JointData::mouth,0x01,0x00,0x00);
        headControl.setJointsDirect(-1, 0, 0.6);
        this->headMotionRequest.mouth = -1500000;
      break;
  default:
    errorHandler.error("GT2005BasicBehaviorOtherHeadMovements::execute(): head control mode \"%s\" is not implemented.", 
      HeadControlMode::getHeadControlModeName(headControlMode.headControlMode));
    break;
  }
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorNone::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      // do nothing
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookLeft::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
        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);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookRight::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
        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);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorScanForObstacles::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
        Vector3<double> left(0.0,.50,.25), middle(0.0,0,.25), right(0.0,-.50,.25);
      Vector3<double> leftLow(-.5,.50,.25), middleLow(-.5,0,.25), rightLow(-.5,-.50,.25);

      if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
      {
        if(lastScanWasLeft)
        {
          Vector3<double> points[3]={left,middle, right};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 600);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[3]={rightLow,middleLow, leftLow};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 600);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorRealSlowScan::execute()
//  case HeadControlMode::realSlowScan:
    {
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }

  Vector3<double> left(0.0,pi_2,0.0), middle(0.0,0.0,0.0), right(0.0,-pi_2,0.0);

      if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
      {
        if(lastScanWasLeft)
        {
          Vector3<double> points[3]={left,middle, right};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 8000);
          lastScanWasLeft = !lastScanWasLeft;
        }
        else
        {
          Vector3<double> points[3]={right, middle, left};
          headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 8000);
          lastScanWasLeft = !lastScanWasLeft;
        }
      }
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
    }
 
void GT2005BasicBehaviorLookBetweenFeetForCarriedBall::execute()
//  case HeadControlMode::lookBetweenFeetForCarriedBall:
    {
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }

  Vector3<double> 
        //t0(-1.1, 0.0, 0.7), // carry ball starting position 
        t1(-0.55, 0.0, 0.8), // move head up to avoid accidental pushing of the ball
        t2(-0.25, 0.0, 0.3), // move head back 
        t3(-0.1, 0.0, -0.4);

      if(headControl.lastHeadControlMode != headControlMode.headControlMode)
      {
        Vector3<double> points[]={t1, t2, t3, t3};
        long duration[] = {80, 120, 10, 2080};
        //headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 500);
        headPathPlanner.init(points, duration, sizeof(points)/sizeof(Vector3<double>), false);
      }
      headPathPlanner.getAngles(neckTilt, headPan, headTilt);
      headControl.setJointsDirect(neckTilt, headPan, headTilt);
    }
    
void GT2005BasicBehaviorSearchForLandmarks::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      //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);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorSearchForLandmarksHeadLow::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
        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);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookAtBluePinkLandmark::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
    double angleToLandmark = 0;
    double distanceToLandmark = 1000;
    for(int i = 0; i < landmarksPercept.numberOfFlags; i++)
    {
      if(landmarksPercept.flags[i].type == Flag::skyblueAbovePink)
      {
        angleToLandmark = landmarksPercept.flags[i].angle;
        distanceToLandmark = landmarksPercept.flags[i].distance;
      }
    }

    double flagx = cos(angleToLandmark) * distanceToLandmark;
    double flagy = sin(angleToLandmark) * distanceToLandmark;

    headControl.simpleLookAtPointRelativeToRobot(
    Vector3<double>(flagx, flagy, 300), 
    Vector2<int>(0, 0), 
    neckTilt, headPan, headTilt);

    headControl.setJointsDirect(neckTilt, headPan, headTilt);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookStraightAhead::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      pidData.setToDefaults();
    headControl.setJointsDirect(0.0, 0.0, 0.0, 0.05); 
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookTowardOpponentGoal::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      headControl.setJointsDirect(0.0, -robotPose.rotation, 0.0, 0.0); 
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookBetweenFeet::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      headControl.setJointsDirect(-1.0, 0.0, 0.0); 
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookToStars::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorSnapAtFinger::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      if(sensorDataBuffer.frame[0].mouth == 0)
    {
      headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, 0);
    }
    else
    {
      headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
    }
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorLookParallelToGround::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      if (bodyPosture.bodyTiltCalculatedFromAccelerationSensors>0)
      headControl.setJoints(0.0, 0.0, bodyPosture.bodyTiltCalculatedFromAccelerationSensors); 
    else
      headControl.setJoints(bodyPosture.bodyTiltCalculatedFromAccelerationSensors, 0.0, 0.0); 
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorDirect::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      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);
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorStayAsForced::execute()
{
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
      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);
      }
  lastOdometryData = currentOdometryData;
}

void GT2005BasicBehaviorWatchOrigin::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
  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 const 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 const 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);
    }
  }
  lastOdometryData = currentOdometryData;
}
void GT2005BasicBehaviorCalibrateHeadSpeed::execute()
{
  double neckTilt, headPan, headTilt;
  if (robotPose.getValidity()>0.5)
  {
    lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
  }
  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);
  }
  lastOdometryData = currentOdometryData;
}

---