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

# Tools/PotentialFields/Actionfield.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Actionfield.cpp
    00003 *
    00004 * Implementation of class Actionfield
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009 #include "Actionfield.h"
    00010 #include "FutureWorldModelGenerator.h"
    00011 #include "FieldObject.h"
    00012 #include "PotentialfieldComposition.h"
    00013 #include "FormationObject.h"
    00014
    00015
    00016
    00017 Action::Action()
    00018 {
    00019   manipulatedObject = 0;
    00020   name = "";
    00021 }
    00022
    00023
    00024 Action::~Action()
    00025 {
    00026   impactAreas.clear();
    00027   std::vector < PotentialfieldTransformation* >::iterator t;
    00028   for(t = transformations.begin(); t != transformations.end(); ++t)
    00029   {
    00030     delete (*t);
    00031   }
    00032   transformations.clear();
    00033 }
    00034
    00035
    00036 Action::Action(const Action& a)
    00037 {
    00038   name = a.name;
    00039   manipulatedObject = a.manipulatedObject;
    00040   joinAction = a.joinAction;
    00041   actionType = a.actionType;
    00042   impactAreas = a.impactAreas;
    00043   std::vector < PotentialfieldTransformation* >::const_iterator t;
    00044   for(t = a.transformations.begin(); t != a.transformations.end(); ++t)
    00045   {
    00046     transformations.push_back((*t)->copy());
    00047   }
    00048 }
    00049
    00050
    00051 bool Action::canBeApplied(const PfPose& robotPose,
    00052                           const std::vector<PotentialFieldsObject*>& objects)
    00053 {
    00054   if((actionType == MOVE_SELF) || (actionType == MEASURE_SELF))
    00055   {
    00056     return true;
    00057   }
    00058   if(actionType == MEASURE_OBJECT)
    00059   {
    00060     return objects[objectIdx]->isActive();
    00061   }
    00062   else //(actionType == MOVE_OBJECT)
    00063   {
    00064     if(!(objects[objectIdx]->isActive()))
    00065     {
    00066       return false;
    00067     }
    00068     unsigned int i,j;
    00069     PfPose manipulatedObjectPose(objects[objectIdx]->getPose());
    00070     PfPose objPose, robPose;
    00071     if(manipulatedObjectPose.hasProbabilityDistribution)
    00072     {
    00073       if(robotPose.hasProbabilityDistribution)
    00074       {
    00075         for(j=0; j<robotPose.probabilityDistribution.size(); j++)
    00076         {
    00077           for(i=0; i<manipulatedObjectPose.probabilityDistribution.size(); i++)
    00078           {
    00079             if(poseInsideImpactArea(robotPose.probabilityDistribution[j],
    00080                                     manipulatedObjectPose.probabilityDistribution[i]))
    00081             {
    00082               return true;
    00083             }
    00084           }
    00085         }
    00086       }
    00087       else
    00088       {
    00089         for(i=0; i<manipulatedObjectPose.probabilityDistribution.size(); i++)
    00090         {
    00091           if(poseInsideImpactArea(robotPose,
    00092                                   manipulatedObjectPose.probabilityDistribution[i]))
    00093           {
    00094             return true;
    00095           }
    00096         }
    00097       }
    00098     }
    00099     else
    00100     {
    00101       if(robotPose.hasProbabilityDistribution)
    00102       {
    00103         for(i=0; i<robotPose.probabilityDistribution.size(); i++)
    00104         {
    00105           if(poseInsideImpactArea(robotPose.probabilityDistribution[i],
    00106                                   manipulatedObjectPose))
    00107           {
    00108             return true;
    00109           }
    00110         }
    00111       }
    00112       else
    00113       {
    00114         return poseInsideImpactArea(robotPose, manipulatedObjectPose);
    00115       }
    00116     }
    00117   }
    00118   return false;
    00119 }
    00120
    00121
    00122 inline bool Action::poseInsideImpactArea(const PfPose& robPose, const PfPose& objPose)
    00123 {
    00124   PfVec relPosition(objPose.pos);
    00125   relPosition -= robPose.pos;
    00126   relPosition.rotate(-robPose.rotation);
    00127   std::vector< Polygon >::const_iterator polygon;
    00128   for(polygon = impactAreas.begin(); polygon != impactAreas.end(); ++polygon)
    00129   {
    00130     if((*polygon).pointInside(relPosition))
    00131     {
    00132       return true;
    00133     }
    00134   }
    00135   return false;
    00136 }
    00137
    00138
    00139
    00140 Actionfield::Actionfield(const std::string& name)
    00141 {
    00142   this->name = name;
    00143   futureWorldModelGenerator = FutureWorldModelGenerator::getFutureWorldModelGenerator();
    00144 }
    00145
    00146
    00147 Actionfield::~Actionfield()
    00148 {
    00149   actions.clear();
    00150   unsigned int numOfObjects(dynamicObjects.size());
    00151   for(unsigned int i=0; i<worldStateDepth; i++)
    00152   {
    00153     for(unsigned int j=0; j<numOfObjects; j++)
    00154     {
    00155       delete futureWorldStates[i][j];
    00156     }
    00157   }
    00158   futureWorldStates.clear();
    00159 }
    00160
    00161
    00162 void Actionfield::init()
    00163 {
    00164   // Add manipulated objects to object lists
    00165   std::vector < Action >::iterator action;
    00166   for (action = actions.begin(); action != actions.end(); ++action)
    00167   {
    00168     if(((*action).actionType == MOVE_OBJECT) || ((*action).actionType == MEASURE_OBJECT))
    00169     {
    00170       addManipulatedObject((*action));
    00171     }
    00172   }
    00173   // Allocate memory for future world states
    00174   if(actionfieldType == SINGLE_ACTION_FIELD)
    00175   {
    00176     worldStateDepth = 1;
    00177   }
    00178   else if(actionfieldType == FIXED_SEQUENCE_FIELD)
    00179   {
    00180     worldStateDepth = actions.size();
    00181   }
    00182
    00183   //std::vector<Object*> testStates(numObjects);
    00184
    00185   futureWorldStates.clear();
    00186   futureWorldStates.resize(worldStateDepth);
    00187   unsigned int numOfObjects(dynamicObjects.size());
    00188   for(unsigned int i=0; i<worldStateDepth; i++)
    00189   {
    00190     futureWorldStates[i].resize(numOfObjects);
    00191     for(unsigned int j=0; j<numOfObjects; j++)
    00192     {
    00193       futureWorldStates[i][j] = dynamicObjects[j]->getCopy();
    00194     }
    00195   }
    00196   futureRobotPoses.resize(worldStateDepth);
    00197 }
    00198
    00199
    00200 void Actionfield::addObject(PotentialFieldsObject* object)
    00201 {
    00202   Potentialfield::addObject(object);
    00203   if(object->isStatic())
    00204   {
    00205     staticObjects.push_back(object);
    00206   }
    00207   else
    00208   {
    00209     dynamicObjects.push_back(object);
    00210   }
    00211 }
    00212
    00213
    00214 void Actionfield::addManipulatedObject(Action& action)
    00215 {
    00216   for(unsigned int i=0; i<dynamicObjects.size(); i++)
    00217   {
    00218     if(dynamicObjects[i] == action.manipulatedObject)
    00219     {
    00220       action.objectIdx = i;
    00221       return;
    00222     }
    00223   }
    00224   action.objectIdx = dynamicObjects.size();
    00225   dynamicObjects.push_back(action.manipulatedObject);
    00226   objects.push_back(action.manipulatedObject);
    00227 }
    00228
    00229
    00230 void Actionfield::addAction(const Action& action)
    00231 {
    00232   actions.push_back(action);
    00233 }
    00234
    00235
    00236 void Actionfield::execute(const PfPose& pose, PotentialfieldResult& result)
    00237 {
    00238   result.value = 0.0;
    00239   result.motion.speed = 42;
    00240   result.actionPossible = false;
    00241   if((actionfieldType == SINGLE_ACTION_FIELD) || (actionfieldType == FIXED_SEQUENCE_FIELD))
    00242   {
    00243     unsigned int currentAction(0);
    00244     double time(0.0);
    00245     if(actions[currentAction].canBeApplied(pose, dynamicObjects))
    00246     {
    00247       result.actionPossible = true;
    00248       result.subAction = actions[0].name;
    00249       futureWorldModelGenerator->transformWorldState(
    00250                       pose, futureRobotPoses[0], actions[0], this,
    00251                       dynamicObjects, futureWorldStates[0], staticObjects);
    00252       time += actions[0].time;
    00253       currentAction++;
    00254       while(currentAction < actions.size())
    00255       {
    00256         if(actions[currentAction].canBeApplied(futureRobotPoses[currentAction-1],
    00257                                                futureWorldStates[currentAction-1]))
    00258         {
    00259           futureWorldModelGenerator->transformWorldState(
    00260             futureRobotPoses[currentAction-1], futureRobotPoses[currentAction],
    00261             actions[currentAction], this,
    00262             futureWorldStates[currentAction-1], futureWorldStates[currentAction],
    00263             staticObjects);
    00264           time += actions[currentAction].time;
    00265           currentAction++;
    00266         }
    00267         else
    00268         {
    00269           break;
    00270         }
    00271       }
    00272       if(criterion == CRITERION_CONST)
    00273       {
    00274         result.value = criterionParameter;
    00275       }
    00276       else
    00277       {
    00278         bool dummy;
    00279         result.value = computeActionValue(actions[currentAction-1], time, pose,
    00280                                           futureRobotPoses[currentAction-1],
    00281                                           dynamicObjects, futureWorldStates[currentAction-1], dummy);
    00282       }
    00283       result.motion = actions[0].motionParameters;
    00284     }
    00285   }
    00286   else //if(actionfieldType == BEST_SEQUENCE_FIELD)
    00287   {
    00288     double time(0.0);
    00289     std::vector<unsigned int> actionSequenceList;
    00290     for(unsigned int currentAction=0; currentAction<actions.size(); currentAction++)
    00291     {
    00292       if(actions[currentAction].canBeApplied(pose, dynamicObjects))
    00293       {
    00294         futureWorldModelGenerator->transformWorldState(
    00295             pose, futureRobotPoses[0], actions[currentAction], this,
    00296             dynamicObjects, futureWorldStates[0], staticObjects);
    00297         time = actions[currentAction].time;
    00298         bool pruningCheckPassed;
    00299         double value = computeActionValue(actions[currentAction], time, pose,
    00300                                           futureRobotPoses[0], dynamicObjects,
    00301                                           futureWorldStates[0], pruningCheckPassed);
    00302         if(!pruningCheckPassed)
    00303         {
    00304           continue;
    00305         }
    00306         if(!result.actionPossible)
    00307         {
    00308           result.actionPossible = true;
    00309           result.value = value;
    00310           result.motion = actions[currentAction].motionParameters;
    00311           result.subAction = actions[currentAction].name;
    00312         }
    00313         else if(value < result.value)
    00314         {
    00315           result.value = value;
    00316           result.motion = actions[currentAction].motionParameters;
    00317           result.subAction = actions[currentAction].name;
    00318         }
    00319         if(worldStateDepth > 1)
    00320         {
    00321           actionSequenceList.push_back(currentAction);
    00322           findBestSequence(1, pose, actionSequenceList, time, value, result);
    00323           actionSequenceList.pop_back();
    00324         }
    00325       }
    00326     }
    00327   }
    00328 }
    00329
    00330
    00331 void Actionfield::findBestSequence(unsigned int depth, const PfPose& robotPose,
    00332                                    std::vector<unsigned int>& actionSequenceList,
    00333                                    double time, double previousValue,
    00334                                    PotentialfieldResult& result)
    00335 {
    00336   double currentTime(0);
    00337   for(unsigned int currentAction=0; currentAction<actions.size(); currentAction++)
    00338   {
    00339     if(actions[currentAction].canBeApplied(futureRobotPoses[depth-1],
    00340                                            futureWorldStates[depth-1]))
    00341     {
    00342       futureWorldModelGenerator->transformWorldState(
    00343             futureRobotPoses[depth-1], futureRobotPoses[depth],
    00344             actions[currentAction], this, futureWorldStates[depth-1],
    00345             futureWorldStates[depth], staticObjects);
    00346       currentTime = actions[currentAction].time + time;
    00347       bool pruningCheckPassed;
    00348       double value = computeActionValue(actions[currentAction], currentTime,
    00349             robotPose, futureRobotPoses[depth], dynamicObjects,
    00350             futureWorldStates[depth], pruningCheckPassed);
    00351       if(!pruningCheckPassed)
    00352       {
    00353         continue;
    00354       }
    00355       if(value < result.value)
    00356       {
    00357         result.value = value;
    00358         result.motion = actions[currentAction].motionParameters;
    00359         result.subAction = actions[actionSequenceList[0]].name;
    00360       }
    00361       if(depth+1 < worldStateDepth)
    00362       {
    00363         actionSequenceList.push_back(currentAction);
    00364         findBestSequence(depth+1, robotPose, actionSequenceList,
    00365                          currentTime, value, result);
    00366         actionSequenceList.pop_back();
    00367       }
    00368     }
    00369   }
    00370 }
    00371
    00372
    00373 double Actionfield::computeValueAtPose(const PfPose& pose,
    00374                                        const std::vector<PotentialFieldsObject*>& dynamicObjects,
    00375                                        int excludedDynamicObject)
    00376 {
    00377   unsigned int skip;
    00378   if(excludedDynamicObject == -1)
    00379   {
    00380     skip = dynamicObjects.size();
    00381   }
    00382   else
    00383   {
    00384     skip = static_cast<unsigned int>(excludedDynamicObject);
    00385   }
    00386   double value(0.0);
    00387   std::vector < PotentialFieldsObject* >::const_iterator object;
    00388   for (object = staticObjects.begin(); object != staticObjects.end(); ++object)
    00389   {
    00390     value += (*object)->computeChargeAt(pose);
    00391   }
    00392   for (unsigned int i=0; i<dynamicObjects.size(); i++)
    00393   {
    00394     if(i != skip)
    00395     {
    00396       value += dynamicObjects[i]->computeChargeAt(pose);
    00397     }
    00398   }
    00399   return value;
    00400 }
    00401
    00402
    00403 PfVec Actionfield::getFutureFieldVecAt(const PfPose& pose,
    00404                                        const std::vector<PotentialFieldsObject*>& dynamicObjects,
    00405                                        int excludedDynamicObject)
    00406 {
    00407   unsigned int skip;
    00408   if(excludedDynamicObject == -1)
    00409   {
    00410     skip = dynamicObjects.size();
    00411   }
    00412   else
    00413   {
    00414     skip = static_cast<unsigned int>(excludedDynamicObject);
    00415   }
    00416   PfVec gradient(0.0,0.0);
    00417   std::vector < PotentialFieldsObject* >::const_iterator object;
    00418   for (object = staticObjects.begin(); object != staticObjects.end(); ++object)
    00419   {
    00420     gradient += (*object)->computeAbsFieldVecAt(pose);
    00421   }
    00422   for (unsigned int i=0; i<dynamicObjects.size(); i++)
    00423   {
    00424     if(i != skip)
    00425     {
    00426       gradient += dynamicObjects[i]->computeAbsFieldVecAt(pose);
    00427     }
    00428   }
    00429   return gradient;
    00430 }
    00431
    00432
    00433 double Actionfield::computeActionValue(const Action& action, double time,
    00434                                        const PfPose& ownPoseBefore,
    00435                                        const PfPose& ownPoseAfter,
    00436                                        const std::vector<PotentialFieldsObject*>& objectsBefore,
    00437                                        const std::vector<PotentialFieldsObject*>& objectsAfter,
    00438                                        bool& passedPruningCheck)
    00439 {
    00440   passedPruningCheck = true;
    00441   double valueAfterAction(0.0), valueBeforeAction(0.0), actionValue(0.0);
    00442   switch(action.actionType)
    00443   {
    00444   case MOVE_SELF:
    00445   case MEASURE_SELF: valueAfterAction = computeValueAtPose(ownPoseAfter, objectsAfter);
    00446     break;
    00447   case MOVE_OBJECT:
    00448   case MEASURE_OBJECT: valueAfterAction = computeValueAtPose(
    00449                          objectsAfter[action.objectIdx]->getPose(),
    00450                          objectsAfter, action.objectIdx);
    00451     break;
    00452   }
    00453   if((criterion != CRITERION_ABSOLUTE) || decreasingValuesOnly)
    00454   {
    00455     switch(action.actionType)
    00456     {
    00457     case MOVE_SELF:
    00458     case MEASURE_SELF: valueBeforeAction = computeValueAtPose(ownPoseAfter, objectsBefore);
    00459       break;
    00460     case MOVE_OBJECT:
    00461     case MEASURE_OBJECT: valueBeforeAction = computeValueAtPose(
    00462                            objectsBefore[action.objectIdx]->getPose(),
    00463                            objectsBefore, action.objectIdx);
    00464       break;
    00465     }
    00466   }
    00467   if(decreasingValuesOnly)
    00468   {
    00469     passedPruningCheck = (valueAfterAction < valueBeforeAction);
    00470   }
    00471   if(criterion == CRITERION_GAIN)
    00472   {
    00473     actionValue = (valueAfterAction - valueBeforeAction);
    00474   }
    00475   else if(criterion == CRITERION_ABSOLUTE)
    00476   {
    00477     actionValue = valueAfterAction;
    00478   }
    00479   else // criterion should be CRITERION_GRADIENT
    00480   {
    00481     double dist((objectsAfter[action.objectIdx]->getPose().pos -
    00482                  objectsBefore[action.objectIdx]->getPose().pos).length());
    00483     if(fabs(dist) < EPSILON)
    00484     {
    00485       return 0.0;
    00486     }
    00487     else
    00488     {
    00489       actionValue = ((valueBeforeAction - valueAfterAction) / dist);
    00490     }
    00491   }
    00492   if(considerTime)
    00493   {
    00494     if(time < EPSILON)
    00495     {
    00496       return actionValue;
    00497     }
    00498     else
    00499     {
    00500       return actionValue/time;
    00501     }
    00502   }
    00503   else
    00504   {
    00505     return actionValue;
    00506   }
    00507 }