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

# Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005HeadPathPlanner.cpp
    00003 *
    00004 * Implementation of class GT2005HeadPathPlanner
    00005 *
    00006 * @author Uwe Düffert
    00007 * @author Marc Dassler
    00008 */
    00009
    00010
    00011 #include "GT2005HeadPathPlanner.h"
    00012 #include "Tools/Math/Geometry.h"
    00013
    00014 const double GT2005HeadPathPlanner::minimumHeadSpeed=0.004;
    00015
    00016 void GT2005HeadPathPlanner::init(const Vector3<double>* vectors, long* durations,int numberOfVectors, bool optimizeTimings)
    00017 {
    00018   const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();
    00019   if (numberOfVectors>maxNumberOfPoints-1) numberOfVectors=maxNumberOfPoints-1;
    00020
    00021   enum {FRAMEQUOTIENT = 8};
    00022
    00023   const Range<double> jointRangeNeckTilt(robotDimensions.jointLimitNeckTiltN,robotDimensions.jointLimitNeckTiltP);
    00024   const Range<double> jointRangeHeadPan( robotDimensions.jointLimitHeadPanN, robotDimensions.jointLimitHeadPanP);
    00025   const Range<double> jointRangeHeadTilt(robotDimensions.jointLimitHeadTiltN,robotDimensions.jointLimitHeadTiltP);
    00026
    00027   //start from current position
    00028   points[0].x = jointRangeNeckTilt.limit(lastNeckTilt);
    00029   points[0].y = jointRangeHeadPan.limit(lastHeadPan);
    00030   points[0].z = jointRangeHeadTilt.limit(lastHeadTilt);
    00031   // sometimes the values are weird, so the first frame should be ignored
    00032   if (points[0].x != lastNeckTilt || points[0].y != lastHeadPan || points[0].z != lastHeadTilt)
    00033   {
    00034     points[0].x = jointRangeNeckTilt.limit(vectors[0].x);
    00035     points[0].y = jointRangeHeadPan.limit(vectors[0].y);
    00036     points[0].z = jointRangeHeadTilt.limit(vectors[0].z);
    00037     durations[0]=8;
    00038   }
    00039
    00040   currentPoint = 0;
    00041   numberOfPoints = numberOfVectors;
    00042   currentFrame = 0;
    00043
    00044   //calculate total distance and speed
    00045    int i;
    00046   for (i=0;i<numberOfVectors;i++)
    00047   {
    00048     //clip arcs to physical limits
    00049     points[i+1].x = jointRangeNeckTilt.limit(vectors[i].x);
    00050     points[i+1].y = jointRangeHeadPan.limit(vectors[i].y);
    00051     points[i+1].z = jointRangeHeadTilt.limit(vectors[i].z);
    00052     if (optimizeTimings)
    00053     {
    00054       long tempTime = calculateHeadTiming(points[i],points[i+1]);
    00055       if (durations[i] < tempTime)
    00056         durations[i] = tempTime;
    00057     }
    00058   }
    00059
    00060   long overAllDuration = calculateDurationsSum(durations, numberOfVectors);
    00061   numberOfFrames = overAllDuration / FRAMEQUOTIENT;
    00062
    00063   //calculate duration for each part of the route
    00064   firstFrame[0] = 0;
    00065   for (i=0;i<numberOfVectors;i++)
    00066   {
    00067     firstFrame[i + 1] = firstFrame[i] + (durations[i] / FRAMEQUOTIENT);
    00068   }
    00069 }
    00070
    00071 long GT2005HeadPathPlanner::calculateDurationsSum(long* duration, int durations)
    00072 {
    00073   long sum=0;
    00074   for (int i=0;i<durations;i++)
    00075   {
    00076     // correcting, if duration is shorter than min movement time
    00077     if (duration[i]<8) duration[i]=8;
    00078     sum+=duration[i];
    00079   }
    00080   return sum;
    00081 }
    00082
    00083
    00084 bool GT2005HeadPathPlanner::headPositionReached(Vector3<double> pos)
    00085 {
    00086   double grad2Rad = toMicroRad(pi / 180);
    00087   Vector3<long> posInMicroRad;
    00088
    00089   posInMicroRad.x = (long) toMicroRad(pos.x);
    00090   posInMicroRad.y = (long) toMicroRad(pos.y);
    00091   posInMicroRad.z = (long) toMicroRad(pos.z);
    00092
    00093   if (   abs(sensorDataBuffer.lastFrame().data[SensorData::neckTilt]-posInMicroRad.x) < 8 * grad2Rad
    00094       && abs(sensorDataBuffer.lastFrame().data[SensorData::headPan]-posInMicroRad.y)  < 8 * grad2Rad
    00095       && abs(sensorDataBuffer.lastFrame().data[SensorData::headTilt]-posInMicroRad.z) < 10 * grad2Rad)
    00096     return true;
    00097   return false;
    00098 }
    00099
    00100
    00101 bool GT2005HeadPathPlanner::getAngles(double& neckTilt, double& headPan, double& headTilt)
    00102 {
    00103   if (currentFrame<numberOfFrames)
    00104   {
    00105     currentFrame++;
    00106     while ((currentFrame>firstFrame[currentPoint+1])&&
    00107            (currentPoint<numberOfPoints-1)&&
    00108            (currentFrame<numberOfFrames))
    00109     {
    00110       currentPoint++;
    00111     }
    00112
    00113     double distanceInFrames=0;
    00114     if (currentPoint<numberOfPoints)
    00115       distanceInFrames = firstFrame[currentPoint+1]-firstFrame[currentPoint];
    00116
    00117     if (distanceInFrames==0)
    00118     {
    00119       neckTilt = points[currentPoint].x;
    00120       headPan  = points[currentPoint].y;
    00121       headTilt = points[currentPoint].z;
    00122     }
    00123     else
    00124     {
    00125       double leftFactor = (firstFrame[currentPoint+1]-currentFrame)/distanceInFrames;
    00126       double rightFactor = 1-leftFactor;
    00127       neckTilt = (leftFactor*points[currentPoint].x + rightFactor*points[currentPoint+1].x);
    00128       headPan  = (leftFactor*points[currentPoint].y + rightFactor*points[currentPoint+1].y);
    00129       headTilt = (leftFactor*points[currentPoint].z + rightFactor*points[currentPoint+1].z);
    00130     }
    00131   }
    00132   else
    00133   {
    00134     neckTilt = (points[currentPoint+1].x);
    00135     headPan  = (points[currentPoint+1].y);
    00136     headTilt = (points[currentPoint+1].z);
    00137   }
    00138   return (currentFrame>=numberOfFrames);
    00139 }
    00140
    00141 long GT2005HeadPathPlanner::calculateHeadTiming(Vector3<double> &pos1,Vector3<double> &pos2)
    00142 {
    00143   Vector3<double> minSpeed;
    00144   minSpeed.x = fabs(pos1.x-pos2.x)/headPathSpeedNeckTilt;
    00145   minSpeed.y = fabs(pos1.y-pos2.y)/headPathSpeedHeadPan;
    00146   minSpeed.z = fabs(pos1.z-pos2.z)/headPathSpeedHeadTilt;
    00147
    00148   // explain slowest speed
    00149   return (long) max(max(minSpeed.x,minSpeed.y),minSpeed.z);
    00150 }