Tools/PotentialFields/Actionfield.cpp

Go to the documentation of this file.

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

#include "Actionfield.h"
#include "FutureWorldModelGenerator.h"
#include "FieldObject.h"
#include "PotentialfieldComposition.h"
#include "FormationObject.h"



Action::Action()
{
  manipulatedObject = 0;
  name = "";
}


Action::~Action()
{
  impactAreas.clear();
  std::vector < PotentialfieldTransformation* >::iterator t;
  for(t = transformations.begin(); t != transformations.end(); ++t)
  {
    delete (*t);
  }
  transformations.clear();
}


Action::Action(const Action& a)
{
  name = a.name;
  manipulatedObject = a.manipulatedObject;
  joinAction = a.joinAction;
  actionType = a.actionType;
  impactAreas = a.impactAreas;
  std::vector < PotentialfieldTransformation* >::const_iterator t;
  for(t = a.transformations.begin(); t != a.transformations.end(); ++t)
  {
    transformations.push_back((*t)->copy());
  }
}


bool Action::canBeApplied(const PfPose& robotPose,
                          const std::vector<PotentialFieldsObject*>& objects)
{
  if((actionType == MOVE_SELF) || (actionType == MEASURE_SELF))
  {
    return true;
  }
  if(actionType == MEASURE_OBJECT)
  {
    return objects[objectIdx]->isActive();
  }
  else //(actionType == MOVE_OBJECT)
  {
    if(!(objects[objectIdx]->isActive()))
    {
      return false;
    }
    unsigned int i,j;
    PfPose manipulatedObjectPose(objects[objectIdx]->getPose());
    PfPose objPose, robPose;
    if(manipulatedObjectPose.hasProbabilityDistribution)
    {
      if(robotPose.hasProbabilityDistribution)
      {
        for(j=0; j<robotPose.probabilityDistribution.size(); j++)
        {
          for(i=0; i<manipulatedObjectPose.probabilityDistribution.size(); i++)
          {
            if(poseInsideImpactArea(robotPose.probabilityDistribution[j], 
                                    manipulatedObjectPose.probabilityDistribution[i]))
            {
              return true;
            }
          }
        }
      }
      else
      {
        for(i=0; i<manipulatedObjectPose.probabilityDistribution.size(); i++)
        {
          if(poseInsideImpactArea(robotPose, 
                                  manipulatedObjectPose.probabilityDistribution[i]))
          {
            return true;
          }
        }
      }
    }
    else
    {
      if(robotPose.hasProbabilityDistribution)
      {
        for(i=0; i<robotPose.probabilityDistribution.size(); i++)
        {
          if(poseInsideImpactArea(robotPose.probabilityDistribution[i], 
                                  manipulatedObjectPose))
          {
            return true;
          }
        }
      }
      else
      {
        return poseInsideImpactArea(robotPose, manipulatedObjectPose);
      }
    }
  }
  return false;
}


inline bool Action::poseInsideImpactArea(const PfPose& robPose, const PfPose& objPose)
{
  PfVec relPosition(objPose.pos);
  relPosition -= robPose.pos;
  relPosition.rotate(-robPose.rotation);
  std::vector< Polygon >::const_iterator polygon;
  for(polygon = impactAreas.begin(); polygon != impactAreas.end(); ++polygon)
  {
    if((*polygon).pointInside(relPosition))
    {  
      return true;
    }
  }
  return false;
}



Actionfield::Actionfield(const std::string& name)
{
  this->name = name;
  futureWorldModelGenerator = FutureWorldModelGenerator::getFutureWorldModelGenerator();
}


Actionfield::~Actionfield()
{
  actions.clear();
  unsigned int numOfObjects(dynamicObjects.size());
  for(unsigned int i=0; i<worldStateDepth; i++)
  {
    for(unsigned int j=0; j<numOfObjects; j++)
    {
      delete futureWorldStates[i][j];      
    }
  }
  futureWorldStates.clear();
}


void Actionfield::init()
{
  // Add manipulated objects to object lists
  std::vector < Action >::iterator action;
  for (action = actions.begin(); action != actions.end(); ++action)
  {
    if(((*action).actionType == MOVE_OBJECT) || ((*action).actionType == MEASURE_OBJECT))
    {
      addManipulatedObject((*action));
    }
  } 
  // Allocate memory for future world states
  if(actionfieldType == SINGLE_ACTION_FIELD)
  {
    worldStateDepth = 1;
  }
  else if(actionfieldType == FIXED_SEQUENCE_FIELD)
  {
    worldStateDepth = actions.size();
  }

  //std::vector<Object*> testStates(numObjects);

  futureWorldStates.clear();
  futureWorldStates.resize(worldStateDepth);
  unsigned int numOfObjects(dynamicObjects.size());
  for(unsigned int i=0; i<worldStateDepth; i++)
  {
    futureWorldStates[i].resize(numOfObjects);
    for(unsigned int j=0; j<numOfObjects; j++)
    {
      futureWorldStates[i][j] = dynamicObjects[j]->getCopy();
    }
  }
  futureRobotPoses.resize(worldStateDepth);
}


void Actionfield::addObject(PotentialFieldsObject* object)
{
  Potentialfield::addObject(object);
  if(object->isStatic())
  {
    staticObjects.push_back(object);
  }
  else
  {
    dynamicObjects.push_back(object);
  }
}


void Actionfield::addManipulatedObject(Action& action)
{
  for(unsigned int i=0; i<dynamicObjects.size(); i++)
  {
    if(dynamicObjects[i] == action.manipulatedObject)
    {
      action.objectIdx = i;
      return;
    }
  }
  action.objectIdx = dynamicObjects.size();
  dynamicObjects.push_back(action.manipulatedObject);
  objects.push_back(action.manipulatedObject);
}


void Actionfield::addAction(const Action& action)
{ 
  actions.push_back(action); 
}


void Actionfield::execute(const PfPose& pose, PotentialfieldResult& result)
{
  result.value = 0.0;
  result.motion.speed = 42;
  result.actionPossible = false;
  if((actionfieldType == SINGLE_ACTION_FIELD) || (actionfieldType == FIXED_SEQUENCE_FIELD))
  {
    unsigned int currentAction(0);
    double time(0.0);
    if(actions[currentAction].canBeApplied(pose, dynamicObjects))
    {
      result.actionPossible = true;
      result.subAction = actions[0].name;
      futureWorldModelGenerator->transformWorldState(
                      pose, futureRobotPoses[0], actions[0], this, 
                      dynamicObjects, futureWorldStates[0], staticObjects);
      time += actions[0].time;
      currentAction++;
      while(currentAction < actions.size())
      {
        if(actions[currentAction].canBeApplied(futureRobotPoses[currentAction-1],
                                               futureWorldStates[currentAction-1]))
        {
          futureWorldModelGenerator->transformWorldState(
            futureRobotPoses[currentAction-1], futureRobotPoses[currentAction], 
            actions[currentAction], this, 
            futureWorldStates[currentAction-1], futureWorldStates[currentAction], 
            staticObjects);
          time += actions[currentAction].time;
          currentAction++;
        }
        else
        {
          break;
        }
      }
      if(criterion == CRITERION_CONST)
      {
        result.value = criterionParameter;
      }
      else
      {
        bool dummy;
        result.value = computeActionValue(actions[currentAction-1], time, pose, 
                                          futureRobotPoses[currentAction-1], 
                                          dynamicObjects, futureWorldStates[currentAction-1], dummy);
      }
      result.motion = actions[0].motionParameters;
    }
  }
  else //if(actionfieldType == BEST_SEQUENCE_FIELD)
  {
    double time(0.0);
    std::vector<unsigned int> actionSequenceList;
    for(unsigned int currentAction=0; currentAction<actions.size(); currentAction++)
    {
      if(actions[currentAction].canBeApplied(pose, dynamicObjects))
      {
        futureWorldModelGenerator->transformWorldState(
            pose, futureRobotPoses[0], actions[currentAction], this, 
            dynamicObjects, futureWorldStates[0], staticObjects);
        time = actions[currentAction].time;
        bool pruningCheckPassed;
        double value = computeActionValue(actions[currentAction], time, pose, 
                                          futureRobotPoses[0], dynamicObjects, 
                                          futureWorldStates[0], pruningCheckPassed);
        if(!pruningCheckPassed)
        {
          continue;
        }
        if(!result.actionPossible)
        {
          result.actionPossible = true;
          result.value = value;
          result.motion = actions[currentAction].motionParameters;
          result.subAction = actions[currentAction].name;
        }
        else if(value < result.value)
        {
          result.value = value;
          result.motion = actions[currentAction].motionParameters;
          result.subAction = actions[currentAction].name;
        }
        if(worldStateDepth > 1)
        {
          actionSequenceList.push_back(currentAction);
          findBestSequence(1, pose, actionSequenceList, time, value, result);
          actionSequenceList.pop_back();
        }
      }
    }
  }
}


void Actionfield::findBestSequence(unsigned int depth, const PfPose& robotPose,
                                   std::vector<unsigned int>& actionSequenceList, 
                                   double time, double previousValue,
                                   PotentialfieldResult& result)
{
  double currentTime(0);
  for(unsigned int currentAction=0; currentAction<actions.size(); currentAction++)
  {
    if(actions[currentAction].canBeApplied(futureRobotPoses[depth-1], 
                                           futureWorldStates[depth-1]))
    {
      futureWorldModelGenerator->transformWorldState(
            futureRobotPoses[depth-1], futureRobotPoses[depth], 
            actions[currentAction], this, futureWorldStates[depth-1], 
            futureWorldStates[depth], staticObjects);
      currentTime = actions[currentAction].time + time;
      bool pruningCheckPassed;
      double value = computeActionValue(actions[currentAction], currentTime, 
            robotPose, futureRobotPoses[depth], dynamicObjects, 
            futureWorldStates[depth], pruningCheckPassed);
      if(!pruningCheckPassed)
      {
        continue;
      }
      if(value < result.value)
      {
        result.value = value;
        result.motion = actions[currentAction].motionParameters;
        result.subAction = actions[actionSequenceList[0]].name;
      }
      if(depth+1 < worldStateDepth)
      {
        actionSequenceList.push_back(currentAction);
        findBestSequence(depth+1, robotPose, actionSequenceList, 
                         currentTime, value, result);
        actionSequenceList.pop_back();
      }
    }
  }
}


double Actionfield::computeValueAtPose(const PfPose& pose, 
                                       const std::vector<PotentialFieldsObject*>& dynamicObjects,
                                       int excludedDynamicObject)
{
  unsigned int skip;
  if(excludedDynamicObject == -1)
  {
    skip = dynamicObjects.size();
  }
  else
  {
    skip = static_cast<unsigned int>(excludedDynamicObject);
  }
  double value(0.0);
  std::vector < PotentialFieldsObject* >::const_iterator object;
  for (object = staticObjects.begin(); object != staticObjects.end(); ++object)
  {
    value += (*object)->computeChargeAt(pose);
  }
  for (unsigned int i=0; i<dynamicObjects.size(); i++)
  {
    if(i != skip)
    {
      value += dynamicObjects[i]->computeChargeAt(pose);
    }
  }
  return value;
}


PfVec Actionfield::getFutureFieldVecAt(const PfPose& pose, 
                                       const std::vector<PotentialFieldsObject*>& dynamicObjects,
                                       int excludedDynamicObject)
{
  unsigned int skip;
  if(excludedDynamicObject == -1)
  {
    skip = dynamicObjects.size();
  }
  else
  {
    skip = static_cast<unsigned int>(excludedDynamicObject);
  }
  PfVec gradient(0.0,0.0);
  std::vector < PotentialFieldsObject* >::const_iterator object;
  for (object = staticObjects.begin(); object != staticObjects.end(); ++object)
  {
    gradient += (*object)->computeAbsFieldVecAt(pose);
  }
  for (unsigned int i=0; i<dynamicObjects.size(); i++)
  {
    if(i != skip)
    {
      gradient += dynamicObjects[i]->computeAbsFieldVecAt(pose);
    }
  }
  return gradient;
}


double Actionfield::computeActionValue(const Action& action, double time,
                                       const PfPose& ownPoseBefore,
                                       const PfPose& ownPoseAfter,
                                       const std::vector<PotentialFieldsObject*>& objectsBefore,
                                       const std::vector<PotentialFieldsObject*>& objectsAfter,
                                       bool& passedPruningCheck)
{
  passedPruningCheck = true;
  double valueAfterAction(0.0), valueBeforeAction(0.0), actionValue(0.0);
  switch(action.actionType)
  {
  case MOVE_SELF: 
  case MEASURE_SELF: valueAfterAction = computeValueAtPose(ownPoseAfter, objectsAfter); 
    break;
  case MOVE_OBJECT: 
  case MEASURE_OBJECT: valueAfterAction = computeValueAtPose(
                         objectsAfter[action.objectIdx]->getPose(),
                         objectsAfter, action.objectIdx); 
    break;
  }
  if((criterion != CRITERION_ABSOLUTE) || decreasingValuesOnly)
  {
    switch(action.actionType)
    {
    case MOVE_SELF: 
    case MEASURE_SELF: valueBeforeAction = computeValueAtPose(ownPoseAfter, objectsBefore); 
      break;
    case MOVE_OBJECT: 
    case MEASURE_OBJECT: valueBeforeAction = computeValueAtPose(
                           objectsBefore[action.objectIdx]->getPose(),
                           objectsBefore, action.objectIdx); 
      break;
    }
  }
  if(decreasingValuesOnly)
  {
    passedPruningCheck = (valueAfterAction < valueBeforeAction);
  }
  if(criterion == CRITERION_GAIN)
  {
    actionValue = (valueAfterAction - valueBeforeAction);
  }
  else if(criterion == CRITERION_ABSOLUTE)
  {
    actionValue = valueAfterAction;
  }
  else // criterion should be CRITERION_GRADIENT
  {
    double dist((objectsAfter[action.objectIdx]->getPose().pos -
                 objectsBefore[action.objectIdx]->getPose().pos).length());
    if(fabs(dist) < EPSILON)
    {
      return 0.0;
    }
    else
    {
      actionValue = ((valueBeforeAction - valueAfterAction) / dist);
    }
  }
  if(considerTime)
  {
    if(time < EPSILON)
    {
      return actionValue;
    }
    else
    {
      return actionValue/time;
    }
  }
  else
  {
    return actionValue;
  }
}

---