Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file  Modules/HeadControl/GT2005HeadControl/GT2005HeadControlBasicBehaviors.cpp
    00003 *
    00004 * Implementation of basic behaviors defined in "basic-behaviors.xml".
    00005 *
    00006 * @author Martin L�tzsch
    00007 */
    00008
    00009 #include "GT2005HeadControl.h"
    00010 #include "Tools/Math/Common.h"
    00011 #include "Tools/Math/Geometry.h"
    00012 #include "Tools/FieldDimensions.h"
    00013 #include "Representations/Perception/LandmarksPercept.h"
    00014
    00015 void GT2005HeadControlBasicBehaviors::registerBasicBehaviors(Xabsl2Engine& engine)
    00016 {
    00017   engine.registerBasicBehavior(basicBehaviorLookAtMostInformativeLandmark);
    00018   engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionSeen);
    00019   engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionPropagated);
    00020   engine.registerBasicBehavior(basicBehaviorBeginBallSearchAtBallPositionCommunicated);
    00021   engine.registerBasicBehavior(basicBehaviorLookAroundAtSeenBall);
    00022   engine.registerBasicBehavior(basicBehaviorFindBall);
    00023   engine.registerBasicBehavior(basicBehaviorLookAtBall);
    00024   engine.registerBasicBehavior(basicBehaviorLookAtCloseLandmark);
    00025   engine.registerBasicBehavior(basicBehaviorLookAtBallAndClosestLandmark);
    00026   engine.registerBasicBehavior(basicBehaviorReturnToBall);
    00027   engine.registerBasicBehavior(basicBehaviorDirectedScanForLandmarks);
    00028   engine.registerBasicBehavior(basicBehaviorDirectedScanForObstacles);
    00029   engine.registerBasicBehavior(basicBehaviorScanBackToBall);
    00030   engine.registerBasicBehavior(basicBehaviorScanAwayFromBall);
    00031   engine.registerBasicBehavior(basicBehaviorGrabBall);
    00032   engine.registerBasicBehavior(basicBehaviorGrabBallHigh);
    00033   engine.registerBasicBehavior(basicBehaviorReleaseBall);
    00034   engine.registerBasicBehavior(basicBehaviorWaitForGrab);
    00035   engine.registerBasicBehavior(basicBehaviorSearchForBallLeft);
    00036   engine.registerBasicBehavior(basicBehaviorSearchForBallRight);
    00037
    00038   engine.registerBasicBehavior(basicBehaviorHoldBall);
    00039   engine.registerBasicBehavior(basicBehaviorOpenChallenge);
    00040   engine.registerBasicBehavior(basicBehaviorNone);
    00041   engine.registerBasicBehavior(basicBehaviorLookLeft);
    00042   engine.registerBasicBehavior(basicBehaviorLookRight);
    00043   engine.registerBasicBehavior(basicBehaviorScanForObstacles);
    00044   engine.registerBasicBehavior(basicBehaviorSearchForLandmarks);
    00045   engine.registerBasicBehavior(basicBehaviorSearchForLandmarksHeadLow);
    00046   engine.registerBasicBehavior(basicBehaviorLookAtBluePinkLandmark);
    00047   engine.registerBasicBehavior(basicBehaviorLookStraightAhead);
    00048   engine.registerBasicBehavior(basicBehaviorLookTowardOpponentGoal);
    00049   engine.registerBasicBehavior(basicBehaviorLookBetweenFeet);
    00050   engine.registerBasicBehavior(basicBehaviorLookToStars);
    00051   engine.registerBasicBehavior(basicBehaviorSnapAtFinger);
    00052   engine.registerBasicBehavior(basicBehaviorLookParallelToGround);
    00053   engine.registerBasicBehavior(basicBehaviorDirect);
    00054   engine.registerBasicBehavior(basicBehaviorStayAsForced);
    00055   engine.registerBasicBehavior(basicBehaviorWatchOrigin);
    00056   engine.registerBasicBehavior(basicBehaviorCalibrateHeadSpeed);
    00057
    00058   engine.registerBasicBehavior(basicBehaviorRealSlowScan);
    00059   engine.registerBasicBehavior(basicBehaviorLookBetweenFeetForCarriedBall);
    00060 }
    00061
    00062 void GT2005BasicBehaviorLookAtBall::execute()
    00063 {
    00064   // trust the communicated ball if it get lost
    00065   headControl.useCommunicatedBall = true;
    00066
    00067   double neckTilt, headPan, headTilt;
    00068   headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTilt, headPan, headTilt);
    00069   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00070 }
    00071
    00072
    00073 void GT2005BasicBehaviorSearchForBallLeft::execute()
    00074 {
    00075   double neckTilt, headPan, headTilt;
    00076   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00077   {
    00078     headControl.searchForBallLeft();
    00079   }
    00080   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00081   headControl.setJoints(neckTilt, headPan, headTilt);
    00082 }
    00083
    00084 void GT2005BasicBehaviorSearchForBallRight::execute()
    00085 {
    00086   double neckTilt, headPan, headTilt;
    00087   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00088   {
    00089     headControl.searchForBallRight();
    00090   }
    00091   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00092   headControl.setJoints(neckTilt, headPan, headTilt);
    00093 }
    00094
    00095
    00096 void GT2005BasicBehaviorLookAtMostInformativeLandmark::execute()
    00097 {
    00098   double neckTilt, headPan, headTilt;
    00099   double neckTilt180, headPan180, headTilt180;
    00100   int closest, closest180, debugCircleColor = Drawings::red;
    00101
    00102   if (robotPose.directionOfGreatestUncertaintyExists)
    00103   {
    00104     Pose2D uncertainty = robotPose.greatestUncertainty;
    00105     Pose2D uncertainty180 = uncertainty;
    00106     uncertainty180.rotation = normalize(uncertainty.rotation + pi);
    00107
    00108     closest = headControl.calculateClosestLandmark(uncertainty, 0, 0, true);
    00109     closest180 = headControl.calculateClosestLandmark(uncertainty180, 0, 0, true);
    00110
    00111     headControl.aimAtLandmark(closest, neckTilt, headPan, headTilt);
    00112     headControl.aimAtLandmark(closest180, neckTilt180, headPan180, headTilt180);
    00113
    00114     if(fabs(headPan) > fabs(headPan180))
    00115     {
    00116       closest = closest180;
    00117     }
    00118   }
    00119   else
    00120   {
    00121     debugCircleColor = Drawings::blue;
    00122     closest = headControl.calculateClosestLandmark();
    00123   }
    00124
    00125   headControl.aimAtLandmark(closest, neckTilt, headPan, headTilt);
    00126   headControl.setJoints(neckTilt, headPan, headTilt, 4.0);
    00127
    00128   //NDECLARE_DEBUGDRAWING("gt05headcontrol:mostInformativeLandmark", "drawingOnField", "marks the landmark that the robot is looking at when using head control mode *look at most informative landmark*");
    00129   CIRCLE(headControlField,
    00130     targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 200,
    00131     2, 0, debugCircleColor);
    00132   DEBUG_DRAWING_FINISHED(headControlField);
    00133
    00134 }
    00135
    00136 void GT2005BasicBehaviorLookAroundAtSeenBall::execute()
    00137 {
    00138   double neckTilt, headPan, headTilt;
    00139
    00140   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00141   {
    00142     Vector3<double> leftDown,
    00143                     rightDown,
    00144                     leftTop,
    00145                     rightTop;
    00146
    00147     Vector3<double> ballPosition3d;
    00148     ballPosition3d.x = (double) ballModel.seen.positionField.x;
    00149     ballPosition3d.y = (double) ballModel.seen.positionField.y;
    00150     ballPosition3d.z = 0;
    00151     Vector2<double> toBall = ballModel.seen.positionField - robotPose.translation;
    00152
    00153     // center the ball view in the middle of the image
    00154     // if the ball is near, the ball should be seen, if we look halfway down
    00155     // constante definition of distance to ball
    00156
    00157     Vector2<int> cameraImageOffset(0,0);
    00158
    00159     double neckTilt,headPan,headTilt;
    00160     headControl.simpleLookAtPointOnField(ballPosition3d,cameraImageOffset,neckTilt,headPan,headTilt);
    00161     Vector3<double> ballAngles (neckTilt,headPan,headTilt);
    00162
    00163
    00164     //headControl.getSensorHeadAngles(leftDown);
    00165     leftDown = ballAngles;
    00166     rightDown = leftDown;
    00167     leftTop = leftDown;
    00168     rightDown = leftDown;
    00169
    00170     leftDown.y += 0.6;
    00171     //leftDown.x -= 0.2;
    00172
    00173     rightDown.y -= 0.6;
    00174     //rightDown.x -= 0.2;
    00175
    00176     leftTop.y += 0.6;
    00177     leftTop.x += 0.2;
    00178
    00179     rightTop.y -= 0.6;
    00180     rightTop.x += 0.2;
    00181
    00182     Vector3<double> points[]={leftDown,
    00183                               rightDown,
    00184                               rightTop,
    00185                               leftTop};
    00186     long durations[] =   {100,100,100,100};
    00187
    00188     headPathPlanner.init(points,durations,(sizeof(points)/sizeof(Vector3<double>)),false);
    00189
    00190   }
    00191   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00192   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00193
    00194 }
    00195
    00196 void GT2005BasicBehaviorLookAtCloseLandmark::execute()
    00197 {
    00198   double neckTilt, headPan, headTilt;
    00199   int closest = headControl.calculateClosestLandmark(headPathPlanner.lastHeadPan, 0);
    00200
    00201   // scan sidewards beginning with side on which the head is actually oriented
    00202   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00203   {
    00204   panCount = 0;
    00205   lastScanWasLeft = (headPathPlanner.lastHeadPan < 0);
    00206   }
    00207
    00208   // Only for testing
    00209   // closest = -1;
    00210
    00211   if (closest == -1 && panCount < 2)
    00212   {
    00213     OUTPUT (idText, text, "Looking at closest landmark...");
    00214     headPathPlanner.getAngles (neckTilt, headPan, headTilt);
    00215
    00216     if (headPan > headControl.headMiddleLeft.y)
    00217     {
    00218       lastScanWasLeft = true;
    00219       panCount++;
    00220       headControl.setJoints (headControl.headRight.x, headControl.headRight.y, headControl.headRight.z);
    00221     }
    00222     else if (headPan > headControl.headMiddleRight.y)
    00223     {
    00224       lastScanWasLeft = false;
    00225       panCount++;
    00226       headControl.setJoints (headControl.headLeft.x, headControl.headLeft.y, headControl.headLeft.z);
    00227     }
    00228     else if (!lastScanWasLeft)
    00229       headControl.setJoints (headControl.headLeft.x, headControl.headLeft.y, headControl.headLeft.z);
    00230     else
    00231       headControl.setJoints (headControl.headRight.x, headControl.headRight.y, headControl.headRight.z);
    00232
    00233     headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00234     closest = headControl.calculateClosestLandmark (headPan, 0);
    00235   }
    00236
    00237   headControl.aimAtLandmark(closest, neckTilt, headPan, headTilt);
    00238
    00239   headControl.setJoints(neckTilt, headPan, headTilt, 4.0);
    00240
    00241   //NDECLARE_DEBUGDRAWING("gt05headcontrol:closeLandmark", "drawingOnField", "marks the close(st) landmark that the robot is looking at *look at close landmark*");
    00242
    00243   CIRCLE(headControlField,
    00244     targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 150,
    00245     2, 0, 2);
    00246
    00247   DEBUG_DRAWING_FINISHED(headControlField);
    00248 }
    00249
    00250
    00251 void GT2005BasicBehaviorDirectedScanForLandmarks::execute()
    00252 {
    00253   double neckTilt, headPan, headTilt;
    00254
    00255   /** alternate scan direction whenever basic behavior is called for the first time */
    00256   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00257   {
    00258     leftOrRight *= -1;
    00259     lastScanWasLeft = (leftOrRight > 0);
    00260   }
    00261
    00262
    00263   /** The following calculates the closest landmark and compares it to the
    00264   one that the scan is currently aiming at. If the closest is further in the
    00265   direction of the scan, it is used as the new target. If it is in the opposite
    00266   direction, it is not used to guarantee a stable head movement */
    00267
    00268   int closest = headControl.calculateClosestLandmark(headPathPlanner.lastHeadPan + leftOrRight*.1, leftOrRight);
    00269
    00270   /** reset variables when a new scan is started */
    00271   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00272   {
    00273     currentLandmark = closest;
    00274     nextLandmark = -1;
    00275     waitSomeBeforeLookingAtNextLandmark = 0;
    00276   }
    00277
    00278   /** if a landmark is found to be closest that is not the
    00279   one the robot is currently aiming at, store the ID in nextLandmark
    00280   but don't change the target yet. */
    00281   if (closest != nextLandmark)
    00282   {
    00283     if ((nextLandmark != -1) || (closest != -1))
    00284     {
    00285       nextLandmark = closest;
    00286       waitSomeBeforeLookingAtNextLandmark = 1;
    00287     }
    00288 //    else
    00289 //    {
    00290 //      double angleToClosest = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y));
    00291 //      double angleToCurrent = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y));
    00292 //      if ((angleToCurrent - angleToClosest)*leftOrRight < 0)
    00293 //        nextLandmark = closest;
    00294 //      waitSomeBeforeLookingAtNextLandmark = 1;
    00295 //    }
    00296   }
    00297
    00298   /** the following functions as a delay so that
    00299   the robot continues to look at a landmark for a
    00300   brief period of time before aiming at the next */
    00301   if (waitSomeBeforeLookingAtNextLandmark)
    00302   {
    00303     CIRCLE(headControlField,
    00304     targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y, 100,
    00305     2, 0, Drawings::red);
    00306     waitSomeBeforeLookingAtNextLandmark++;
    00307   }
    00308
    00309   /* after the robot has aimed at the previous/old target for
    00310   more than 8 secs, select new target */
    00311   if (waitSomeBeforeLookingAtNextLandmark > 15)
    00312   {
    00313     currentLandmark = nextLandmark;
    00314     waitSomeBeforeLookingAtNextLandmark = -1;
    00315   }
    00316
    00317   CIRCLE(headControlField,
    00318     targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y, 80,
    00319     2, 0, Drawings::yellow);
    00320
    00321   DEBUG_DRAWING_FINISHED(headControlField);
    00322
    00323   /** calculate the XABSL input symbol "nextLandmarkIsWithinReach" */
    00324   double angleToCurrent = Geometry::angleTo(robotPose, Vector2<double>(targetPointOnLandmark[currentLandmark].position.x, targetPointOnLandmark[currentLandmark].position.y));
    00325   if((closest == -1) ||
    00326     (fabs(angleToCurrent) > headControl.jointLimitHeadPanP + cameraInfo.openingAngleWidth/2))
    00327   {
    00328     nextLandmarkIsWithinReach = false;
    00329   }
    00330   else
    00331   {
    00332     nextLandmarkIsWithinReach = true;
    00333   }
    00334
    00335   if (currentLandmark != -1)
    00336     headControl.aimAtLandmark(currentLandmark, neckTilt, headPan, headTilt);
    00337
    00338   headControl.setJoints(neckTilt, headPan, headTilt, 4);
    00339 }
    00340
    00341
    00342
    00343
    00344 void GT2005BasicBehaviorDirectedScanForObstacles::execute()
    00345 {
    00346   double neckTilt = 0, headPan = 0, headPanDummy, headTilt = 0;
    00347
    00348   /*
    00349   Outline/Ideas/To-do's
    00350
    00351   - gaze direction (tilt): closest obstacle in visible sector
    00352   - inherit scan direction from directed scan for landmarks
    00353   - scan around center which is determined by the current walk request
    00354   - left/right scan amplitude is determined by the proximity of obstacles (near: only scan where the robot is headed, far: wide scan)
    00355
    00356   BUGS:
    00357
    00358   - motionRequest does not contain valid data (not filled?)
    00359
    00360   */
    00361
    00362   // init
    00363   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00364   {
    00365     leftOrRight *= -1;
    00366     lastScanWasLeft = (leftOrRight > 0);
    00367     lastPan = headPathPlanner.lastHeadPan;
    00368   }
    00369
    00370   // first determine the direction (center) of the scan
    00371   Pose2D predictedMotionRequestDirection,
    00372     startingPoint(robotPose),
    00373     walkParams(motionRequest.walkRequest.walkParams);
    00374
    00375   walkParams.translation /= 5;
    00376   walkParams.rotation /= 5;
    00377
    00378   if(walkParams.translation.x == 0)
    00379     walkParams.translation.x = 0.1;
    00380
    00381   int i;
    00382   for(i = 0; i < 5; i++) // predict where the robot will be in the near future
    00383   {
    00384     predictedMotionRequestDirection = startingPoint + walkParams;
    00385
    00386     LINE(headControlField,
    00387       startingPoint.translation.x, startingPoint.translation.y,
    00388       predictedMotionRequestDirection.translation.x, predictedMotionRequestDirection.translation.y,
    00389       2, 0, 3);
    00390
    00391     startingPoint = predictedMotionRequestDirection;
    00392   }
    00393   double centerOfScan = Geometry::angleTo(robotPose, predictedMotionRequestDirection.translation);
    00394
    00395   double directionOfClosestObstacle = -1.5,
    00396     currentDirection,
    00397     currentFreeSpace,
    00398     minFreeSpace = 10000;
    00399
    00400   for(currentDirection = -1.4; currentDirection <= 1.4; currentDirection += .2)
    00401   {
    00402     currentFreeSpace = obstaclesModel.getDistanceInDirection(currentDirection, .2);
    00403     if (currentFreeSpace < minFreeSpace)
    00404     {
    00405       minFreeSpace = currentFreeSpace;
    00406       directionOfClosestObstacle = currentDirection;
    00407     }
    00408   }
    00409   if (fabs(directionOfClosestObstacle) == 1.4)
    00410     directionOfClosestObstacle = 0;
    00411
    00412   centerOfScan += directionOfClosestObstacle;
    00413   centerOfScan /= 2;
    00414
    00415   // scan amplitude as a function of the free space
    00416   double scanAmplitude = .7, stepSize = .025, scanAmplitudeRatio = 1000;
    00417   double freeSpaceInWalkDirection = obstaclesModel.getDistanceInDirection(centerOfScan, 1.2);
    00418
    00419   // debug output
    00420     Pose2D lineEndPoint(0, freeSpaceInWalkDirection*cos(centerOfScan), freeSpaceInWalkDirection*sin(centerOfScan));
    00421     lineEndPoint = robotPose + lineEndPoint;
    00422
    00423     LINE(headControlField,
    00424       robotPose.translation.x, robotPose.translation.y,
    00425       lineEndPoint.translation.x, lineEndPoint.translation.y,
    00426       2, 0, 5);
    00427
    00428
    00429   MODIFY("headControl:directedScanForObstacles:scanAmplitude", scanAmplitude);
    00430   MODIFY("headControl:directedScanForObstacles:stepAmplitudeRatio", scanAmplitudeRatio);
    00431   MODIFY("headControl:directedScanForObstacles:stepSize", stepSize);
    00432
    00433   double ratio = freeSpaceInWalkDirection/scanAmplitudeRatio;
    00434   if (ratio > 1) ratio = 1;
    00435   if ((walkParams.translation.x < 10) && (walkParams.translation.y < 10) && (walkParams.rotation < 10))
    00436   {
    00437     ratio = 1;
    00438     centerOfScan = 0;
    00439     scanAmplitude = 1.5;
    00440   }
    00441   scanAmplitude *= ratio;
    00442
    00443   Range<double> panRange(-1.5, 1.5);
    00444
    00445   headPan = lastPan + leftOrRight * stepSize;
    00446   lastPan = headPan;
    00447
    00448   if (headPan > panRange.limit(centerOfScan + scanAmplitude))
    00449     leftOrRight = -1;
    00450   else if (headPan < panRange.limit(centerOfScan - scanAmplitude))
    00451     leftOrRight = +1;
    00452
    00453   double length = obstaclesModel.getDistanceInDirection(headPan, .1);
    00454   Pose2D pointOfClosestObstacle(0, length*cos(headPan), length*sin(headPan));
    00455   pointOfClosestObstacle = robotPose + pointOfClosestObstacle;
    00456
    00457     LINE(headControlField,
    00458       robotPose.translation.x, robotPose.translation.y,
    00459       pointOfClosestObstacle.translation.x, pointOfClosestObstacle.translation.y,
    00460       2, 0, 1);
    00461     DEBUG_DRAWING_FINISHED(headControlField);
    00462
    00463   headControl.simpleLookAtPointOnField(
    00464       Vector3<double>(pointOfClosestObstacle.translation.x, pointOfClosestObstacle.translation.y, 0),
    00465       Vector2<int>(0, 0),
    00466       neckTilt, headPanDummy, headTilt);
    00467   headControl.setJoints(neckTilt, headPan, headTilt, 4);
    00468 }
    00469
    00470
    00471 void GT2005BasicBehaviorLookAtBallAndClosestLandmark::execute()
    00472 {
    00473   double neckTiltBall, headPanBall, headTiltBall;
    00474   headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTiltBall, headPanBall, headTiltBall);
    00475
    00476   double neckTiltLM, headPanLM, headTiltLM;
    00477   int closest = headControl.calculateClosestLandmark(Geometry::angleTo(robotPose, ballModel.seen.positionField));
    00478   if (closest != -1)
    00479   {
    00480     headControl.simpleLookAtPointOnField(
    00481       targetPointOnLandmark[closest].position,
    00482       Vector2<int>(0, 0),
    00483       neckTiltLM, headPanLM, headTiltLM);
    00484   }
    00485   else
    00486   {
    00487     /** @todo: do this properly!! */
    00488   }
    00489
    00490   /** calculate the sizes of the ball and the interesting landmark in opening angles */
    00491   double openingAngleOfBall = tan(2*ballRadius/((ballModel.seen.positionField-robotPose.translation).abs()));
    00492   Vector2<double> landmarkInXYPlane(targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y);
    00493   double openingAngleWidthOfLandmark = tan(2*targetPointOnLandmark[closest].width/((landmarkInXYPlane-robotPose.translation).abs()));
    00494
    00495   /** the difference in pan between looking at the ball and looking at the landmark */
    00496   double panDiff = headPanBall - headPanLM;
    00497   double tiltDiff = headTiltBall - headTiltLM;
    00498
    00499   /**  limit the panDiffTmp to the range defined here:
    00500   (this is necessary for the weight calculation to work!)  */
    00501   double minAngle = .2, maxAngle = 1.2;
    00502   Range<double> angleRange(minAngle, maxAngle);
    00503   double panDiffTmp = angleRange.limit(panDiff);
    00504
    00505   /** caculate a weight that determines how much of the adjustment is
    00506   actually performed. This is useful when the landmark is far away and
    00507   it is most likely better to just center on the ball */
    00508   double adjustmentWeight = 1-(panDiffTmp-minAngle)/(maxAngle-minAngle);
    00509   Range<double> weightRange(0,1);
    00510   adjustmentWeight = weightRange.limit(adjustmentWeight);
    00511
    00512   /** only perform adjustments if the ball ist more than 50 cms away */
    00513   double minBClose = 350, maxBClose = 700;
    00514   Range<double> ballCloseRange(minBClose, maxBClose);
    00515   double ballIsClose = (Geometry::fieldCoord2Relative(robotPose, ballModel.seen.positionField)).abs();
    00516   ballIsClose = ballCloseRange.limit(ballIsClose);
    00517   ballIsClose = (ballIsClose - minBClose)/(maxBClose-minBClose);
    00518
    00519   adjustmentWeight *= ballIsClose;
    00520
    00521   /** adjust pan direction: gaze between ball and landmark */
    00522   if (fabs(panDiff) + openingAngleWidthOfLandmark > cameraInfo.openingAngleWidth/2)
    00523   {
    00524     double maxPanAdjustment = cameraInfo.openingAngleWidth/2 - 2.5*openingAngleOfBall;
    00525     double panAdjustment = -panDiff+sgn(panDiff)*(cameraInfo.openingAngleWidth/2.5-openingAngleWidthOfLandmark);
    00526     if (fabs(headPanLM) > 0.7*headControl.jointLimitHeadPanP)
    00527       panAdjustment = 0;
    00528
    00529     panAdjustment = Range<double>::Range(-maxPanAdjustment,maxPanAdjustment).limit(panAdjustment);
    00530
    00531     headPanBall += panAdjustment*adjustmentWeight;
    00532   }
    00533
    00534   /** for tilt: only adjust gaze direction upwards (!)
    00535   and only if the landmark is reachable; furthermore, onyl raise the gaze
    00536   direction as much as necessary to get landmark into view. */
    00537
    00538   if (headTiltLM > headTiltBall)
    00539     if (headTiltLM - headTiltBall > cameraInfo.openingAngleHeight/2)
    00540     {
    00541       double maxTiltAdjustment = cameraInfo.openingAngleHeight/2 - openingAngleOfBall;
    00542       if(maxTiltAdjustment < 0)
    00543         maxTiltAdjustment = 0;
    00544       double tiltAdjustment = -tiltDiff-cameraInfo.openingAngleHeight/2.5;
    00545       if (fabs(headPanLM) > 0.7*headControl.jointLimitHeadPanP)
    00546         tiltAdjustment = 0;
    00547       Range<double> tiltClipping(0, maxTiltAdjustment);
    00548       tiltAdjustment = tiltClipping.limit(tiltAdjustment);
    00549
    00550       // only do this adjustment if landmark is almost in sight...
    00551       headTiltBall += tiltAdjustment*adjustmentWeight;
    00552     }
    00553
    00554   headControl.setJoints(neckTiltBall, headPanBall, headTiltBall);
    00555
    00556   LINE(headControlField,
    00557     robotPose.translation.x, robotPose.translation.y,
    00558     robotPose.translation.x + 2000*cos(headPanBall+robotPose.rotation), robotPose.translation.y + 2000*sin(headPanBall+robotPose.rotation),
    00559     20, 0, Drawings::red);
    00560
    00561   CIRCLE(headControlField,
    00562     targetPointOnLandmark[closest].position.x, targetPointOnLandmark[closest].position.y, 100,
    00563     2, 0, Drawings::red);
    00564   DEBUG_DRAWING_FINISHED(headControlField);
    00565 }
    00566
    00567 void GT2005BasicBehaviorBeginBallSearchAtBallPositionSeen::execute()
    00568 {
    00569   double neckTilt, headPan, headTilt;
    00570   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00571   {
    00572     // now start search and init headpathplaner
    00573     // not very nice...
    00574     headControl.beginBallSearchAt((Vector2<double>) ballModel.seen.positionField);
    00575   }
    00576   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00577   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00578
    00579 }
    00580
    00581 void GT2005BasicBehaviorBeginBallSearchAtBallPositionPropagated::execute()
    00582 {
    00583   double neckTilt, headPan, headTilt;
    00584   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00585   {
    00586     // now start search and init headpathplaner
    00587     // not very nice...
    00588     headControl.beginBallSearchAt((Vector2<double>) ballModel.propagated.positionField);
    00589   }
    00590   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00591   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00592
    00593 }
    00594
    00595 void GT2005BasicBehaviorBeginBallSearchAtBallPositionCommunicated::execute()
    00596 {
    00597   double neckTilt, headPan, headTilt;
    00598   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00599   {
    00600     // now start search and init headpathplaner
    00601     // not very nice...
    00602     headControl.beginBallSearchAt((Vector2<double>) ballModel.communicated.positionField);
    00603   }
    00604   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00605   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00606
    00607 }
    00608
    00609
    00610 void GT2005BasicBehaviorFindBall::execute()
    00611 {
    00612   double neckTilt, headPan, headTilt;
    00613   bool headPanSide=headControl.headPanIsLeft();
    00614   int jumped=0;
    00615
    00616   if ((!basicBehaviorWasActiveDuringLastExecutionOfEngine) || headPathPlanner.isLastPathFinished())
    00617   {
    00618     Vector3<double> leftRightSweepTop,
    00619                     leftRightSweepBottom,
    00620                     halfLeftRightSweep,
    00621                     halfLeftRightSweepBottom;
    00622
    00623     if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00624     {
    00625       if (headControlMode.headControlMode == HeadControlMode::searchForBallLeft)
    00626         lastScanWasLeft = false;
    00627       else if (headControlMode.headControlMode == HeadControlMode::searchForBallRight)
    00628         lastScanWasLeft = true;
    00629       else
    00630         lastScanWasLeft=!headPanSide;
    00631     }
    00632     if (!lastScanWasLeft)
    00633     {
    00634       leftRightSweepTop = headControl.headLeft;
    00635       leftRightSweepBottom = headControl.headLeftDown;
    00636       halfLeftRightSweep = headControl.headMiddleLeft;
    00637       halfLeftRightSweepBottom = headControl.headMiddleLeftDown;
    00638     }
    00639     else
    00640     {
    00641       leftRightSweepTop = headControl.headRight;
    00642       leftRightSweepBottom = headControl.headRightDown;
    00643       halfLeftRightSweep = headControl.headMiddleRight;
    00644       halfLeftRightSweepBottom = headControl.headMiddleRightDown;
    00645     }
    00646
    00647     Vector3<double> points[]={headControl.headDown,
    00648                               headControl.headDown,
    00649                               halfLeftRightSweepBottom,
    00650                               leftRightSweepBottom,
    00651                               leftRightSweepTop,
    00652                               halfLeftRightSweep,
    00653                               headControl.headUp};
    00654     long durations[] =   {100,40,80,120,80,200,120};
    00655
    00656     // jump over positions, which are far away from aktual headposition
    00657
    00658     Vector3<double> *ppoints = points;
    00659     long *pdurations = durations;
    00660
    00661     while (headControl.headPositionDistanceToActualPosition(points[jumped],headPanSide)<0
    00662            && jumped<3
    00663            && !basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00664     {
    00665       ppoints++;
    00666       pdurations++;
    00667       jumped++;
    00668     }
    00669
    00670     headPathPlanner.init(ppoints,pdurations,(sizeof(points)/sizeof(Vector3<double>))-jumped);
    00671     lastScanWasLeft = !lastScanWasLeft;
    00672   }
    00673   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00674   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00675 }
    00676
    00677 void GT2005BasicBehaviorReturnToBall::execute()
    00678 {
    00679   double neckTilt, headPan, headTilt;
    00680   headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTilt, headPan, headTilt);
    00681   headControl.setJoints(neckTilt, headPan, headTilt);
    00682 }
    00683
    00684 void GT2005BasicBehaviorScanAwayFromBall::execute()
    00685 {
    00686   double neckTilt, headPan, headTilt;
    00687   if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00688   {
    00689     if (lastScanWasLeft)
    00690     {
    00691       Vector3<double> centerTop(   0, headPathPlanner.lastHeadPan-0.07, 0.25),
    00692         rightTop(    0, max(-1.2,headPathPlanner.lastHeadPan-1.5),0),
    00693         rightBottom( 0, max(-1.2,headPathPlanner.lastHeadPan-1.5),-0.25);
    00694       Vector3<double>  points[3]={centerTop,rightTop,rightBottom};
    00695       headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    00696     }
    00697     else
    00698     {
    00699       Vector3<double> centerTop(   0, headPathPlanner.lastHeadPan+0.07,min(headPathPlanner.lastHeadTilt+0.2,0.1)),
    00700         leftTop(     0, min(1.2,headPathPlanner.lastHeadPan+1.5),0),
    00701         leftBottom(  0, min(1.2,headPathPlanner.lastHeadPan+1.5),-0.25);
    00702       Vector3<double> points[3]={centerTop,leftTop,leftBottom};
    00703       headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    00704     }
    00705     lastScanWasLeft = !lastScanWasLeft;
    00706   }
    00707
    00708   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00709   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00710 }
    00711
    00712 void GT2005BasicBehaviorScanBackToBall::execute()
    00713 {
    00714   double neckTilt, headPan, headTilt;
    00715   headControl.getLookAtBallAngles(ballModel.seen.positionField, neckTilt, headPan, headTilt);
    00716   headControl.setJoints(neckTilt, headPan, headTilt, 4.0);
    00717 }
    00718
    00719 void GT2005BasicBehaviorGrabBall::execute()
    00720 {
    00721   double neckTilt, headPan, headTilt;
    00722     // Change PID values for faster head motion
    00723     pidData.setValues(JointData::neckTilt,0x20,0x08,0x0C);
    00724     pidData.setValues(JointData::headPan,0x0A,0x08,0x0C);
    00725     pidData.setValues(JointData::headTilt,0x0A,0x08,0x0C);
    00726   if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00727   {
    00728
    00729     Vector3<double>
    00730       prepare(-0.5, 0.0, 0.37),
    00731       grab(   -1.1, 0.0, 0.7);
    00732
    00733     Vector3<double> points[2]={prepare, grab};
    00734     long durations[2]={50,50} ;
    00735     headPathPlanner.init(points, durations,sizeof(points)/sizeof(Vector3<double>),false);
    00736   }
    00737   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00738   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00739   this->headMotionRequest.mouth = 0;
    00740
    00741 }
    00742
    00743
    00744 void GT2005BasicBehaviorGrabBallHigh::execute()
    00745 {
    00746   double neckTilt, headPan, headTilt;
    00747     // Change PID values for faster head motion
    00748     pidData.setValues(JointData::neckTilt,0x20,0x08,0x0C);
    00749     pidData.setValues(JointData::headPan,0x0A,0x08,0x0C);
    00750     pidData.setValues(JointData::headTilt,0x0A,0x08,0x0C);
    00751   if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00752   {
    00753   this->headMotionRequest.mouth = 0;
    00754     Vector3<double>
    00755       prepare(   -0.5, 0.0, 0.37),
    00756       grab(      -1.1, 0.0, 0.7),
    00757     grabhigh ( -1.0, 0.0, 1.1);
    00758     Vector3<double> points[3]={prepare, grab, grabhigh};
    00759     long durations[3]={130,130,100} ;
    00760     headPathPlanner.init (points, durations,sizeof(points)/sizeof(Vector3<double>),false);
    00761   timeOfLastExecution = SystemCall::getCurrentSystemTime();
    00762   }
    00763   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00764   headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00765
    00766   if(SystemCall::getCurrentSystemTime() - timeOfLastExecution > 360)
    00767   this->headMotionRequest.mouth = -1000000;
    00768   else  this->headMotionRequest.mouth = 0;
    00769
    00770 }
    00771
    00772
    00773 void GT2005BasicBehaviorReleaseBall::execute()
    00774 {
    00775   double neckTilt, headPan, headTilt;
    00776   if(!basicBehaviorWasActiveDuringLastExecutionOfEngine)
    00777   {
    00778     // Change PID values for faster head motion
    00779     pidData.setValues(JointData::neckTilt,0x20,0x08,0x0C);
    00780     pidData.setValues(JointData::headPan,0x0A,0x08,0x0C);
    00781     pidData.setValues(JointData::headTilt,0x0A,0x08,0x0C);
    00782
    00783     Vector3<double>
    00784       release(0.0, 0.0, 0.0);
    00785     Vector3<double> points[1]={release};
    00786     headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    00787   }
    00788   headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00789   if(!headPathPlanner.isLastPathFinished())
    00790     headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00791 }
    00792
    00793 void GT2005BasicBehaviorWaitForGrab::execute()
    00794 {
    00795   headControl.setJointsDirect(-0.5, 0.0, 0.37);
    00796 }
    00797
    00798 void GT2005BasicBehaviorHoldBall::execute()
    00799 {
    00800   if (robotPose.getValidity()>0.5)
    00801   {
    00802     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00803   }
    00804           pidData.setToDefaults();
    00805 //        pidData.setValues(JointData::mouth,0x02,0x01,0x02);
    00806         pidData.setValues(JointData::neckTilt,0x00,0x00,0x00);
    00807         this->headMotionRequest.tilt  = -1400000;
    00808         this->headMotionRequest.pan   =        0;
    00809         this->headMotionRequest.roll  =   880000;
    00810         this->headMotionRequest.mouth = -1500000;
    00811   lastOdometryData = currentOdometryData;
    00812 }
    00813
    00814 void GT2005BasicBehaviorOpenChallenge::execute()
    00815 {
    00816   if (robotPose.getValidity()>0.5)
    00817   {
    00818     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00819   }
    00820   switch (headControlMode.headControlMode)
    00821   {
    00822 case HeadControlMode::openChallengeReset:
    00823         pidData.setToDefaults();
    00824 //        pidData.setValues(JointData::mouth,0x02,0x01,0x02);
    00825         pidData.setValues(JointData::mouth,0x01,0x08,0x01);
    00826         this->headMotionRequest.mouth = -1500000;
    00827       break;
    00828     case HeadControlMode::openChallengeJoysickMode:
    00829         pidData.setToDefaults();
    00830         pidData.setValues(JointData::mouth,0x01,0x00,0x00);
    00831         this->headMotionRequest.mouth = -1500000;
    00832       break;
    00833     case HeadControlMode::openChallengePullBridge:
    00834         pidData.setToDefaults();
    00835         pidData.setValues(JointData::mouth,0x01,0x01,0x08);
    00836         this->headMotionRequest.mouth = -1500000;
    00837       break;
    00838     case HeadControlMode::openChallengeTest:
    00839         //sensorDataBuffer.frame[0]
    00840         pidData.setToDefaults();
    00841         //pidData.setValues(JointData::neckTilt,0,0,0);
    00842         pidData.setValues(JointData::mouth,0,0,0);
    00843         pidData.setValues(JointData::mouth,0x02,0x01,0x02);
    00844         this->headMotionRequest.mouth = -1500000;
    00845       break;
    00846     case HeadControlMode::openChallengeGoToBridge:
    00847         pidData.setToDefaults();
    00848         pidData.setValues(JointData::mouth,0x01,0x00,0x00);
    00849         headControl.setJointsDirect(-1, 0, 0.6);
    00850         this->headMotionRequest.mouth = -1500000;
    00851       break;
    00852   default:
    00853     errorHandler.error("GT2005BasicBehaviorOtherHeadMovements::execute(): head control mode \"%s\" is not implemented.",
    00854       HeadControlMode::getHeadControlModeName(headControlMode.headControlMode));
    00855     break;
    00856   }
    00857   lastOdometryData = currentOdometryData;
    00858 }
    00859
    00860 void GT2005BasicBehaviorNone::execute()
    00861 {
    00862   if (robotPose.getValidity()>0.5)
    00863   {
    00864     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00865   }
    00866       // do nothing
    00867   lastOdometryData = currentOdometryData;
    00868 }
    00869
    00870 void GT2005BasicBehaviorLookLeft::execute()
    00871 {
    00872   double neckTilt, headPan, headTilt;
    00873   if (robotPose.getValidity()>0.5)
    00874   {
    00875     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00876   }
    00877         Vector3<double> left(-0.55, 1.6, -0.3);
    00878       lastScanWasLeft = true;
    00879       if(headControl.lastHeadControlMode != headControlMode.headControlMode)
    00880       {
    00881         Vector3<double> points[]={headControl.headDown,left};
    00882         headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    00883       }
    00884       headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00885       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00886   lastOdometryData = currentOdometryData;
    00887 }
    00888
    00889 void GT2005BasicBehaviorLookRight::execute()
    00890 {
    00891   double neckTilt, headPan, headTilt;
    00892   if (robotPose.getValidity()>0.5)
    00893   {
    00894     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00895   }
    00896         Vector3<double> right(-0.55, -1.6, -0.3);
    00897
    00898       lastScanWasLeft = false;
    00899       if(headControl.lastHeadControlMode != headControlMode.headControlMode)
    00900       {
    00901         Vector3<double> points[]={headControl.headDown,right};
    00902         headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    00903       }
    00904       headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00905       headControl.setJointsDirect(neckTilt, headPan, headTilt, 0);
    00906   lastOdometryData = currentOdometryData;
    00907 }
    00908
    00909 void GT2005BasicBehaviorScanForObstacles::execute()
    00910 {
    00911   double neckTilt, headPan, headTilt;
    00912   if (robotPose.getValidity()>0.5)
    00913   {
    00914     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00915   }
    00916         Vector3<double> left(0.0,.50,.25), middle(0.0,0,.25), right(0.0,-.50,.25);
    00917       Vector3<double> leftLow(-.5,.50,.25), middleLow(-.5,0,.25), rightLow(-.5,-.50,.25);
    00918
    00919       if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
    00920       {
    00921         if(lastScanWasLeft)
    00922         {
    00923           Vector3<double> points[3]={left,middle, right};
    00924           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 600);
    00925           lastScanWasLeft = !lastScanWasLeft;
    00926         }
    00927         else
    00928         {
    00929           Vector3<double> points[3]={rightLow,middleLow, leftLow};
    00930           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 600);
    00931           lastScanWasLeft = !lastScanWasLeft;
    00932         }
    00933       }
    00934       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00935   lastOdometryData = currentOdometryData;
    00936 }
    00937
    00938 void GT2005BasicBehaviorRealSlowScan::execute()
    00939 //  case HeadControlMode::realSlowScan:
    00940     {
    00941   double neckTilt, headPan, headTilt;
    00942   if (robotPose.getValidity()>0.5)
    00943   {
    00944     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00945   }
    00946
    00947   Vector3<double> left(0.0,pi_2,0.0), middle(0.0,0.0,0.0), right(0.0,-pi_2,0.0);
    00948
    00949       if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
    00950       {
    00951         if(lastScanWasLeft)
    00952         {
    00953           Vector3<double> points[3]={left,middle, right};
    00954           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 8000);
    00955           lastScanWasLeft = !lastScanWasLeft;
    00956         }
    00957         else
    00958         {
    00959           Vector3<double> points[3]={right, middle, left};
    00960           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 8000);
    00961           lastScanWasLeft = !lastScanWasLeft;
    00962         }
    00963       }
    00964       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00965     }
    00966
    00967 void GT2005BasicBehaviorLookBetweenFeetForCarriedBall::execute()
    00968 //  case HeadControlMode::lookBetweenFeetForCarriedBall:
    00969     {
    00970   double neckTilt, headPan, headTilt;
    00971   if (robotPose.getValidity()>0.5)
    00972   {
    00973     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00974   }
    00975
    00976   Vector3<double>
    00977         //t0(-1.1, 0.0, 0.7), // carry ball starting position
    00978         t1(-0.55, 0.0, 0.8), // move head up to avoid accidental pushing of the ball
    00979         t2(-0.25, 0.0, 0.3), // move head back
    00980         t3(-0.1, 0.0, -0.4);
    00981
    00982       if(headControl.lastHeadControlMode != headControlMode.headControlMode)
    00983       {
    00984         Vector3<double> points[]={t1, t2, t3, t3};
    00985         long duration[] = {80, 120, 10, 2080};
    00986         //headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 500);
    00987         headPathPlanner.init(points, duration, sizeof(points)/sizeof(Vector3<double>), false);
    00988       }
    00989       headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    00990       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    00991     }
    00992
    00993 void GT2005BasicBehaviorSearchForLandmarks::execute()
    00994 {
    00995   double neckTilt, headPan, headTilt;
    00996   if (robotPose.getValidity()>0.5)
    00997   {
    00998     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    00999   }
    01000       //lastScanWasLeft = (headPathPlanner.lastHeadPan>0);
    01001       //Vector3<double> left((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0),   1.5, 0.0);
    01002       //Vector3<double> right((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0), -1.5, 0.0);
    01003       //Vector3<double> center((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 2.0), 0.0, 0.0);
    01004
    01005       if(headControl.lastHeadControlMode != headControlMode.headControlMode)
    01006       {
    01007         if(lastScanWasLeft)
    01008         {
    01009           Vector3<double> points[2]={headControl.headLeft,headControl.headRight};
    01010           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01011           lastScanWasLeft = !lastScanWasLeft;
    01012         }
    01013         else
    01014         {
    01015           Vector3<double> points[3]={headControl.headRight, headControl.headUp, headControl.headLeft};
    01016           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01017           lastScanWasLeft = !lastScanWasLeft;
    01018         }
    01019       }
    01020
    01021       if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
    01022       {
    01023         if(lastScanWasLeft)
    01024         {
    01025           Vector3<double> points[2]={headControl.headUp,headControl.headRight};
    01026           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01027           lastScanWasLeft = !lastScanWasLeft;
    01028         }
    01029         else
    01030         {
    01031           Vector3<double> points[2]={headControl.headUp, headControl.headLeft};
    01032           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01033           lastScanWasLeft = !lastScanWasLeft;
    01034         }
    01035       }
    01036       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    01037   lastOdometryData = currentOdometryData;
    01038 }
    01039
    01040 void GT2005BasicBehaviorSearchForLandmarksHeadLow::execute()
    01041 {
    01042   double neckTilt, headPan, headTilt;
    01043   if (robotPose.getValidity()>0.5)
    01044   {
    01045     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01046   }
    01047         Vector3<double> left((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0 - .8),   1.0, 0.5);
    01048       Vector3<double> right((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 6.0 - .8), -1.0, 0.5);
    01049       Vector3<double> center((bodyPosture.bodyTiltCalculatedFromLegSensors - cameraInfo.openingAngleHeight / 2.0 - .2), 0.0, 0.5);
    01050
    01051       if(headControl.lastHeadControlMode != headControlMode.headControlMode)
    01052       {
    01053         if(lastScanWasLeft)
    01054         {
    01055           Vector3<double> points[2]={left, right};
    01056           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
    01057           lastScanWasLeft = !lastScanWasLeft;
    01058         }
    01059         else
    01060         {
    01061           Vector3<double> points[3]={right, center, left};
    01062           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
    01063           lastScanWasLeft = !lastScanWasLeft;
    01064         }
    01065       }
    01066
    01067       if(headPathPlanner.getAngles(neckTilt, headPan, headTilt))
    01068       {
    01069         if(lastScanWasLeft)
    01070         {
    01071           Vector3<double> points[2]={center, right};
    01072           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
    01073           lastScanWasLeft = !lastScanWasLeft;
    01074         }
    01075         else
    01076         {
    01077           Vector3<double> points[2]={center, left};
    01078           headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 150);
    01079           lastScanWasLeft = !lastScanWasLeft;
    01080         }
    01081       }
    01082       headControl.setJointsDirect(neckTilt, headPan, headTilt);
    01083   lastOdometryData = currentOdometryData;
    01084 }
    01085
    01086 void GT2005BasicBehaviorLookAtBluePinkLandmark::execute()
    01087 {
    01088   double neckTilt, headPan, headTilt;
    01089   if (robotPose.getValidity()>0.5)
    01090   {
    01091     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01092   }
    01093     double angleToLandmark = 0;
    01094     double distanceToLandmark = 1000;
    01095     for(int i = 0; i < landmarksPercept.numberOfFlags; i++)
    01096     {
    01097       if(landmarksPercept.flags[i].type == Flag::skyblueAbovePink)
    01098       {
    01099         angleToLandmark = landmarksPercept.flags[i].angle;
    01100         distanceToLandmark = landmarksPercept.flags[i].distance;
    01101       }
    01102     }
    01103
    01104     double flagx = cos(angleToLandmark) * distanceToLandmark;
    01105     double flagy = sin(angleToLandmark) * distanceToLandmark;
    01106
    01107     headControl.simpleLookAtPointRelativeToRobot(
    01108     Vector3<double>(flagx, flagy, 300),
    01109     Vector2<int>(0, 0),
    01110     neckTilt, headPan, headTilt);
    01111
    01112     headControl.setJointsDirect(neckTilt, headPan, headTilt);
    01113   lastOdometryData = currentOdometryData;
    01114 }
    01115
    01116 void GT2005BasicBehaviorLookStraightAhead::execute()
    01117 {
    01118   if (robotPose.getValidity()>0.5)
    01119   {
    01120     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01121   }
    01122       pidData.setToDefaults();
    01123     headControl.setJointsDirect(0.0, 0.0, 0.0, 0.05);
    01124   lastOdometryData = currentOdometryData;
    01125 }
    01126
    01127 void GT2005BasicBehaviorLookTowardOpponentGoal::execute()
    01128 {
    01129   if (robotPose.getValidity()>0.5)
    01130   {
    01131     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01132   }
    01133       headControl.setJointsDirect(0.0, -robotPose.rotation, 0.0, 0.0);
    01134   lastOdometryData = currentOdometryData;
    01135 }
    01136
    01137 void GT2005BasicBehaviorLookBetweenFeet::execute()
    01138 {
    01139   if (robotPose.getValidity()>0.5)
    01140   {
    01141     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01142   }
    01143       headControl.setJointsDirect(-1.0, 0.0, 0.0);
    01144   lastOdometryData = currentOdometryData;
    01145 }
    01146
    01147 void GT2005BasicBehaviorLookToStars::execute()
    01148 {
    01149   if (robotPose.getValidity()>0.5)
    01150   {
    01151     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01152   }
    01153       headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
    01154   lastOdometryData = currentOdometryData;
    01155 }
    01156
    01157 void GT2005BasicBehaviorSnapAtFinger::execute()
    01158 {
    01159   if (robotPose.getValidity()>0.5)
    01160   {
    01161     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01162   }
    01163       if(sensorDataBuffer.frame[0].mouth == 0)
    01164     {
    01165       headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, 0);
    01166     }
    01167     else
    01168     {
    01169       headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
    01170     }
    01171   lastOdometryData = currentOdometryData;
    01172 }
    01173
    01174 void GT2005BasicBehaviorLookParallelToGround::execute()
    01175 {
    01176   if (robotPose.getValidity()>0.5)
    01177   {
    01178     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01179   }
    01180       if (bodyPosture.bodyTiltCalculatedFromAccelerationSensors>0)
    01181       headControl.setJoints(0.0, 0.0, bodyPosture.bodyTiltCalculatedFromAccelerationSensors);
    01182     else
    01183       headControl.setJoints(bodyPosture.bodyTiltCalculatedFromAccelerationSensors, 0.0, 0.0);
    01184   lastOdometryData = currentOdometryData;
    01185 }
    01186
    01187 void GT2005BasicBehaviorDirect::execute()
    01188 {
    01189   double neckTilt, headPan, headTilt;
    01190   if (robotPose.getValidity()>0.5)
    01191   {
    01192     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01193   }
    01194       if (headPathPlanner.isLastPathFinished())
    01195     {
    01196       Vector3<double> point((double )headControlMode.directTilt/1000000.0,(double )headControlMode.directPan/1000000.0,(double )headControlMode.directRoll/1000000.0);
    01197       Vector3<double> points[1]={point};
    01198       headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 2000);
    01199     }
    01200     headPathPlanner.getAngles(neckTilt, headPan, headTilt);
    01201     headControl.setJointsDirect(neckTilt, headPan, headTilt);
    01202   lastOdometryData = currentOdometryData;
    01203 }
    01204
    01205 void GT2005BasicBehaviorStayAsForced::execute()
    01206 {
    01207   if (robotPose.getValidity()>0.5)
    01208   {
    01209     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01210   }
    01211       const double neckTiltTolerance = 0.06;
    01212       const double headPanTolerance   = 0.06;
    01213       const double headTiltTolerance = 0.05;
    01214
    01215       double neckTiltDiff = sensorDataBuffer.lastFrame().data[SensorData::neckTilt] - headMotionRequest.tilt;
    01216       double headPanDiff  = sensorDataBuffer.lastFrame().data[SensorData::headPan]  - headMotionRequest.pan;
    01217       double headTiltDiff = sensorDataBuffer.lastFrame().data[SensorData::headTilt] - headMotionRequest.roll;
    01218
    01219       if (neckTiltDiff > neckTiltTolerance)
    01220       {
    01221         headMotionRequest.tilt += (long)(neckTiltDiff-neckTiltTolerance);
    01222       }
    01223       else if (neckTiltDiff < -neckTiltTolerance)
    01224       {
    01225         headMotionRequest.tilt += (long)(neckTiltDiff+neckTiltTolerance);
    01226       }
    01227
    01228       if (headPanDiff > headPanTolerance)
    01229       {
    01230         headMotionRequest.pan += (long)(headPanDiff-headPanTolerance);
    01231       }
    01232       else if (headPanDiff < -headPanTolerance)
    01233       {
    01234         headMotionRequest.pan += (long)(headPanDiff+headPanTolerance);
    01235       }
    01236
    01237       if (headTiltDiff > headTiltTolerance)
    01238       {
    01239         headMotionRequest.roll += (long)(headTiltDiff-headTiltTolerance);
    01240       }
    01241       else if (headTiltDiff < -headTiltTolerance)
    01242       {
    01243         headMotionRequest.roll += (long)(headTiltDiff+headTiltTolerance);
    01244       }
    01245   lastOdometryData = currentOdometryData;
    01246 }
    01247
    01248 void GT2005BasicBehaviorWatchOrigin::execute()
    01249 {
    01250   double neckTilt, headPan, headTilt;
    01251   if (robotPose.getValidity()>0.5)
    01252   {
    01253     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01254   }
    01255   Vector3<double> point(0,0,180);
    01256   //its OK to start immediately here, because we scan smooth and in a way that we can always see it again:
    01257   if (robotPose.getValidity()<0.5)
    01258   {
    01259     //if we dont know where to look, we look there smooth:
    01260     if(headPathPlanner.isLastPathFinished())
    01261     {
    01262       unsigned long blindTime=SystemCall::getTimeSince(lastTimeOfGoodSL);
    01263       double odoRot=currentOdometryData.rotation-lastOdometryData.rotation;
    01264       if ((fabs(robotPose.rotation)<1.3)&&
    01265           ((blindTime<500)||
    01266           ((fabs(odoRot)<0.004)&&(blindTime<1300))
    01267           )
    01268          )
    01269       {
    01270         //scan for origin
    01271         if(headPathPlanner.isLastPathFinished())
    01272         {
    01273           if(lastScanWasLeft)
    01274           {
    01275             headControl.simpleLookAtPointOnField(point,Vector2<int>(-65,0),neckTilt,headPan,headTilt);
    01276             Vector3<double> right(neckTilt,headPan,headTilt);
    01277             Vector3<double> points[1]={right};
    01278             headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01279           }
    01280           else
    01281           {
    01282             headControl.simpleLookAtPointOnField(point,Vector2<int>(65,0),neckTilt,headPan,headTilt);
    01283             Vector3<double> left(neckTilt,headPan,headTilt);
    01284             Vector3<double> points[1]={left};
    01285             headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 100);
    01286           }
    01287           lastScanWasLeft=!lastScanWasLeft;
    01288         }
    01289       }
    01290       else if (odoRot==0)
    01291       {
    01292         //2do: somethimg usefull
    01293       }
    01294       else if (odoRot>0)
    01295       {
    01296         //if we can/will see the origin, then left
    01297
    01298         lastScanWasLeft=true;
    01299         static const Vector3<double> point7(0.05,1.5 ,0.05);
    01300         Vector3<double> points[1]={point7};
    01301         headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 450);
    01302       }
    01303       else
    01304       {
    01305         //if we can/will see the origin, then right
    01306         lastScanWasLeft=false;
    01307         static const Vector3<double> point7(0.05,-1.5 ,0.05);
    01308         Vector3<double> points[1]={point7};
    01309         headPathPlanner.oldInit(points, sizeof(points)/sizeof(Vector3<double>), 450);
    01310       }
    01311     }
    01312     headPathPlanner.getAngles(neckTilt,headPan,headTilt);
    01313     headControl.setJointsDirect(neckTilt,headPan,headTilt);
    01314   }
    01315   else
    01316   {
    01317     //if we know where to look, we look there immediately:
    01318     if (robotPose.rotation<-1.45)
    01319     {
    01320       headControl.setJointsDirect(0.05, 1.5, 0.05);
    01321     }
    01322     else if (robotPose.rotation>1.45)
    01323     {
    01324       headControl.setJointsDirect(0.05, -1.5, 0.05);
    01325     }
    01326     else
    01327     {
    01328       double rota=-robotPose.speedbyDistanceToGoal*30;
    01329       rota /= 2;
    01330       headControl.simpleLookAtPointOnField(point,Vector2<int>((int)rota,0),neckTilt,headPan,headTilt);
    01331       headControl.setJointsDirect(neckTilt,headPan,headTilt);
    01332     }
    01333   }
    01334   lastOdometryData = currentOdometryData;
    01335 }
    01336 void GT2005BasicBehaviorCalibrateHeadSpeed::execute()
    01337 {
    01338   double neckTilt, headPan, headTilt;
    01339   if (robotPose.getValidity()>0.5)
    01340   {
    01341     lastTimeOfGoodSL=SystemCall::getCurrentSystemTime();
    01342   }
    01343   if (headControl.calibrateHeadSpeedIsReady())
    01344   {
    01345     // look to the stars
    01346     headControl.setJoints(headControl.jointLimitNeckTiltP, 0.0, headControl.jointLimitHeadTiltP, 0, -.7);
    01347   }
    01348   else
    01349   {
    01350     headControl.calibrateHeadSpeed();
    01351     headPathPlanner.getAngles(neckTilt,headPan,headTilt);
    01352     headControl.setJointsDirect(neckTilt,headPan,headTilt);
    01353   }
    01354   lastOdometryData = currentOdometryData;
    01355 }