Modules/HeadControl/GT2005HeadControl/GT2005HeadPathPlanner.cpp

Go to the documentation of this file.

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


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

const double GT2005HeadPathPlanner::minimumHeadSpeed=0.004;

void GT2005HeadPathPlanner::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 GT2005HeadPathPlanner::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 GT2005HeadPathPlanner::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 GT2005HeadPathPlanner::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 GT2005HeadPathPlanner::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);
}

---