Modules/HeadControl/GT2004HeadControl/GT2004HeadPathPlanner.h

Go to the documentation of this file.

/**
* @file GT2004HeadPathPlanner.h
* 
* Definition of class GT2004HeadPathPlanner.
*
* @author Uwe Düffert
*/

#ifndef __GT2004HeadPathPlanner_h__
#define __GT2004HeadPathPlanner_h__

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

/** 
* @class GT2004HeadPathPlanner
*
* Calculate a smooth series of head joint angles from a gives 
* set of way points and an overall duration.
*
* @author Uwe Düffert
*/
class GT2004HeadPathPlanner
{
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
  */

  GT2004HeadPathPlanner(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 //__GT2004HeadPathPlanner_h__

/*
* Change log :
* 
* $Log: GT2004HeadPathPlanner.h,v $
* Revision 1.6  2004/06/17 14:34:46  dassler
* GT2004HeadControl updated
* Now looks after propergated ball, followed up withe the communicated ball
* GT2004HeadPathPlanner work now with time optimized moves
* Middle Beacons removed of the Landmarkspercept
*
* Revision 1.5  2004/06/16 10:39:42  jhoffman
* - made head path planner more robust thus removing the look-left/right bug
*
* Revision 1.4  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.3  2004/05/24 21:47:58  dueffert
* someone wanted headpathplanner to use rad
*
* Revision 1.2  2004/05/24 18:19:43  jhoffman
* microrad --> rad
*
* Revision 1.1.1.1  2004/05/22 17:19:24  cvsadm
* created new repository GT2004_WM
*
* Revision 1.1  2004/05/14 11:37:08  loetzsch
* support for multiple xabsl2engines in different modules
* preliminary GT2004HeadControl (does not work at all)
*
*/

---