Tools/PotentialFields/FutureWorldModelGenerator.cpp

Go to the documentation of this file.

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

#include "PfieldConfig.h"
#include "FutureWorldModelGenerator.h"
#include "Pfield.h"
#include "Actionfield.h"
#include "PfieldDatatypes.h"
#include "FieldObject.h"
#include "PfieldGeometry.h"



void FutureWorldModelGenerator::transformWorldState(
                         const PfPose& poseNow, PfPose& poseThen, 
                         Action& action, Actionfield* callingField,
                         std::vector<PotentialFieldsObject*>& worldStateNow, 
                         std::vector<PotentialFieldsObject*>& worldStateThen,
                         std::vector<PotentialFieldsObject*>& worldStateStatic)
{
  for(unsigned int i=0; i<worldStateThen.size(); i++)
  {
    worldStateThen[i]->getMinimalCopyFrom(worldStateNow[i]);
  }
  poseThen = poseNow;
  if((action.actionType == MEASURE_OBJECT) || (action.actionType == MEASURE_SELF))
  {
    return;
  }
  if(action.transformations.size() == 1) 
  {
    executeSingleTransformation(poseThen, action, action.transformations[0],
                                callingField, worldStateThen, worldStateStatic);
    action.time = action.transformations[0]->time;
  }
  else
  {
    PfPose newRobotPose, newObjectPose, objectPose;
    newRobotPose.hasProbabilityDistribution = true;
    action.time = 0;
    if(action.actionType == MOVE_OBJECT) 
    {
      newObjectPose.hasProbabilityDistribution = true;
      objectPose = worldStateThen[action.objectIdx]->getPose();
    }
    for(unsigned int j=0; j<action.transformations.size(); j++)
    {
      PfPose tempRobotPose(poseThen);
      if(action.actionType == MOVE_OBJECT) 
      {
        worldStateThen[action.objectIdx]->setPose(objectPose);
      }
      executeSingleTransformation(tempRobotPose, action, action.transformations[j],
                                callingField, worldStateThen, worldStateStatic);
      if(tempRobotPose.hasProbabilityDistribution)
      {
        for(unsigned int d=0; d<tempRobotPose.probabilityDistribution.size(); d++)
        {
          newRobotPose.probabilityDistribution.push_back(tempRobotPose.probabilityDistribution[d]);
          newRobotPose.probabilityDistribution[newRobotPose.probabilityDistribution.size()-1].probability 
          = action.transformations[j]->probability*tempRobotPose.probabilityDistribution[d].probability;
        }
      }
      else
      {
        newRobotPose.probabilityDistribution.push_back(tempRobotPose);
        newRobotPose.probabilityDistribution[newRobotPose.probabilityDistribution.size()-1].probability 
          = action.transformations[j]->probability;
      }
      if(action.actionType == MOVE_OBJECT)
      {
        PfPose tempObjectPose(worldStateThen[action.objectIdx]->getPose());
        if(tempObjectPose.hasProbabilityDistribution)
        {
          for(unsigned int r=0; r<tempObjectPose.probabilityDistribution.size(); r++)
          {
            newObjectPose.probabilityDistribution.push_back(tempObjectPose.probabilityDistribution[r]);
            newObjectPose.probabilityDistribution[newObjectPose.probabilityDistribution.size()-1].probability 
            = action.transformations[j]->probability*tempObjectPose.probabilityDistribution[r].probability;
          }
        }
        else
        {
          newObjectPose.probabilityDistribution.push_back(tempObjectPose);
          newObjectPose.probabilityDistribution[newObjectPose.probabilityDistribution.size()-1].probability 
            = action.transformations[j]->probability;
        }
      }
      action.time += action.transformations[j]->time*action.transformations[j]->probability;
    }
    if((action.actionType == MOVE_SELF) || (action.joinAction))
    {
      newRobotPose.setPoseFromSamples();
      poseThen = newRobotPose;
    }
    if(action.actionType == MOVE_OBJECT)
    {
      newObjectPose.setPoseFromSamples();
      worldStateThen[action.objectIdx]->setPose(newObjectPose);
    }
  }
}


void FutureWorldModelGenerator::executeSingleTransformation(
                         PfPose& pose, Action& action, 
                         PotentialfieldTransformation* transformation,
                         Actionfield* callingField,
                         std::vector<PotentialFieldsObject*>& dynamicWorldState,
                         std::vector<PotentialFieldsObject*>& staticWorldState)
{
  TransformationType transformationType = transformation->getType();
  if(transformationType == TRANSLATION)
  {
    PfVec transVec(0.0,0.0);
    Translation* translation = (Translation*)transformation;
    if(translation->alongGradient)
    {
      PfVec dirVec;
      PfPose startPose;
      if(action.actionType==MOVE_OBJECT)
      {
        dirVec = (callingField->getFutureFieldVecAt(
                          dynamicWorldState[action.objectIdx]->getPose(),
                          dynamicWorldState, action.objectIdx));
        startPose = dynamicWorldState[action.objectIdx]->getPose();
      }
      else //(action.actionType==MOVE_SELF)
      {
        dirVec = callingField->getFutureFieldVecAt(pose, dynamicWorldState);
        startPose = pose;
      }
      dirVec.normalize();
      dirVec *= translation->stepLength;
      PfPose alongPose(startPose);
      alongPose.addAbsVector(dirVec);
      double startAngle(alongPose.pos.getAngle());
      while((fabs(dirVec.getAngle()-startAngle) < translation->maxGradientDeviation)
            && (startPose.pos.distanceTo(alongPose.pos) < translation->maxLength))
      {
        dirVec = callingField->getFutureFieldVecAt(alongPose, dynamicWorldState);
        dirVec.normalize();
        dirVec *= translation->stepLength;
        transVec += dirVec;
        alongPose.addAbsVector(dirVec);
      }
      transVec -= dirVec;
      translation->time = (transVec.length() / translation->speed);
      action.motionParameters.pos = transVec;
      action.motionParameters.pos.rotate(-pose.rotation);
    }
    else if(translation->toObject != 0)
    {
      PfPose startPose;
      if(action.actionType==MOVE_OBJECT)
      {
        startPose = dynamicWorldState[action.objectIdx]->getPose();
      }
      else //(action.actionType==MOVE_SELF)
      {
        startPose = pose;
      }
      transVec = (translation->toObject->getPose().pos - startPose.pos);
      translation->time = (transVec.length() / translation->speed);
      action.motionParameters.pos = transVec;
      action.motionParameters.pos.rotate(-pose.rotation);
      action.motionParameters.rotation = 0.0;
    }
    else
    {
      transVec = translation->translation;
      transVec.rotate(pose.rotation);
    }
    PfVec standardTransVec(transVec);
    if(action.actionType == MOVE_OBJECT)
    {
      PfPose objPose(dynamicWorldState[action.objectIdx]->getPose());
      transVec *= getMaxTranslationForObject(objPose, transVec, 
                                             dynamicWorldState, staticWorldState, action.objectIdx);
      objPose.pos += transVec;
      if(objPose.hasProbabilityDistribution)
      {
        PfVec probVec;
        for(unsigned int i=0; i<objPose.probabilityDistribution.size(); i++)
        {
          if(action.canBeApplied(objPose.probabilityDistribution[i], dynamicWorldState))
          {
            probVec = standardTransVec;
            probVec *= getMaxTranslationForObject
                             (objPose.probabilityDistribution[i], probVec, 
                              dynamicWorldState, staticWorldState, action.objectIdx);
            objPose.probabilityDistribution[i].pos += probVec;
          }
        }
      }
      dynamicWorldState[action.objectIdx]->setPose(objPose);
      if(action.joinAction)
      {
        pose.addAbsVector(transVec);
      }
    }
    else //(action.actionType == MOVE_SELF)
    {
      transVec *= getMaxTranslationForObject(pose, transVec, dynamicWorldState, staticWorldState);
      pose.pos += transVec;
      if(pose.hasProbabilityDistribution)
      {
        for(unsigned int i=0; i<pose.probabilityDistribution.size(); i++)
        {
          transVec = standardTransVec;
          transVec *= getMaxTranslationForObject
                           (pose.probabilityDistribution[i], transVec, 
                            dynamicWorldState, staticWorldState);
          pose.probabilityDistribution[i].pos += transVec;
        }
      }
    }
  }
  else if(transformationType == ROTATION)
  {
    double rot(0.0);
    Rotation* rotation = (Rotation*)transformation;
    if(rotation->toGradient)
    {
      PfVec gradientVec;
      if(action.actionType==MOVE_OBJECT)
      {
        gradientVec = (callingField->getFutureFieldVecAt(
                          dynamicWorldState[action.objectIdx]->getPose(),
                          dynamicWorldState, action.objectIdx));
      }
      else //(action.actionType==MOVE_SELF)
      {
        gradientVec = callingField->getFutureFieldVecAt(pose, dynamicWorldState);
      }
      if(gradientVec.length() != 0.0)
      {
        gradientVec.normalize();
        gradientVec.rotate(-pose.rotation);
        rot = atan2(gradientVec.y,gradientVec.x);
      }
      rotation->time = (rot / rotation->speed);
      action.motionParameters.rotation = rot;
    }
    else if(rotation->toObject != 0)
    {
      PfPose startPose;
      if(action.actionType==MOVE_OBJECT)
      {
        startPose = dynamicWorldState[action.objectIdx]->getPose();
      }
      else //(action.actionType==MOVE_SELF)
      {
        startPose = pose;
      }
      PfVec vecToObject(rotation->toObject->getPose().pos - startPose.pos);
      if(vecToObject.length() != 0.0)
      {
        vecToObject.normalize();
        vecToObject.rotate(-pose.rotation);
        rot = atan2(vecToObject.y,vecToObject.x);
      }
      rotation->time = (rot / rotation->speed);
      action.motionParameters.rotation = rot;
    }
    else
    {
      rot = rotation->angle;
    }
    double standardRotation(rot);
    if(action.actionType == MOVE_OBJECT)
    {
      PfPose objPose(dynamicWorldState[action.objectIdx]->getPose());
      rot *= getMaxRotationForObject(objPose, pose, rot, 
                                     dynamicWorldState, staticWorldState, action.objectIdx);
      objPose.rotateAround(pose.pos, rot);
      if(objPose.hasProbabilityDistribution)
      {
        double probRot;
        for(unsigned int i=0; i<objPose.probabilityDistribution.size(); i++)
        {
          if(action.canBeApplied(objPose.probabilityDistribution[i], dynamicWorldState))
          {
            probRot = standardRotation;
            probRot *= getMaxRotationForObject
                             (objPose.probabilityDistribution[i], pose, probRot, 
                              dynamicWorldState, staticWorldState, action.objectIdx);
            objPose.probabilityDistribution[i].rotateAround(pose.pos, probRot);
          }
        }
      }
      dynamicWorldState[action.objectIdx]->setPose(objPose);
      if(action.joinAction)
      {
        pose.rotation += rot;
        pose.normRotation();
      }
    }
    else //(action.actionType == MOVE_SELF)
    {
      rot *= getMaxRotationForObject(pose, pose, rot, dynamicWorldState, staticWorldState);
      pose.rotation += rot;
      pose.normRotation();
      if(pose.hasProbabilityDistribution)
      {
        for(unsigned int i=0; i<pose.probabilityDistribution.size(); i++)
        {
          rot = standardRotation;
          rot *= getMaxRotationForObject
                           (pose.probabilityDistribution[i], pose, rot, 
                            dynamicWorldState, staticWorldState);
          pose.probabilityDistribution[i].rotation += rot;
          pose.probabilityDistribution[i].normRotation();
        }
      }
    }
  }
}


double FutureWorldModelGenerator::getMaxTranslationForObject(
                                  const PfPose& objectPose,
                                  const PfVec& translation,
                                  std::vector<PotentialFieldsObject*>& otherObjects,
                                  std::vector<PotentialFieldsObject*>& otherStaticObjects,
                                  int excludedObject)
{
  Line wayOfObject;
  wayOfObject.p1 = objectPose.pos;
  wayOfObject.p2 = (wayOfObject.p1 + translation);
  PfVec halfTrans(translation * 0.5);
  wayOfObject.position = (wayOfObject.p1 + halfTrans);
  wayOfObject.radiusOfCollisionCircle = halfTrans.length();
  std::vector<PfVec> intersectionPoints;
  unsigned int skip;
  if(excludedObject == -1)
  {
    skip = otherObjects.size();
  }
  else
  {
    skip = static_cast<unsigned int>(excludedObject);
  }
  for(unsigned int i = 0; i<otherObjects.size(); i++)
  {
    if((otherObjects[i]->isActive()) && (i != skip))
    {
      PfieldGeometricObject* geoObj = otherObjects[i]->getAbsGeometry();
      intersectGeometricObjects(&wayOfObject, geoObj, intersectionPoints);
    }
  }
  for(unsigned int j = 0; j<otherStaticObjects.size(); j++)
  {
    PfieldGeometricObject* geoObj = otherStaticObjects[j]->getAbsGeometry();
    intersectGeometricObjects(&wayOfObject, geoObj, intersectionPoints);
  }
  if(intersectionPoints.size()>0)
  {
    //Find nearest point
    PfVec nearestPoint(intersectionPoints[0]);
    double dist = (nearestPoint - wayOfObject.p1).length();
    for(unsigned int j = 1; j<intersectionPoints.size(); j++)
    {
      PfVec testPoint(intersectionPoints[j]);
      double distToTest = (testPoint - wayOfObject.p1).length();
      if(distToTest < dist)
      {
        dist = distToTest;
        nearestPoint = testPoint;
      }
    }
    double transLength = translation.length();
    if(dist >= transLength)
    {
      return 1.0;
    }
    else
    {
      return (dist/transLength);
    }
  }
  else
  {
    return 1.0;
  }
}


double FutureWorldModelGenerator::getMaxRotationForObject(
                                 const PfPose& objectPose,
                                 const PfPose& rotationPose,
                                 double rotation,
                                 std::vector<PotentialFieldsObject*>& otherObjects,
                                 std::vector<PotentialFieldsObject*>& otherStaticObjects,
                                 int excludedObject)
{
  double radius((objectPose.pos-rotationPose.pos).length());
  if(radius < EPSILON)
  {
    return 1.0;
  }
  Circle rotationCircle;
  rotationCircle.position = rotationPose.pos;
  rotationCircle.radius = radius;
  rotationCircle.initRadiusOfCollisionCircle();
  std::vector<PfVec> intersectionPoints;
  unsigned int skip;
  if(excludedObject == -1)
  {
    skip = otherObjects.size();
  }
  else
  {
    skip = static_cast<unsigned int>(excludedObject);
  }
  for(unsigned int i = 0; i<otherObjects.size(); i++)
  {
    if((otherObjects[i]->isActive()) && (i != skip))
    {
      PfieldGeometricObject* geoObj = otherObjects[i]->getAbsGeometry();
      intersectGeometricObjects(&rotationCircle, geoObj, intersectionPoints);
    }
  }
  for(unsigned int j = 0; j<otherStaticObjects.size(); j++)
  {
    PfieldGeometricObject* geoObj = otherStaticObjects[j]->getAbsGeometry();
    intersectGeometricObjects(&rotationCircle, geoObj, intersectionPoints);
  }
  if(intersectionPoints.size()>0)
  {
    PfPose sectorBase(rotationPose);
    sectorBase.rotation += rotation/2.0;
    sectorBase.normRotation();
    Sector sector;
    sector.openingAngle = rotation;
    std::vector< PfVec >::const_iterator point;
    double nearestAngle = fabs(rotation);
    for(point = intersectionPoints.begin(); 
        point != intersectionPoints.end(); ++point)
    {
       if(sector.pointInside(sectorBase, (*point)))
       {
         double angleToPoint(fabs(rotationPose.getAngleTo((*point))));
         if(angleToPoint < nearestAngle)
         {
           nearestAngle = angleToPoint;
         }
       }
    }
    return fabs(nearestAngle/rotation);
  }
  else
  {
    return 1.0;
  }
}

---