Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/PotentialFields/Motionfield.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Motionfield.cpp
    00003 *
    00004 * Implementation of class Motionfield
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009 #include "Motionfield.h"
    00010 #include "FieldObject.h"
    00011 #include "PfieldDatatypes.h"
    00012 #include "RandomMotionGenerator.h"
    00013 #include "AStarSearch.h"
    00014 #include "PotentialfieldComposition.h"
    00015 #ifdef POTENTIALFIELDS_FOR_GT2004_
    00016 #include "Tools/Debugging/Debugging.h"
    00017 #include "Tools/Debugging/DebugDrawings.h"
    00018 #endif //POTENTIALFIELDS_FOR_GT2004_
    00019
    00020
    00021 Motionfield::Motionfield(const std::string& name)
    00022 {
    00023   this->name = name;
    00024   randomMotionGenerator = 0;
    00025   pathPlanner = 0;
    00026   pathPlannerActive = false;
    00027   alwaysUsePathPlanner = false;
    00028   translationDisabled = false;
    00029   rotationDisabled = false;
    00030   lastMotionVector.x = 0.0;
    00031   lastMotionVector.y = 0.0;
    00032 }
    00033
    00034
    00035 Motionfield::~Motionfield()
    00036 {
    00037   if(randomMotionGenerator)
    00038   {
    00039     delete randomMotionGenerator;
    00040   }
    00041   if(pathPlanner)
    00042   {
    00043     delete pathPlanner;
    00044   }
    00045   if(aStarParameterSet.stabilizationObject)
    00046   {
    00047     delete aStarParameterSet.stabilizationObject;
    00048   }
    00049 }
    00050
    00051
    00052 void Motionfield::execute(const PfPose& pose, PotentialfieldResult& result)
    00053 {
    00054   result.actionPossible = true;
    00055   result.subAction = "";
    00056   PfVec gradientVec;
    00057 #ifdef NOPOTFIELD
    00058   gradientVec = getFieldVecAt(pose);
    00059 #else
    00060   if(pathPlanner == 0)
    00061   {
    00062     gradientVec = getFieldVecAt(pose);
    00063   }
    00064   else if(alwaysUsePathPlanner)
    00065   {
    00066     gradientVec = getFieldVecFromAStarSearch(pose);
    00067   }
    00068   else if(pathPlannerActive)
    00069   {
    00070     gradientVec = getFieldVecFromAStarSearch(pose);
    00071     pathPlannerActive = pathPlanningStillNeeded(pose);
    00072   }
    00073   else
    00074   {
    00075     gradientVec = getFieldVecAt(pose);
    00076     pathPlannerActive = reachedLocalMinimum(gradientVec.length());
    00077   }
    00078 #endif
    00079   correctMotionVector(gradientVec);
    00080   lastMotionVector = gradientVec;
    00081   gradientVec.rotate(-pose.rotation);
    00082   if(translationDisabled)
    00083   {
    00084     result.motion.pos.x = 0.0;
    00085     result.motion.pos.y = 0.0;
    00086   }
    00087   else
    00088   {
    00089     result.motion.pos = gradientVec;
    00090     result.motion.pos.normalize();
    00091   }
    00092   if(rotationDisabled || (gradientVec.length() == 0))
    00093   {
    00094     result.motion.rotation = 0.0;
    00095   }
    00096   else
    00097   {
    00098     result.motion.rotation = gradientVec.getAngle();
    00099   }
    00100   result.motion.speed = gradientVec.length();
    00101   if(criterion == CRITERION_CONST)
    00102   {
    00103     result.value = criterionParameter;
    00104   }
    00105   else if(criterion == CRITERION_GRADIENT)
    00106   {
    00107     result.value = -result.motion.speed;
    00108   }
    00109 }
    00110
    00111
    00112 PfVec Motionfield::getFieldVecFromAStarSearch(const PfPose& pose)
    00113 {
    00114   PotentialfieldAStarNode start;
    00115   PfPose goalPose(this->goal->getPose(pose));
    00116   PfPose startPose(pose);
    00117   startPose.probabilityDistribution.clear();
    00118   startPose.hasProbabilityDistribution = false;
    00119   start.setPose(pose);
    00120   start.setFunctionValues(0,0);
    00121   start.setValueAtPos(aStarParameterSet);
    00122   aStarParameterSet.startPosition = start.getPose().pos;
    00123   PotentialfieldAStarNode goal;
    00124   goal.setPose(goalPose);
    00125   if(aStarParameterSet.useStabilization && aStarParameterSet.numberOfCalls)
    00126   {
    00127     PfVec stabilizationPosition(-lastMotionVector.x,-lastMotionVector.y);
    00128     stabilizationPosition.normalize();
    00129     stabilizationPosition *= aStarParameterSet.stabilizationDistance;
    00130     stabilizationPosition += pose.pos;
    00131     PfPose stabObjectPose;
    00132     stabObjectPose.pos = stabilizationPosition;
    00133     stabObjectPose.rotation = (pose.pos - stabilizationPosition).getAngle();
    00134     aStarParameterSet.stabilizationObject->setPose(stabObjectPose);
    00135   }
    00136   PotentialfieldAStarNode nextPosition =
    00137       pathPlanner->search(start, goal, aStarParameterSet, plannedPathLengthToGoal);
    00138   PfVec result(nextPosition.getPose().pos-pose.pos);
    00139   result.normalize();
    00140   result *= aStarParameterSet.standardGradientLength;
    00141   aStarParameterSet.numberOfCalls++;
    00142   return result;
    00143 }
    00144
    00145
    00146 bool Motionfield::reachedLocalMinimum(double currentGradientLength)
    00147 {
    00148   return (currentGradientLength <= maxGradientLengthForLocalMinimum);
    00149 }
    00150
    00151
    00152 bool Motionfield::pathPlanningStillNeeded(const PfPose& pose)
    00153 {
    00154   PfPose currentPose(pose);
    00155   PfPose goalPose(this->goal->getPose());
    00156   PfVec nextStep;
    00157   double gradientTrajectoryLength(0.0);
    00158   double stepLength(aStarParameterSet.minExpansionRadius);
    00159   double maxLength(plannedPathLengthToGoal*2.0);
    00160   while(gradientTrajectoryLength < maxLength)
    00161   {
    00162     if((currentPose.pos - goalPose.pos).length() < aStarParameterSet.distanceToGoal)
    00163     {
    00164       aStarParameterSet.numberOfCalls = 0; //reset call counter
    00165 #ifdef POTENTIALFIELDS_FOR_GT2004_
    00166       DEBUG_DRAWING_FINISHED(behavior_aStarSearch);
    00167 #endif //POTENTIALFIELDS_FOR_GT2004_
    00168       return false;
    00169     }
    00170     nextStep = getFieldVecAt(currentPose);
    00171     nextStep.normalize();
    00172     nextStep *= stepLength;
    00173     currentPose.addAbsVector(nextStep);
    00174     gradientTrajectoryLength += stepLength;
    00175   }
    00176   return true;
    00177 }
    00178
    00179
    00180 void Motionfield::correctMotionVector(PfVec& motionVector) const
    00181 {
    00182   double secondsSinceLastResult(((double)(getSystemTime()-lastResult.timeStamp))/1000.0);
    00183   if(maxAccel > 0.0)
    00184   {
    00185     double currentSpeed(motionVector.length());
    00186     double lastSpeed(lastMotionVector.length());
    00187     double speedDiff((currentSpeed - lastSpeed)/secondsSinceLastResult);
    00188     if(speedDiff > maxAccel)
    00189     {
    00190       motionVector.normalize();
    00191       motionVector *= (lastSpeed + maxAccel*secondsSinceLastResult);
    00192     }
    00193     else if(speedDiff < -maxAccel)
    00194     {
    00195       motionVector.normalize();
    00196       motionVector *= (lastSpeed - maxAccel*secondsSinceLastResult);
    00197     }
    00198   }
    00199   if(maxGradientDifference > 0.0)
    00200   {
    00201     double currentAngle(motionVector.getAngle());
    00202     double lastAngle(lastMotionVector.getAngle());
    00203     double diffAngle(currentAngle-lastAngle);
    00204     PfPose dummyPose; dummyPose.rotation = diffAngle; dummyPose.normRotation();
    00205     diffAngle = dummyPose.rotation;
    00206     double currentRotAccel(diffAngle / secondsSinceLastResult);
    00207     if(currentRotAccel > maxGradientDifference)
    00208     {
    00209       motionVector.rotate(-diffAngle + maxGradientDifference*secondsSinceLastResult);
    00210     }
    00211     else if(currentRotAccel < -maxGradientDifference)
    00212     {
    00213       motionVector.rotate(-diffAngle - maxGradientDifference*secondsSinceLastResult);
    00214     }
    00215   }
    00216 }