Modules/HeadControl/GT2004HeadControl/GT2004HeadPathPlanner.cpp

Go to the documentation of this file.

/**
* @file GT2004HeadPathPlanner.cpp
*
* Implementation of class GT2004HeadPathPlanner
*
* @author Uwe Düffert
* @author Marc Dassler
*/


#include "GT2004HeadPathPlanner.h"
#include "Tools/Math/Geometry.h"

const double GT2004HeadPathPlanner::minimumHeadSpeed=0.004;

void GT2004HeadPathPlanner::init(const Vector3<double>* vectors, long* durations,int numberOfVectors, bool optimizeTimings)
{
  const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();
  if (numberOfVectors>maxNumberOfPoints-1) numberOfVectors=maxNumberOfPoints-1;
  
  enum {FRAMEQUOTIENT = 8};
  
  const Range<double> jointRangeNeckTilt(robotDimensions.jointLimitNeckTiltN,robotDimensions.jointLimitNeckTiltP);
  const Range<double> jointRangeHeadPan( robotDimensions.jointLimitHeadPanN, robotDimensions.jointLimitHeadPanP);
  const Range<double> jointRangeHeadTilt(robotDimensions.jointLimitHeadTiltN,robotDimensions.jointLimitHeadTiltP);

  //start from current position
  points[0].x = jointRangeNeckTilt.limit(lastNeckTilt);
  points[0].y = jointRangeHeadPan.limit(lastHeadPan);
  points[0].z = jointRangeHeadTilt.limit(lastHeadTilt);
  // sometimes the values are weird, so the first frame should be ignored
  if (points[0].x != lastNeckTilt || points[0].y != lastHeadPan || points[0].z != lastHeadTilt)
  {
    points[0].x = jointRangeNeckTilt.limit(vectors[0].x);
    points[0].y = jointRangeHeadPan.limit(vectors[0].y);
    points[0].z = jointRangeHeadTilt.limit(vectors[0].z);
    durations[0]=8;
  }
  
  currentPoint = 0;
  numberOfPoints = numberOfVectors;
  currentFrame = 0;
 
  //calculate total distance and speed
   int i;
  for (i=0;i<numberOfVectors;i++)
  {
    //clip arcs to physical limits
    points[i+1].x = jointRangeNeckTilt.limit(vectors[i].x);
    points[i+1].y = jointRangeHeadPan.limit(vectors[i].y);
    points[i+1].z = jointRangeHeadTilt.limit(vectors[i].z);
    if (optimizeTimings)
    {
      long tempTime = calculateHeadTiming(points[i],points[i+1]);
      if (durations[i] < tempTime)
        durations[i] = tempTime;
    }
  }
  
  long overAllDuration = calculateDurationsSum(durations, numberOfVectors);
  numberOfFrames = overAllDuration / FRAMEQUOTIENT;
  
  //calculate duration for each part of the route
  firstFrame[0] = 0;
  for (i=0;i<numberOfVectors;i++)
  {
    firstFrame[i + 1] = firstFrame[i] + (durations[i] / FRAMEQUOTIENT);
  }
}

long GT2004HeadPathPlanner::calculateDurationsSum(long* duration, int durations)
{
  long sum=0;
  for (int i=0;i<durations;i++)
  {
    // correcting, if duration is shorter than min movement time
    if (duration[i]<8) duration[i]=8;
    sum+=duration[i];
  }
  return sum;
}


bool GT2004HeadPathPlanner::headPositionReached(Vector3<double> pos)
{
  double grad2Rad = toMicroRad(pi / 180);
  Vector3<long> posInMicroRad;
  
  posInMicroRad.x = (long) toMicroRad(pos.x);
  posInMicroRad.y = (long) toMicroRad(pos.y);
  posInMicroRad.z = (long) toMicroRad(pos.z);

  if (   abs(sensorDataBuffer.lastFrame().data[SensorData::neckTilt]-posInMicroRad.x) < 8 * grad2Rad
      && abs(sensorDataBuffer.lastFrame().data[SensorData::headPan]-posInMicroRad.y)  < 8 * grad2Rad
      && abs(sensorDataBuffer.lastFrame().data[SensorData::headTilt]-posInMicroRad.z) < 10 * grad2Rad)
    return true;
  return false;
}


bool GT2004HeadPathPlanner::getAngles(double& neckTilt, double& headPan, double& headTilt)
{
  if (currentFrame<numberOfFrames)
  {
    currentFrame++;
    while ((currentFrame>firstFrame[currentPoint+1])&&
           (currentPoint<numberOfPoints-1)&&
           (currentFrame<numberOfFrames))
    {
      currentPoint++;
    }
        
    double distanceInFrames=0;
    if (currentPoint<numberOfPoints)
      distanceInFrames = firstFrame[currentPoint+1]-firstFrame[currentPoint];
    
    if (distanceInFrames==0)
    {
      neckTilt = points[currentPoint].x;
      headPan  = points[currentPoint].y;
      headTilt = points[currentPoint].z;
    }
    else
    {
      double leftFactor = (firstFrame[currentPoint+1]-currentFrame)/distanceInFrames;
      double rightFactor = 1-leftFactor;
      neckTilt = (leftFactor*points[currentPoint].x + rightFactor*points[currentPoint+1].x);
      headPan  = (leftFactor*points[currentPoint].y + rightFactor*points[currentPoint+1].y);
      headTilt = (leftFactor*points[currentPoint].z + rightFactor*points[currentPoint+1].z);
    }
  }
  else
  {
    neckTilt = (points[currentPoint+1].x);
    headPan  = (points[currentPoint+1].y);
    headTilt = (points[currentPoint+1].z);
  }
  return (currentFrame>=numberOfFrames);
}

long GT2004HeadPathPlanner::calculateHeadTiming(Vector3<double> &pos1,Vector3<double> &pos2)
{
  Vector3<double> minSpeed;
  minSpeed.x = fabs(pos1.x-pos2.x)/headPathSpeedNeckTilt;
  minSpeed.y = fabs(pos1.y-pos2.y)/headPathSpeedHeadPan;
  minSpeed.z = fabs(pos1.z-pos2.z)/headPathSpeedHeadTilt;

  // explain slowest speed
  return (long) max(max(minSpeed.x,minSpeed.y),minSpeed.z);
}

/*
* Change log :
* 
* $Log: GT2004HeadPathPlanner.cpp,v $
* Revision 1.8  2004/06/27 15:37:45  dassler
* introduced ball speed to headcontrol
*
* Revision 1.7  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.6  2004/06/16 10:39:42  jhoffman
* - made head path planner more robust thus removing the look-left/right bug
*
* Revision 1.5  2004/06/14 20:12:10  jhoffman
* - numerous changes and additions to headcontrol
* - cameraInfo default constructor now creates ERS7 info
* - debug drawing "headcontrolfield" added
*
* 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)
*
*/

---