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

# Modules/HeadControl/GT2005HeadControl/GT2005HeadControl.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005HeadControl.cpp
    00003 *
    00004 * Implementation of class GT2005HeadControl
    00005 *
    00006 * @author Marc Dassler
    00007 * @author Jan Hoffmann
    00008 * @author Uwe Düffert
    00009 * @author Martin Lötzsch
    00010 */
    00011
    00012
    00013 #include "Platform/GTAssert.h"
    00014 #include "GT2005HeadControl.h"
    00015 #include "Tools/Math/Geometry.h"
    00016 #include "Tools/Actorics/Kinematics.h"
    00017 #include "Tools/Math/Common.h"
    00018 #include "Tools/FieldDimensions.h"
    00019 #include "Platform/SystemCall.h"
    00020
    00021
    00022 #ifdef _WIN32
    00023 #pragma warning(disable:4355) // VC++: "this" in initializer list is ok
    00024 #endif
    00025
    00026 GT2005HeadControl::GT2005HeadControl(HeadControlInterfaces& interfaces)
    00027 : Xabsl2HeadControl(interfaces, SolutionRequest::hc_gt2005),
    00028 headPathPlanner(sensorDataBuffer),
    00029 symbols(interfaces,*this,basicBehaviors.basicBehaviorDirectedScanForLandmarks),
    00030 basicBehaviors(errorHandler,*this,*this,headPathPlanner,lastScanWasLeft,cameraInfo),
    00031 lastScanWasLeft(true),
    00032 lastHeadControlMode(HeadControlMode::none),
    00033 lastOdometryData(), lastRobotPose()
    00034 {
    00035   Xabsl2FileInputSource file("Xabsl2/HeadCtrl/gt05-ic.dat");
    00036   init(file);
    00037
    00038   headPathPlanner.lastNeckTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
    00039   headPathPlanner.lastHeadPan  = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
    00040   headPathPlanner.lastHeadTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;
    00041
    00042   const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();
    00043   cameraInfo = CameraInfo::CameraInfo(getRobotConfiguration().getRobotDesign());
    00044
    00045   setupMainAngles();
    00046
    00047   jointRangeNeckTilt = Range<double>(robotDimensions.jointLimitNeckTiltN,robotDimensions.jointLimitNeckTiltP);
    00048   jointRangeHeadPan  = Range<double>(robotDimensions.jointLimitHeadPanN, robotDimensions.jointLimitHeadPanP);
    00049   jointRangeHeadTilt = Range<double>(robotDimensions.jointLimitHeadTiltN,robotDimensions.jointLimitHeadTiltP);
    00050
    00051   // these values are the default settings for standard pid values
    00052   speedNeckTilt = 0.002000;
    00053   speedHeadPan  = 0.005350;
    00054   speedHeadTilt = 0.002550;
    00055
    00056   // setting speed in headpath planner
    00057   headPathPlanner.headPathSpeedNeckTilt = speedNeckTilt;
    00058   headPathPlanner.headPathSpeedHeadPan  = speedHeadPan;
    00059   headPathPlanner.headPathSpeedHeadTilt = speedHeadTilt;
    00060
    00061   calibrationReset();
    00062
    00063   useCommunicatedBall = true;
    00064
    00065
    00066 }
    00067
    00068 int GT2005HeadControl::getLastSeenBeaconIndex()
    00069 {
    00070   return landmarksState.lastSeenBeaconIndex();
    00071 }
    00072 long GT2005HeadControl::getTimeOfLastSeenBeacon(int index)
    00073 {
    00074   return landmarksState.timeOfLastSeenBeacon(index);
    00075 }
    00076
    00077 long GT2005HeadControl::getTimeBetweenSeen2LastBeacons(int index)
    00078 {
    00079   return landmarksState.timeBetweenSeen2LastBeacons(index);
    00080 }
    00081
    00082 bool GT2005HeadControl::headPanIsLeft()
    00083 {
    00084   return ((((double )sensorDataBuffer.lastFrame().data[SensorData::headPan]))>0);
    00085 }
    00086
    00087 void GT2005HeadControl::getSensorHeadAngles(Vector3<double>& pos)
    00088 {
    00089   pos.x = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
    00090   pos.y = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
    00091   pos.z = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;
    00092 }
    00093
    00094 void GT2005HeadControl::searchForBallRight()
    00095 {
    00096   Vector3<double> points[]={  headDown,
    00097                               headMiddleRight,
    00098                               headRight,
    00099                               headRight};
    00100
    00101   long durations[] =   {100,100,200,160};
    00102   headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
    00103
    00104   lastScanWasLeft = false;
    00105 }
    00106
    00107 void GT2005HeadControl::searchForBallLeft()
    00108 {
    00109   Vector3<double> points[]={  headDown,
    00110                               headMiddleLeft,
    00111                               headLeft,
    00112                               headLeft};
    00113
    00114   long durations[] =   {100,100,200,160};
    00115   headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
    00116
    00117   lastScanWasLeft = true;
    00118 }
    00119
    00120 double GT2005HeadControl::headPositionDistanceToActualPosition(Vector3<double> comp,bool leftSide)
    00121 {
    00122   double pos;
    00123   pos = ((double)sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
    00124
    00125   return -fabs(pos-comp.y);;
    00126
    00127 }
    00128 void GT2005HeadControl::registerSymbolsAndBasicBehaviors()
    00129 {
    00130   symbols.registerSymbols(*pEngine);
    00131   basicBehaviors.registerBasicBehaviors(*pEngine);
    00132 }
    00133
    00134 void GT2005HeadControl::execute()
    00135 {
    00136   pidData.setToDefaults();
    00137   symbols.update();
    00138
    00139   if(headIsBlockedBySpecialActionOrWalk)
    00140   {
    00141     headPathPlanner.lastNeckTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
    00142     headPathPlanner.lastHeadPan  = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
    00143     headPathPlanner.lastHeadTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;
    00144   }
    00145
    00146   if(sensorDataBuffer.lastFrame().data[SensorData::chin] == 1)
    00147   {
    00148     if(
    00149       robotState.getButtonPressed(BodyPercept::backBack) &&
    00150       robotState.getButtonDuration(BodyPercept::backBack) > 1000 &&
    00151       profiler.profilerCollectMode == GTXabsl2Profiler::collectProfiles
    00152       )
    00153     {
    00154       profiler.profilerCollectMode = GTXabsl2Profiler::dontCollectProfiles;
    00155       profiler.profilerWriteMode = GTXabsl2Profiler::writeCompleteProfiles;
    00156     }
    00157
    00158     if(
    00159       robotState.getButtonPressed(BodyPercept::backFront) &&
    00160       robotState.getButtonDuration(BodyPercept::backFront) > 1000
    00161       )
    00162     {
    00163       profiler.profilerCollectMode = GTXabsl2Profiler::collectProfiles;
    00164     }
    00165   }
    00166
    00167   executeEngine();
    00168   DEBUG_RESPONSE("LED, tail, mouth: show position standard deviation with mouth", showVisualizationQuality(); );
    00169   lastHeadControlMode = headControlMode.headControlMode;
    00170 }
    00171
    00172
    00173
    00174 void GT2005HeadControl::showVisualizationQuality()
    00175 {
    00176
    00177   if ((headMotionRequest.mouth >= 0) && (headControlMode.headControlMode != HeadControlMode::catchBall))
    00178   {
    00179     double maxStandardDeviation = 500, mouthRange = -1000000;
    00180
    00181     MODIFY("headControl:maxVarianceForMouthOutput", maxStandardDeviation);
    00182     MODIFY("headControl:maxMouthRange", mouthRange);
    00183
    00184     double relativeStandardDeviation = sqrt(robotPose.getPositionVariance())/maxStandardDeviation;
    00185     if (relativeStandardDeviation > 1)
    00186       relativeStandardDeviation = 1;
    00187     headMotionRequest.mouth = (long )( mouthRange*relativeStandardDeviation);
    00188   }
    00189 }
    00190
    00191
    00192
    00193 bool GT2005HeadControl::handleMessage(InMessage& message)
    00194 {
    00195   return Xabsl2HeadControl::handleMessage(message);
    00196 }
    00197
    00198
    00199 void GT2005HeadControl::beginBallSearchAt(Vector2<double> ballPosition2d)
    00200 {
    00201   Vector3<double> ballPosition3d (ballPosition2d.x,ballPosition2d.y,ballRadius);
    00202
    00203   Vector3<double> leftRightSweepTop,
    00204                   leftRightSweepBottom,
    00205                   halfLeftRightSweep,
    00206                   halfLeftRightSweepBottom;
    00207
    00208   Vector2<double> toBall = ballPosition2d - robotPose.translation;
    00209   double angleToBall = normalize(atan2(toBall.y,toBall.x))-robotPose.rotation;
    00210
    00211   // center the ball view in the middle of the image
    00212   // if the ball is near, the ball should be seen, if we look halfway down
    00213   // constante definition of distance to ball
    00214   enum { ballNearBy = 500 };
    00215   Vector2<int> cameraImageOffset(0,25);
    00216
    00217   double neckTilt,headPan,headTilt;
    00218   simpleLookAtPointOnField(ballPosition3d,cameraImageOffset,neckTilt,headPan,headTilt);
    00219   Vector3<double> ballAngles (neckTilt,headPan,headTilt);
    00220
    00221   if (angleToBall>0)
    00222   {
    00223     leftRightSweepTop        = headLeft;
    00224     leftRightSweepBottom     = headLeftDown;
    00225     halfLeftRightSweep       = headMiddleLeft;
    00226     halfLeftRightSweepBottom = headMiddleLeftDown;
    00227
    00228   }
    00229   else
    00230   {
    00231     leftRightSweepTop         = headRight;
    00232     leftRightSweepBottom      = headRightDown;
    00233     halfLeftRightSweep        = headMiddleRight;
    00234     halfLeftRightSweepBottom  = headMiddleRightDown;
    00235   }
    00236
    00237   Vector3<double> points[]={ballAngles,
    00238                             ballAngles,
    00239                             leftRightSweepBottom,
    00240                             leftRightSweepTop,
    00241                             halfLeftRightSweep,
    00242                             headUp,
    00243                             headDown};
    00244
    00245   long durations[] = {0,100,160,120,160,100,80};
    00246   headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
    00247   lastScanWasLeft = (angleToBall>0);
    00248
    00249
    00250 }
    00251 void GT2005HeadControl::setJointsDirect(double neckTilt, double headPan, double headTilt, double mouth)
    00252 {
    00253   headPathPlanner.lastNeckTilt = jointRangeNeckTilt.limit(neckTilt);
    00254   headPathPlanner.lastHeadPan  = jointRangeHeadPan.limit(headPan);
    00255   headPathPlanner.lastHeadTilt = jointRangeHeadTilt.limit(headTilt);
    00256
    00257   headMotionRequest.tilt  = toMicroRad(neckTilt);
    00258   headMotionRequest.pan   = toMicroRad(headPan);
    00259   headMotionRequest.roll  = toMicroRad(headTilt);
    00260   headMotionRequest.mouth = toMicroRad(mouth);
    00261 }
    00262
    00263 void GT2005HeadControl::setJoints(double neckTilt, double headPan, double headTilt, double speed, double mouth)
    00264 {
    00265   const double closeToDesiredAngles = 0.01;
    00266
    00267   //double panCausedByRobotRotation = currentOdometryData.rotation - lastOdometryData.rotation;
    00268
    00269   // test if head has reached his position
    00270   setJointsIsCloseToDestination = headPathPlanner.headPositionReached(Vector3<double> (neckTilt,headPan,headTilt));
    00271
    00272   setJointsMaxPanReached = false;
    00273
    00274   Vector2<double>
    00275     direction(headTilt - headPathPlanner.lastHeadTilt,
    00276     headPan - (headPathPlanner.lastHeadPan/* + panCausedByRobotRotation*/));
    00277
    00278   if (speed > 0)
    00279   {
    00280     Vector2<double> directionWithSpeed = direction;
    00281     directionWithSpeed.normalize();
    00282     directionWithSpeed *= speed/125;
    00283     if (directionWithSpeed.abs() < direction.abs())
    00284       direction = directionWithSpeed;
    00285   }
    00286
    00287   if (direction.abs() < closeToDesiredAngles)
    00288     setJointsIsCloseToDestination++;
    00289   else
    00290     setJointsIsCloseToDestination = 0;
    00291
    00292   headPathPlanner.lastNeckTilt = neckTilt;
    00293   headPathPlanner.lastHeadPan  += direction.y;
    00294   headPathPlanner.lastHeadTilt += direction.x;
    00295
    00296   headPathPlanner.lastNeckTilt = jointRangeNeckTilt.limit(headPathPlanner.lastNeckTilt);
    00297   headPathPlanner.lastHeadPan  = jointRangeHeadPan.limit(headPathPlanner.lastHeadPan);
    00298   headPathPlanner.lastHeadTilt = jointRangeHeadTilt.limit(headPathPlanner.lastHeadTilt);
    00299
    00300   if (fabs(headPathPlanner.lastHeadPan) == jointRangeHeadPan.max)
    00301     setJointsMaxPanReached = true;
    00302
    00303   headMotionRequest.tilt  = (long)(headPathPlanner.lastNeckTilt*1000000.0);
    00304   headMotionRequest.pan   = (long)(headPathPlanner.lastHeadPan *1000000.0);
    00305   headMotionRequest.roll  = (long)(headPathPlanner.lastHeadTilt*1000000.0);
    00306 #ifndef _WIN32
    00307   //headMotionRequest.roll += 150000; // add a little to compensate for the head's weight...;
    00308 #endif
    00309   headMotionRequest.mouth = (long)(mouth*1000000);
    00310
    00311   if(lastRobotPose != robotPose)
    00312   {
    00313     lastOdometryData = currentOdometryData;
    00314     lastRobotPose = robotPose;
    00315   }
    00316 }
    00317
    00318
    00319
    00320
    00321 // tools
    00322 void GT2005HeadControl::aimAtLandmark(int landmark, double& neckTilt, double& headPan, double& headTilt)
    00323 {
    00324   if(targetPointOnLandmark[landmark].height > 0)
    00325   {
    00326     simpleLookAtPointOnField(
    00327       targetPointOnLandmark[landmark].position,
    00328       Vector2<int>(0, -cameraInfo.resolutionHeight/3),
    00329       neckTilt, headPan, headTilt);
    00330   }
    00331   else
    00332   {
    00333     simpleLookAtPointOnField(
    00334       targetPointOnLandmark[landmark].position,
    00335       Vector2<int>(0, 0), //+cameraInfo.resolutionHeight/3),
    00336       neckTilt, headPan, headTilt);
    00337   }
    00338 }
    00339
    00340
    00341
    00342 void GT2005HeadControl::getLookAtBallAngles(const Vector2<double> ballOnField, double& neckTilt, double& headPan, double& headTilt)
    00343 {
    00344
    00345   Vector2<double> ballPos = Geometry::fieldCoord2Relative(robotPose, ballOnField);
    00346   if (ballPos.abs() < 100) // clip ball distances close to and inside of the robot
    00347     ballPos.x = 100;
    00348
    00349   double minBClose = 300, maxBClose = 2000;
    00350   Range<double> ballCloseRange(minBClose, maxBClose);
    00351   double ballIsClose = ballPos.abs();
    00352   ballIsClose = ballCloseRange.limit(ballIsClose);
    00353   ballIsClose = (ballIsClose - minBClose)/(maxBClose-minBClose);
    00354
    00355   simpleLookAtPointRelativeToRobot(
    00356     Vector3<double>(ballPos.x, ballPos.y, 1.8*ballRadius),
    00357     Vector2<int>(0, int( (.167-ballIsClose)*cameraInfo.resolutionHeight/3)),
    00358     neckTilt, headPan, headTilt);
    00359 }
    00360
    00361 void GT2005HeadControl::simpleLookAtPointRelativeToRobot(const Vector3<double> pos, Vector2<int> offsetInImage,
    00362                                     double& neckTilt, double& headPan, double& headTilt)
    00363 {
    00364   Vector3<double> target(pos);
    00365
    00366   /** transform the targets position to the robot's "neck coord. system": */
    00367   target.z -= bodyPosture.neckHeightCalculatedFromLegSensors;
    00368   RotationMatrix bodyRotation;
    00369   bodyRotation.rotateY(-bodyPosture.bodyTiltCalculatedFromLegSensors).
    00370     rotateX(bodyPosture.bodyRollCalculatedFromLegSensors); /** what about the order of the rotations? */
    00371   target = bodyRotation*target;                            /** rotate by the body's rotation(s) */
    00372
    00373
    00374   neckTilt = -0.0;                                    /** first try the calculation of head joints with fixed neck tilt: */
    00375   if (!simpleLookAtPointFixNeckTilt(target, neckTilt, headPan, headTilt))
    00376   {
    00377     if (headTilt < jointLimitHeadTiltN)
    00378     {
    00379       neckTilt = headTilt - jointLimitHeadTiltN;
    00380       headTilt = jointLimitHeadTiltN;
    00381     }
    00382     else if (headTilt > jointLimitHeadTiltP)
    00383   {
    00384     // clip to maximum possible joint angle
    00385     headTilt = jointLimitHeadTiltP;
    00386   }
    00387
    00388     //lookAtPointFixHeadTilt(target, neckTilt, headPan, headTilt);
    00389   }
    00390
    00391   /** now that the angles are found, add the offset-in-image: */
    00392
    00393   /** clip image offset to image bounderies: */
    00394   const Range<int> cameraResY(-cameraInfo.resolutionHeight / 2, cameraInfo.resolutionHeight / 2);
    00395   const Range<int> cameraResX(-cameraInfo.resolutionWidth / 2, cameraInfo.resolutionWidth / 2);
    00396   offsetInImage.x = cameraResX.limit(offsetInImage.x);
    00397   offsetInImage.y = cameraResY.limit(offsetInImage.y);
    00398
    00399   /** add angles for offset in image: */
    00400   headPan  += ((double)offsetInImage.x/cameraInfo.resolutionWidth)*cameraInfo.openingAngleWidth;
    00401   headTilt += ((double)offsetInImage.y/cameraInfo.resolutionHeight)*cameraInfo.openingAngleHeight;
    00402
    00403   /** perform clipping to ranges of possible joint values: */
    00404   const Range<double> jointRangeNeckTilt(jointLimitNeckTiltN,jointLimitNeckTiltP);
    00405   const Range<double> jointRangeHeadPan(jointLimitHeadPanN, jointLimitHeadPanP);
    00406   const Range<double> jointRangeHeadTilt(jointLimitHeadTiltN, jointLimitHeadTiltP);
    00407   neckTilt = jointRangeNeckTilt.limit(neckTilt);
    00408   headPan = jointRangeHeadPan.limit(headPan);
    00409   headTilt = jointRangeHeadTilt.limit(headTilt);
    00410 }
    00411
    00412
    00413 void GT2005HeadControl::simpleLookAtPointOnField(const Vector3<double> pos, Vector2<int> offsetInImage,
    00414                                     double& neckTilt, double& headPan, double& headTilt)
    00415 {
    00416   /** transform the targets position to the robot's "coord. system": */
    00417   Vector3<double> target(pos);
    00418   Pose2D robotPose = this->robotPose.getPose();
    00419   target.x -= robotPose.translation.x;                /** transform from world to relative coordinates */
    00420   target.y -= robotPose.translation.y;
    00421
    00422   RotationMatrix bodyRotation;
    00423   bodyRotation.rotateZ(-robotPose.getAngle());        /** what about the order of the rotations? */
    00424   target = bodyRotation*target;                       /** rotate by the body's rotation(s) */
    00425
    00426   simpleLookAtPointRelativeToRobot(target, offsetInImage, neckTilt, headPan, headTilt);
    00427 }
    00428
    00429
    00430 bool GT2005HeadControl::simpleLookAtPointFixNeckTilt(const Vector3<double> &aim, const double& neckTilt, double& headPan, double& headTilt)
    00431 {
    00432   Vector3<double> target(aim);
    00433
    00434   RotationMatrix rotationByNeckTilt;
    00435   rotationByNeckTilt.rotateY(neckTilt);     /** transformation from "neck-coord" into cam. coord., i.e. */
    00436   target.z -= distanceNeckToPanCenter;      /** translation by distanceNeckToPanCenter   */
    00437   target = rotationByNeckTilt*target;       /** a rotation by the neck-joint Angle followed by */
    00438
    00439   headPan = atan2(target.y, target.x);      /** get the pan angle from looking at where the target is in the xy plane */
    00440                                             /** never mind clipping: if (headPan>=pi) headPan -= pi2; else if (headPan<-pi) headPan += pi2; */
    00441   RotationMatrix rotationByHeadPan;
    00442   rotationByHeadPan.rotateZ(-headPan);
    00443   target = rotationByHeadPan*target;        /** perform (pan-) rotation on target */
    00444
    00445   headTilt = atan2(target.z, target.x);     /** get the headTilt angle by taking the angle to the target in the XZ-Plane */
    00446                                             /** again, no clipping: if (headTilt >= pi) headTilt -= pi2; else if (headTilt < -pi) headTilt += pi2; */
    00447                                             /** For debugging: Rotate camera coord. system by the tilt just found. If everything was done correctly, target should have y = z = 0.0 */
    00448                                             /** RotationMatrix rotationByHeadTilt; rotationByHeadTilt.rotateY(headTilt); target = rotationByHeadTilt*target; */
    00449
    00450   /** if values are our of bounds, return 0 so a different method can be applied! */
    00451   if ((headTilt < jointLimitHeadTiltN) || (headTilt > jointLimitHeadTiltP) ||
    00452     (headPan < jointLimitHeadPanN) || (headPan > jointLimitHeadPanP))
    00453     return false;
    00454
    00455   return true;
    00456 }
    00457
    00458
    00459
    00460 int GT2005HeadControl::calculateClosestLandmark(double direction, double nextLeftOrRight)
    00461 {
    00462   return GT2005HeadControl::calculateClosestLandmark(robotPose, direction, nextLeftOrRight);
    00463 }
    00464
    00465 int GT2005HeadControl::calculateClosestLandmark(Pose2D pose, double direction, double nextLeftOrRight, bool disregardFieldMarkings)
    00466 {
    00467   int i, closest = -1;
    00468   double smallestAngle = 3;
    00469
    00470   LINE(headControlField,
    00471     pose.translation.x, pose.translation.y,
    00472     pose.translation.x + 4000*cos(direction+pose.rotation), pose.translation.y + 4000*sin(direction+pose.rotation),
    00473     20, 0, Drawings::blue);
    00474
    00475   for (i = 0; i < numOfLandmarks; i++)
    00476   {
    00477     if ((disregardFieldMarkings) && (targetPointOnLandmark[i].position.z == 0))
    00478     {
    00479       CIRCLE(headControlField,
    00480         targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 35,
    00481         2, 0, 1);
    00482       continue;
    00483     }
    00484     else
    00485     {
    00486     CIRCLE(headControlField,
    00487       targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 35,
    00488       2, 0, Drawings::blue);
    00489     }
    00490     Vector2<double> landMarkInRobotCoordinates(targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y);
    00491     landMarkInRobotCoordinates -= pose.translation;
    00492     double distToLandmark = landMarkInRobotCoordinates.abs();
    00493
    00494     if (((targetPointOnLandmark[i].height > 0) && (distToLandmark > 250)) ||       // Landmarks such as beacons and goals can be useful from far away
    00495        ((targetPointOnLandmark[i].height == 0) && (distToLandmark > 250) && (distToLandmark < 1500)))         // Landmarks on the floor are only useful if when they're close
    00496     {
    00497       double angleToLandmark;
    00498
    00499       if (nextLeftOrRight == 0)
    00500       {
    00501         angleToLandmark = fabs(normalize(
    00502           landMarkInRobotCoordinates.angle() - pose.rotation - direction));
    00503       }
    00504       else
    00505       {
    00506         angleToLandmark = sgn(nextLeftOrRight)*(normalize(
    00507           landMarkInRobotCoordinates.angle() - pose.rotation - direction));
    00508       }
    00509
    00510       if((0.0 <= angleToLandmark ) && (angleToLandmark < smallestAngle))
    00511       {
    00512         //CIRCLE(headControlField, targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 50, 2, 0, Drawings::blue);
    00513
    00514         smallestAngle = angleToLandmark;
    00515         closest = i;
    00516       }
    00517     }
    00518   }
    00519   return closest;
    00520 }
    00521
    00522 void GT2005HeadControl::setupMainAngles()
    00523 {
    00524     headLeft.x = 0.052400;
    00525   headLeft.y = 1.573200;
    00526   headLeft.z = -0.120000;
    00527
    00528   headLeftDown.x = 0.052400;
    00529   headLeftDown.y = 1.573200;
    00530   headLeftDown.z = -0.350000;
    00531
    00532   headRight.x = 0.052400;
    00533   headRight.y = -1.573200;
    00534   headRight.z = -0.0120000;
    00535
    00536   headRightDown.x = 0.052400;
    00537   headRightDown.y = -1.573200;
    00538   headRightDown.z = -0.350000;
    00539
    00540   headMiddleLeft.x = 0.052400;
    00541   headMiddleLeft.y = 0.560000;
    00542   headMiddleLeft.z = 0.062000;
    00543
    00544   headMiddleLeftDown.x = -0.1;
    00545   headMiddleLeftDown.y = 0.560000;
    00546   headMiddleLeftDown.z = -0.2;
    00547
    00548   headMiddleRight.x = 0.052400;
    00549   headMiddleRight.y = -0.560000;
    00550   headMiddleRight.z = 0.062000;
    00551
    00552   headMiddleRightDown.x = -0.1;
    00553   headMiddleRightDown.y = -0.560000;
    00554   headMiddleRightDown.z = -0.2;
    00555
    00556     headUp.x = 0.052400;
    00557     headUp.y = 0;
    00558     headUp.z = 0.034500;
    00559
    00560     headDown.x = -0.55000;
    00561     headDown.y = 0;
    00562     headDown.z = -2.8;
    00563 }
    00564
    00565
    00566 void GT2005HeadControl::calibrateHeadSpeed()
    00567 {
    00568   // First look up-left,wait some time (roboter has to get up)
    00569   // second look from left to right time (time this)
    00570   // third look up down, using tilt1 (time this)
    00571   // fourth look up down, using tilt2 (time this)
    00572
    00573   Vector3<double> calibrationTilt1Down (-1.385604,0.0,0.040000);
    00574   Vector3<double> calibrationTilt1Up (0.052359,0.0,0.040000);
    00575
    00576   Vector3<double> calibrationTilt2Down (0.0,0.0,-0.326175);
    00577   Vector3<double> calibrationTilt2Up (0.0,0.0,0.750630);
    00578
    00579   Vector3<double> calibrationLeft  (0.052400, 1.608598,-0.120000);
    00580   Vector3<double> calibrationRight (0.052400,-1.608598,-0.120000);
    00581
    00582   enum {calibrationRounds = 3};
    00583
    00584   if (headPathPlanner.isLastPathFinished() || calibrationState == calibrationStateStart)
    00585   {
    00586     switch(calibrationState)
    00587     {
    00588     case calibrationStateStart:
    00589     {
    00590       Vector3<double> points[]={headUp,headUp};
    00591       long durations[] = {0,1000};
    00592
    00593       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
    00594       calibrationState = calibrationStateLeft;
    00595       calibrationRoundCount = 0;
    00596       calibrationSuccessfulRounds = 0;
    00597       speedNeckTilt = 0;
    00598       speedHeadPan = 0;
    00599       speedHeadTilt = 0;
    00600       break;
    00601     }
    00602     case calibrationStateLeft:
    00603     {
    00604       Vector3<double> points[]={calibrationLeft,calibrationLeft};
    00605       long durations[] = {2000,200};
    00606       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00607       calibrationState = calibrationStateLeftWait;
    00608       calibrationTime = SystemCall::getCurrentSystemTime();
    00609       break;
    00610     }
    00611     case calibrationStateLeftWait:
    00612       if (headPositionReached(calibrationLeft) || isTimedOut())
    00613         calibrationState = calibrationStateRight;
    00614       break;
    00615     case calibrationStateRight:
    00616     {
    00617         Vector3<double> points[]={calibrationRight};
    00618       long durations[] = {0};
    00619       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00620       calibrationState = calibrationStateRightWait;
    00621       calibrationTime = SystemCall::getCurrentSystemTime();
    00622       break;
    00623     }
    00624     case calibrationStateRightWait:
    00625       if (headPositionReached(calibrationRight) || isTimedOut())
    00626       {
    00627         if (!isTimedOut())
    00628         {
    00629           speedHeadPan += fabs(calibrationLeft.y-calibrationRight.y) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
    00630           calibrationSuccessfulRounds++;
    00631         }
    00632         else
    00633           calibrationTimeOutsPan++;
    00634         if (calibrationRoundCount < calibrationRounds-1)
    00635         {
    00636           calibrationState = calibrationStateLeft;
    00637           calibrationRoundCount++;
    00638         }
    00639         else
    00640         {
    00641           // calibration of joint ready
    00642           speedHeadPan /= calibrationSuccessfulRounds;
    00643           calibrationSuccessfulRounds = 0;
    00644           calibrationRoundCount = 0;
    00645           calibrationState = calibrationStateDownTilt1;
    00646         }
    00647       }
    00648       break;
    00649     case calibrationStateDownTilt1:
    00650     {
    00651       Vector3<double> points[]={calibrationTilt1Down,calibrationTilt1Down};
    00652       long durations[] = {1500,200};
    00653       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00654       calibrationState = calibrationStateDownTilt1Wait;
    00655       calibrationTime = SystemCall::getCurrentSystemTime();
    00656       break;
    00657     }
    00658     case calibrationStateDownTilt1Wait:
    00659       if (headPositionReached(calibrationTilt1Down) || isTimedOut())
    00660         calibrationState = calibrationStateUpTilt1;
    00661       break;
    00662     case calibrationStateUpTilt1:
    00663     {
    00664       Vector3<double> points[]={calibrationTilt1Up};
    00665       long durations[] = {0};
    00666       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00667       calibrationState = calibrationStateUpTilt1Wait;
    00668       calibrationTime = SystemCall::getCurrentSystemTime();
    00669       break;
    00670     }
    00671     case calibrationStateUpTilt1Wait:
    00672       if (headPositionReached(calibrationTilt1Up) || isTimedOut())
    00673       {
    00674         if (!isTimedOut())
    00675         {
    00676           speedNeckTilt += fabs(calibrationTilt1Down.x-calibrationTilt1Up.x) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
    00677           calibrationSuccessfulRounds++;
    00678         }
    00679         else
    00680           calibrationTimeOutsTilt1++;
    00681         if (calibrationRoundCount < calibrationRounds-1)
    00682         {
    00683           calibrationState = calibrationStateDownTilt1;
    00684           calibrationRoundCount++;
    00685         }
    00686         else
    00687         {
    00688           // calibration of joint ready
    00689           speedNeckTilt /= calibrationSuccessfulRounds;
    00690           calibrationSuccessfulRounds = 0;
    00691           calibrationRoundCount = 0;
    00692           calibrationState = calibrationStateDownTilt2;
    00693         }
    00694       }
    00695       break;
    00696     case calibrationStateDownTilt2:
    00697     {
    00698       Vector3<double> points[]={calibrationTilt2Down,calibrationTilt2Down};
    00699       long durations[] = {1500,200};
    00700       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00701       calibrationState = calibrationStateDownTilt2Wait;
    00702       calibrationTime = SystemCall::getCurrentSystemTime();
    00703       break;
    00704     }
    00705     case calibrationStateDownTilt2Wait:
    00706       if (headPositionReached(calibrationTilt2Down) || isTimedOut())
    00707         calibrationState = calibrationStateUpTilt2;
    00708       break;
    00709     case calibrationStateUpTilt2:
    00710     {
    00711       Vector3<double> points[]={calibrationTilt2Up};
    00712       long durations[] = {0};
    00713       headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
    00714       calibrationState = calibrationStateUpTilt2Wait;
    00715       calibrationTime = SystemCall::getCurrentSystemTime();
    00716       break;
    00717     }
    00718     case calibrationStateUpTilt2Wait:
    00719       if (headPositionReached(calibrationTilt2Up) || isTimedOut())
    00720       {
    00721         if (!isTimedOut())
    00722         {
    00723           speedHeadTilt += fabs(calibrationTilt2Down.z-calibrationTilt2Up.z) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
    00724           calibrationSuccessfulRounds++;
    00725         }
    00726         else
    00727           calibrationTimeOutsTilt2++;
    00728         if (calibrationRoundCount < calibrationRounds-1)
    00729         {
    00730           calibrationState = calibrationStateDownTilt2;
    00731           calibrationRoundCount++;
    00732         }
    00733         else
    00734         {
    00735           // calibration of joint ready
    00736           speedHeadTilt /= calibrationSuccessfulRounds;
    00737           calibrationSuccessfulRounds = 0;
    00738           calibrationRoundCount = 0;
    00739           calibrationState = calibrationStateUseResults;
    00740         }
    00741       }
    00742
    00743       break;
    00744     case calibrationStateUseResults:
    00745       if (   calibrationTimeOutsTilt1 == 0
    00746           && calibrationTimeOutsPan   == 0
    00747           && calibrationTimeOutsTilt2 == 0)
    00748       {
    00749         OUTPUT(idText,text,"Headspeed calibration was successful");
    00750       }
    00751       else
    00752       {
    00753         OUTPUT(idText,text,"Headspeed calibration failed. ");
    00754         OUTPUT(idText,text,"Check the values of function headPositionReached or buy a new head !");
    00755         OUTPUT(idText,text,"Rounds: " << calibrationRounds << "; Timeouts: Tilt1 = " << calibrationTimeOutsTilt1 << "; Pan = " << calibrationTimeOutsPan << "; Tilt2 = " << calibrationTimeOutsTilt2 );
    00756       }
    00757       // if too much timeout  occured, set to standard values
    00758       if (calibrationTimeOutsTilt1-calibrationRounds == 0) speedNeckTilt = 0.001500;
    00759       if (calibrationTimeOutsPan-calibrationRounds == 0)   speedHeadPan  = 0.005350;
    00760       if (calibrationTimeOutsTilt2-calibrationRounds == 0) speedHeadTilt = 0.002350;
    00761
    00762       OUTPUT(idText,text,"speedTilt1:" << speedNeckTilt);
    00763       OUTPUT(idText,text,"speedPan:" << speedHeadPan);
    00764       OUTPUT(idText,text,"speedTilt2:" << speedHeadTilt);
    00765
    00766
    00767       // setting speed in headpath planner
    00768       headPathPlanner.headPathSpeedNeckTilt = (double) speedNeckTilt;
    00769       headPathPlanner.headPathSpeedHeadPan   = (double) speedHeadPan;
    00770       headPathPlanner.headPathSpeedHeadTilt = (double) speedHeadTilt;
    00771       calibrationState = calibrationStateReady;
    00772       break;
    00773     default:
    00774     case calibrationStateReady:
    00775       break;
    00776     }
    00777   }
    00778 }