Tools/PotentialFields/Motionfield.cpp

Go to the documentation of this file.

/**
* @file Motionfield.cpp
* 
* Implementation of class Motionfield
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/

#include "Motionfield.h"
#include "FieldObject.h"
#include "PfieldDatatypes.h"
#include "RandomMotionGenerator.h"
#include "AStarSearch.h"
#include "PotentialfieldComposition.h"
#ifdef POTENTIALFIELDS_FOR_GT2004_
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/DebugDrawings.h"
#endif //POTENTIALFIELDS_FOR_GT2004_


Motionfield::Motionfield(const std::string& name)
{
  this->name = name;
  randomMotionGenerator = 0;
  pathPlanner = 0;
  pathPlannerActive = false;
  alwaysUsePathPlanner = false;
  translationDisabled = false;
  rotationDisabled = false;
  lastMotionVector.x = 0.0;
  lastMotionVector.y = 0.0;
}


Motionfield::~Motionfield()
{
  if(randomMotionGenerator)
  {
    delete randomMotionGenerator;
  }
  if(pathPlanner)
  {
    delete pathPlanner;
  }
  if(aStarParameterSet.stabilizationObject)
  {
    delete aStarParameterSet.stabilizationObject;
  }
}


void Motionfield::execute(const PfPose& pose, PotentialfieldResult& result)
{
  result.actionPossible = true;
  result.subAction = "";
  PfVec gradientVec;
#ifdef NOPOTFIELD
  gradientVec = getFieldVecAt(pose);
#else
  if(pathPlanner == 0)
  {
    gradientVec = getFieldVecAt(pose);
  }
  else if(alwaysUsePathPlanner)
  {
    gradientVec = getFieldVecFromAStarSearch(pose);
  }
  else if(pathPlannerActive)
  {
    gradientVec = getFieldVecFromAStarSearch(pose);
    pathPlannerActive = pathPlanningStillNeeded(pose);
  }
  else
  {
    gradientVec = getFieldVecAt(pose);
    pathPlannerActive = reachedLocalMinimum(gradientVec.length());
  }
#endif
  correctMotionVector(gradientVec);
  lastMotionVector = gradientVec;
  gradientVec.rotate(-pose.rotation);
  if(translationDisabled) 
  {
    result.motion.pos.x = 0.0;
    result.motion.pos.y = 0.0;
  }
  else 
  {
    result.motion.pos = gradientVec;
    result.motion.pos.normalize();
  }
  if(rotationDisabled || (gradientVec.length() == 0)) 
  {
    result.motion.rotation = 0.0;
  }
  else 
  {
    result.motion.rotation = gradientVec.getAngle();
  }
  result.motion.speed = gradientVec.length();
  if(criterion == CRITERION_CONST)
  {
    result.value = criterionParameter;
  }
  else if(criterion == CRITERION_GRADIENT)
  {
    result.value = -result.motion.speed;
  } 
}


PfVec Motionfield::getFieldVecFromAStarSearch(const PfPose& pose)
{
  PotentialfieldAStarNode start;
  PfPose goalPose(this->goal->getPose(pose));
  PfPose startPose(pose);
  startPose.probabilityDistribution.clear();
  startPose.hasProbabilityDistribution = false;
  start.setPose(pose);
  start.setFunctionValues(0,0);
  start.setValueAtPos(aStarParameterSet);
  aStarParameterSet.startPosition = start.getPose().pos;
  PotentialfieldAStarNode goal;
  goal.setPose(goalPose);
  if(aStarParameterSet.useStabilization && aStarParameterSet.numberOfCalls)
  {
    PfVec stabilizationPosition(-lastMotionVector.x,-lastMotionVector.y);
    stabilizationPosition.normalize();
    stabilizationPosition *= aStarParameterSet.stabilizationDistance;
    stabilizationPosition += pose.pos;
    PfPose stabObjectPose;
    stabObjectPose.pos = stabilizationPosition;
    stabObjectPose.rotation = (pose.pos - stabilizationPosition).getAngle();
    aStarParameterSet.stabilizationObject->setPose(stabObjectPose);
  }
  PotentialfieldAStarNode nextPosition = 
      pathPlanner->search(start, goal, aStarParameterSet, plannedPathLengthToGoal);
  PfVec result(nextPosition.getPose().pos-pose.pos);
  result.normalize();
  result *= aStarParameterSet.standardGradientLength;
  aStarParameterSet.numberOfCalls++;
  return result;
}


bool Motionfield::reachedLocalMinimum(double currentGradientLength)
{
  return (currentGradientLength <= maxGradientLengthForLocalMinimum);
}


bool Motionfield::pathPlanningStillNeeded(const PfPose& pose)
{
  PfPose currentPose(pose);
  PfPose goalPose(this->goal->getPose());
  PfVec nextStep;
  double gradientTrajectoryLength(0.0);
  double stepLength(aStarParameterSet.minExpansionRadius);
  double maxLength(plannedPathLengthToGoal*2.0);
  while(gradientTrajectoryLength < maxLength)
  {
    if((currentPose.pos - goalPose.pos).length() < aStarParameterSet.distanceToGoal)
    {
      aStarParameterSet.numberOfCalls = 0; //reset call counter
#ifdef POTENTIALFIELDS_FOR_GT2004_
      DEBUG_DRAWING_FINISHED(behavior_aStarSearch); 
#endif //POTENTIALFIELDS_FOR_GT2004_
      return false;
    }
    nextStep = getFieldVecAt(currentPose);
    nextStep.normalize();
    nextStep *= stepLength;
    currentPose.addAbsVector(nextStep);
    gradientTrajectoryLength += stepLength;
  }
  return true;
}


void Motionfield::correctMotionVector(PfVec& motionVector) const
{
  double secondsSinceLastResult(((double)(getSystemTime()-lastResult.timeStamp))/1000.0);
  if(maxAccel > 0.0)
  {
    double currentSpeed(motionVector.length());
    double lastSpeed(lastMotionVector.length());
    double speedDiff((currentSpeed - lastSpeed)/secondsSinceLastResult);
    if(speedDiff > maxAccel)
    {
      motionVector.normalize();
      motionVector *= (lastSpeed + maxAccel*secondsSinceLastResult);
    }
    else if(speedDiff < -maxAccel)
    {
      motionVector.normalize();
      motionVector *= (lastSpeed - maxAccel*secondsSinceLastResult);
    }
  }
  if(maxGradientDifference > 0.0)
  {
    double currentAngle(motionVector.getAngle());
    double lastAngle(lastMotionVector.getAngle());
    double diffAngle(currentAngle-lastAngle);
    PfPose dummyPose; dummyPose.rotation = diffAngle; dummyPose.normRotation();
    diffAngle = dummyPose.rotation;
    double currentRotAccel(diffAngle / secondsSinceLastResult);
    if(currentRotAccel > maxGradientDifference)
    {
      motionVector.rotate(-diffAngle + maxGradientDifference*secondsSinceLastResult);
    }
    else if(currentRotAccel < -maxGradientDifference)
    {
      motionVector.rotate(-diffAngle - maxGradientDifference*secondsSinceLastResult);
    }
  }
}

---