Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.h

Go to the documentation of this file.

/**
* @file Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.h
* 
* Definition of class GT2005HeadPathPlanner.
*
* @author Uwe Düffert
*/

#ifndef __GT2005HeadPathPlanner_h__
#define __GT2005HeadPathPlanner_h__

#include "Tools/Math/Vector3.h"
#include "Representations/Perception/SensorDataBuffer.h"

/** 
* @class  Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner
*
* Calculate a smooth series of head joint angles from a gives 
* set of way points and an overall duration.
*
* @author Uwe Düffert
*/
class GT2005HeadPathPlanner
{
public:
/**
* Initializes a set of points to visit in a certain time
* @param vectors set of arcs to visit
* @param durations a set of timings which described the time between the arcs
* @param numberOfVectors number of Vector3s in param vectors
* @param optimizeTimings the timings will be optimized by the distance in angles for optimal moving speed
*/
  void init(const Vector3<double>* vectors=0, long* durations=0, int numberOfVectors=0,bool optimizeTimings=true);
  void oldInit(const Vector3<double>* vectors=0,int numberOfVectors=0, long duration=0)
  {
    if (numberOfVectors==1)
    {
      long durations[1]={duration};
      init(vectors,durations,1);
    }
    else
    {
      // divide in equal chunks
      if (numberOfVectors!=0)
      {
        // division by zero work around
        long chunkedDuration = duration / numberOfVectors;
        long durations[maxNumberOfPoints+1];
        // first time
        durations[0]=0;
        for (int i=1;i<=numberOfVectors;i++)
          durations[i]=chunkedDuration;
        init(vectors,durations,numberOfVectors);
      }
    }
  }
  
  /**
  * default constructor
  */

  GT2005HeadPathPlanner(const SensorDataBuffer& sensorDataBuffer):lastNeckTilt(0),lastHeadPan(0),lastHeadTilt(0),currentPoint(0),currentFrame(0),numberOfFrames(0),numberOfPoints(0),sensorDataBuffer(sensorDataBuffer) {}
  
  
  /**
  * Calculates the angles for tilt pan and roll
  * @return true if the last point is reached.
  */
  bool getAngles(double& neckTilt, double& headPan, double& headTilt);
  
  /**
  * Return whether the last initialized path is already finished
  * @return true if last path is finished.
  */
  inline bool isLastPathFinished() { return (currentFrame>=numberOfFrames); }
      
    
  /* returns the minimum time which is needed between the given headposition */
  long calculateHeadTiming(Vector3<double> &pos1,Vector3<double> &pos2);

  /* returns true, if the head has reached the given position */
  bool headPositionReached(Vector3<double> pos);
  

  /** The minimum head speed in rad per frame: 0.004 = 28.6°/s */
  static const double minimumHeadSpeed;
 
  /** The tilt calculated in the last frame */
  double lastNeckTilt;
  
  /** The pan calculated in the last frame */
  double lastHeadPan;
  
  /** The roll calculated in the last frame */
  double lastHeadTilt;
  
  /** the maximum speed of all angles */
  double headPathSpeedNeckTilt,headPathSpeedHeadPan,headPathSpeedHeadTilt;

  /** the sensordatabuffer */
  const SensorDataBuffer& sensorDataBuffer;


private:

  /**
  * Return the whole Duration of the Headpath
  * @return the sum of duration of the headpath
  */
  long calculateDurationsSum(long* duration, int durations);

 /** index of the most recently reached point in head path,
  * so the head is between points[currentPoint] and points[currentPoint+1] */
  long currentPoint;

  /** number of frames (a 8ms) since start of head path */
  long currentFrame;

  /** requested duration of head path in frames (a 8ms) */
  long numberOfFrames;
 
  
  /** maximum number of allowed points in head path*/
  enum {maxNumberOfPoints = 20};

  /** number of points in requested path */
  long numberOfPoints;

  /** the points the head shall visit during the head path*/
  Vector3<double> points[maxNumberOfPoints];

  /** number of the first frame after head path start for a certain point in head path */
  double firstFrame[maxNumberOfPoints];

};


#endif //GT2005HeadPathPlanner

---