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

# Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.h
    00003 *
    00004 * Definition of class GT2005HeadPathPlanner.
    00005 *
    00006 * @author Uwe Düffert
    00007 */
    00008
    00009 #ifndef __GT2005HeadPathPlanner_h__
    00010 #define __GT2005HeadPathPlanner_h__
    00011
    00012 #include "Tools/Math/Vector3.h"
    00013 #include "Representations/Perception/SensorDataBuffer.h"
    00014
    00015 /**
    00016 * @class  Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner
    00017 *
    00018 * Calculate a smooth series of head joint angles from a gives
    00019 * set of way points and an overall duration.
    00020 *
    00021 * @author Uwe Düffert
    00022 */
    00023 class GT2005HeadPathPlanner
    00024 {
    00025 public:
    00026 /**
    00027 * Initializes a set of points to visit in a certain time
    00028 * @param vectors set of arcs to visit
    00029 * @param durations a set of timings which described the time between the arcs
    00030 * @param numberOfVectors number of Vector3s in param vectors
    00031 * @param optimizeTimings the timings will be optimized by the distance in angles for optimal moving speed
    00032 */
    00033   void init(const Vector3<double>* vectors=0, long* durations=0, int numberOfVectors=0,bool optimizeTimings=true);
    00034   void oldInit(const Vector3<double>* vectors=0,int numberOfVectors=0, long duration=0)
    00035   {
    00036     if (numberOfVectors==1)
    00037     {
    00038       long durations[1]={duration};
    00039       init(vectors,durations,1);
    00040     }
    00041     else
    00042     {
    00043       // divide in equal chunks
    00044       if (numberOfVectors!=0)
    00045       {
    00046         // division by zero work around
    00047         long chunkedDuration = duration / numberOfVectors;
    00048         long durations[maxNumberOfPoints+1];
    00049         // first time
    00050         durations[0]=0;
    00051         for (int i=1;i<=numberOfVectors;i++)
    00052           durations[i]=chunkedDuration;
    00053         init(vectors,durations,numberOfVectors);
    00054       }
    00055     }
    00056   }
    00057
    00058   /**
    00059   * default constructor
    00060   */
    00061
    00062   GT2005HeadPathPlanner(const SensorDataBuffer& sensorDataBuffer):lastNeckTilt(0),lastHeadPan(0),lastHeadTilt(0),currentPoint(0),currentFrame(0),numberOfFrames(0),numberOfPoints(0),sensorDataBuffer(sensorDataBuffer) {}
    00063
    00064
    00065   /**
    00066   * Calculates the angles for tilt pan and roll
    00067   * @return true if the last point is reached.
    00068   */
    00069   bool getAngles(double& neckTilt, double& headPan, double& headTilt);
    00070
    00071   /**
    00072   * Return whether the last initialized path is already finished
    00073   * @return true if last path is finished.
    00074   */
    00075   inline bool isLastPathFinished() { return (currentFrame>=numberOfFrames); }
    00076
    00077
    00078   /* returns the minimum time which is needed between the given headposition */
    00079   long calculateHeadTiming(Vector3<double> &pos1,Vector3<double> &pos2);
    00080
    00081   /* returns true, if the head has reached the given position */
    00082   bool headPositionReached(Vector3<double> pos);
    00083
    00084
    00085   /** The minimum head speed in rad per frame: 0.004 = 28.6°/s */
    00086   static const double minimumHeadSpeed;
    00087
    00088   /** The tilt calculated in the last frame */
    00089   double lastNeckTilt;
    00090
    00091   /** The pan calculated in the last frame */
    00092   double lastHeadPan;
    00093
    00094   /** The roll calculated in the last frame */
    00095   double lastHeadTilt;
    00096
    00097   /** the maximum speed of all angles */
    00098   double headPathSpeedNeckTilt,headPathSpeedHeadPan,headPathSpeedHeadTilt;
    00099
    00100   /** the sensordatabuffer */
    00101   const SensorDataBuffer& sensorDataBuffer;
    00102
    00103
    00104 private:
    00105
    00106   /**
    00107   * Return the whole Duration of the Headpath
    00108   * @return the sum of duration of the headpath
    00109   */
    00110   long calculateDurationsSum(long* duration, int durations);
    00111
    00112  /** index of the most recently reached point in head path,
    00113   * so the head is between points[currentPoint] and points[currentPoint+1] */
    00114   long currentPoint;
    00115
    00116   /** number of frames (a 8ms) since start of head path */
    00117   long currentFrame;
    00118
    00119   /** requested duration of head path in frames (a 8ms) */
    00120   long numberOfFrames;
    00121
    00122
    00123   /** maximum number of allowed points in head path*/
    00124   enum {maxNumberOfPoints = 20};
    00125
    00126   /** number of points in requested path */
    00127   long numberOfPoints;
    00128
    00129   /** the points the head shall visit during the head path*/
    00130   Vector3<double> points[maxNumberOfPoints];
    00131
    00132   /** number of the first frame after head path start for a certain point in head path */
    00133   double firstFrame[maxNumberOfPoints];
    00134
    00135 };
    00136
    00137
    00138 #endif //GT2005HeadPathPlanner