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

# Tools/PotentialFields/FutureWorldModelGenerator.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file FutureWorldModelGenerator.cpp
    00003 *
    00004 * Implementation of class FutureWorldModelGenerator
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009 #include "PfieldConfig.h"
    00010 #include "FutureWorldModelGenerator.h"
    00011 #include "Pfield.h"
    00012 #include "Actionfield.h"
    00013 #include "PfieldDatatypes.h"
    00014 #include "FieldObject.h"
    00015 #include "PfieldGeometry.h"
    00016
    00017
    00018
    00019 void FutureWorldModelGenerator::transformWorldState(
    00020                          const PfPose& poseNow, PfPose& poseThen,
    00021                          Action& action, Actionfield* callingField,
    00022                          std::vector<PotentialFieldsObject*>& worldStateNow,
    00023                          std::vector<PotentialFieldsObject*>& worldStateThen,
    00024                          std::vector<PotentialFieldsObject*>& worldStateStatic)
    00025 {
    00026   for(unsigned int i=0; i<worldStateThen.size(); i++)
    00027   {
    00028     worldStateThen[i]->getMinimalCopyFrom(worldStateNow[i]);
    00029   }
    00030   poseThen = poseNow;
    00031   if((action.actionType == MEASURE_OBJECT) || (action.actionType == MEASURE_SELF))
    00032   {
    00033     return;
    00034   }
    00035   if(action.transformations.size() == 1)
    00036   {
    00037     executeSingleTransformation(poseThen, action, action.transformations[0],
    00038                                 callingField, worldStateThen, worldStateStatic);
    00039     action.time = action.transformations[0]->time;
    00040   }
    00041   else
    00042   {
    00043     PfPose newRobotPose, newObjectPose, objectPose;
    00044     newRobotPose.hasProbabilityDistribution = true;
    00045     action.time = 0;
    00046     if(action.actionType == MOVE_OBJECT)
    00047     {
    00048       newObjectPose.hasProbabilityDistribution = true;
    00049       objectPose = worldStateThen[action.objectIdx]->getPose();
    00050     }
    00051     for(unsigned int j=0; j<action.transformations.size(); j++)
    00052     {
    00053       PfPose tempRobotPose(poseThen);
    00054       if(action.actionType == MOVE_OBJECT)
    00055       {
    00056         worldStateThen[action.objectIdx]->setPose(objectPose);
    00057       }
    00058       executeSingleTransformation(tempRobotPose, action, action.transformations[j],
    00059                                 callingField, worldStateThen, worldStateStatic);
    00060       if(tempRobotPose.hasProbabilityDistribution)
    00061       {
    00062         for(unsigned int d=0; d<tempRobotPose.probabilityDistribution.size(); d++)
    00063         {
    00064           newRobotPose.probabilityDistribution.push_back(tempRobotPose.probabilityDistribution[d]);
    00065           newRobotPose.probabilityDistribution[newRobotPose.probabilityDistribution.size()-1].probability
    00066           = action.transformations[j]->probability*tempRobotPose.probabilityDistribution[d].probability;
    00067         }
    00068       }
    00069       else
    00070       {
    00071         newRobotPose.probabilityDistribution.push_back(tempRobotPose);
    00072         newRobotPose.probabilityDistribution[newRobotPose.probabilityDistribution.size()-1].probability
    00073           = action.transformations[j]->probability;
    00074       }
    00075       if(action.actionType == MOVE_OBJECT)
    00076       {
    00077         PfPose tempObjectPose(worldStateThen[action.objectIdx]->getPose());
    00078         if(tempObjectPose.hasProbabilityDistribution)
    00079         {
    00080           for(unsigned int r=0; r<tempObjectPose.probabilityDistribution.size(); r++)
    00081           {
    00082             newObjectPose.probabilityDistribution.push_back(tempObjectPose.probabilityDistribution[r]);
    00083             newObjectPose.probabilityDistribution[newObjectPose.probabilityDistribution.size()-1].probability
    00084             = action.transformations[j]->probability*tempObjectPose.probabilityDistribution[r].probability;
    00085           }
    00086         }
    00087         else
    00088         {
    00089           newObjectPose.probabilityDistribution.push_back(tempObjectPose);
    00090           newObjectPose.probabilityDistribution[newObjectPose.probabilityDistribution.size()-1].probability
    00091             = action.transformations[j]->probability;
    00092         }
    00093       }
    00094       action.time += action.transformations[j]->time*action.transformations[j]->probability;
    00095     }
    00096     if((action.actionType == MOVE_SELF) || (action.joinAction))
    00097     {
    00098       newRobotPose.setPoseFromSamples();
    00099       poseThen = newRobotPose;
    00100     }
    00101     if(action.actionType == MOVE_OBJECT)
    00102     {
    00103       newObjectPose.setPoseFromSamples();
    00104       worldStateThen[action.objectIdx]->setPose(newObjectPose);
    00105     }
    00106   }
    00107 }
    00108
    00109
    00110 void FutureWorldModelGenerator::executeSingleTransformation(
    00111                          PfPose& pose, Action& action,
    00112                          PotentialfieldTransformation* transformation,
    00113                          Actionfield* callingField,
    00114                          std::vector<PotentialFieldsObject*>& dynamicWorldState,
    00115                          std::vector<PotentialFieldsObject*>& staticWorldState)
    00116 {
    00117   TransformationType transformationType = transformation->getType();
    00118   if(transformationType == TRANSLATION)
    00119   {
    00120     PfVec transVec(0.0,0.0);
    00121     Translation* translation = (Translation*)transformation;
    00122     if(translation->alongGradient)
    00123     {
    00124       PfVec dirVec;
    00125       PfPose startPose;
    00126       if(action.actionType==MOVE_OBJECT)
    00127       {
    00128         dirVec = (callingField->getFutureFieldVecAt(
    00129                           dynamicWorldState[action.objectIdx]->getPose(),
    00130                           dynamicWorldState, action.objectIdx));
    00131         startPose = dynamicWorldState[action.objectIdx]->getPose();
    00132       }
    00133       else //(action.actionType==MOVE_SELF)
    00134       {
    00135         dirVec = callingField->getFutureFieldVecAt(pose, dynamicWorldState);
    00136         startPose = pose;
    00137       }
    00138       dirVec.normalize();
    00139       dirVec *= translation->stepLength;
    00140       PfPose alongPose(startPose);
    00141       alongPose.addAbsVector(dirVec);
    00142       double startAngle(alongPose.pos.getAngle());
    00143       while((fabs(dirVec.getAngle()-startAngle) < translation->maxGradientDeviation)
    00144             && (startPose.pos.distanceTo(alongPose.pos) < translation->maxLength))
    00145       {
    00146         dirVec = callingField->getFutureFieldVecAt(alongPose, dynamicWorldState);
    00147         dirVec.normalize();
    00148         dirVec *= translation->stepLength;
    00149         transVec += dirVec;
    00150         alongPose.addAbsVector(dirVec);
    00151       }
    00152       transVec -= dirVec;
    00153       translation->time = (transVec.length() / translation->speed);
    00154       action.motionParameters.pos = transVec;
    00155       action.motionParameters.pos.rotate(-pose.rotation);
    00156     }
    00157     else if(translation->toObject != 0)
    00158     {
    00159       PfPose startPose;
    00160       if(action.actionType==MOVE_OBJECT)
    00161       {
    00162         startPose = dynamicWorldState[action.objectIdx]->getPose();
    00163       }
    00164       else //(action.actionType==MOVE_SELF)
    00165       {
    00166         startPose = pose;
    00167       }
    00168       transVec = (translation->toObject->getPose().pos - startPose.pos);
    00169       translation->time = (transVec.length() / translation->speed);
    00170       action.motionParameters.pos = transVec;
    00171       action.motionParameters.pos.rotate(-pose.rotation);
    00172       action.motionParameters.rotation = 0.0;
    00173     }
    00174     else
    00175     {
    00176       transVec = translation->translation;
    00177       transVec.rotate(pose.rotation);
    00178     }
    00179     PfVec standardTransVec(transVec);
    00180     if(action.actionType == MOVE_OBJECT)
    00181     {
    00182       PfPose objPose(dynamicWorldState[action.objectIdx]->getPose());
    00183       transVec *= getMaxTranslationForObject(objPose, transVec,
    00184                                              dynamicWorldState, staticWorldState, action.objectIdx);
    00185       objPose.pos += transVec;
    00186       if(objPose.hasProbabilityDistribution)
    00187       {
    00188         PfVec probVec;
    00189         for(unsigned int i=0; i<objPose.probabilityDistribution.size(); i++)
    00190         {
    00191           if(action.canBeApplied(objPose.probabilityDistribution[i], dynamicWorldState))
    00192           {
    00193             probVec = standardTransVec;
    00194             probVec *= getMaxTranslationForObject
    00195                              (objPose.probabilityDistribution[i], probVec,
    00196                               dynamicWorldState, staticWorldState, action.objectIdx);
    00197             objPose.probabilityDistribution[i].pos += probVec;
    00198           }
    00199         }
    00200       }
    00201       dynamicWorldState[action.objectIdx]->setPose(objPose);
    00202       if(action.joinAction)
    00203       {
    00204         pose.addAbsVector(transVec);
    00205       }
    00206     }
    00207     else //(action.actionType == MOVE_SELF)
    00208     {
    00209       transVec *= getMaxTranslationForObject(pose, transVec, dynamicWorldState, staticWorldState);
    00210       pose.pos += transVec;
    00211       if(pose.hasProbabilityDistribution)
    00212       {
    00213         for(unsigned int i=0; i<pose.probabilityDistribution.size(); i++)
    00214         {
    00215           transVec = standardTransVec;
    00216           transVec *= getMaxTranslationForObject
    00217                            (pose.probabilityDistribution[i], transVec,
    00218                             dynamicWorldState, staticWorldState);
    00219           pose.probabilityDistribution[i].pos += transVec;
    00220         }
    00221       }
    00222     }
    00223   }
    00224   else if(transformationType == ROTATION)
    00225   {
    00226     double rot(0.0);
    00227     Rotation* rotation = (Rotation*)transformation;
    00228     if(rotation->toGradient)
    00229     {
    00230       PfVec gradientVec;
    00231       if(action.actionType==MOVE_OBJECT)
    00232       {
    00233         gradientVec = (callingField->getFutureFieldVecAt(
    00234                           dynamicWorldState[action.objectIdx]->getPose(),
    00235                           dynamicWorldState, action.objectIdx));
    00236       }
    00237       else //(action.actionType==MOVE_SELF)
    00238       {
    00239         gradientVec = callingField->getFutureFieldVecAt(pose, dynamicWorldState);
    00240       }
    00241       if(gradientVec.length() != 0.0)
    00242       {
    00243         gradientVec.normalize();
    00244         gradientVec.rotate(-pose.rotation);
    00245         rot = atan2(gradientVec.y,gradientVec.x);
    00246       }
    00247       rotation->time = (rot / rotation->speed);
    00248       action.motionParameters.rotation = rot;
    00249     }
    00250     else if(rotation->toObject != 0)
    00251     {
    00252       PfPose startPose;
    00253       if(action.actionType==MOVE_OBJECT)
    00254       {
    00255         startPose = dynamicWorldState[action.objectIdx]->getPose();
    00256       }
    00257       else //(action.actionType==MOVE_SELF)
    00258       {
    00259         startPose = pose;
    00260       }
    00261       PfVec vecToObject(rotation->toObject->getPose().pos - startPose.pos);
    00262       if(vecToObject.length() != 0.0)
    00263       {
    00264         vecToObject.normalize();
    00265         vecToObject.rotate(-pose.rotation);
    00266         rot = atan2(vecToObject.y,vecToObject.x);
    00267       }
    00268       rotation->time = (rot / rotation->speed);
    00269       action.motionParameters.rotation = rot;
    00270     }
    00271     else
    00272     {
    00273       rot = rotation->angle;
    00274     }
    00275     double standardRotation(rot);
    00276     if(action.actionType == MOVE_OBJECT)
    00277     {
    00278       PfPose objPose(dynamicWorldState[action.objectIdx]->getPose());
    00279       rot *= getMaxRotationForObject(objPose, pose, rot,
    00280                                      dynamicWorldState, staticWorldState, action.objectIdx);
    00281       objPose.rotateAround(pose.pos, rot);
    00282       if(objPose.hasProbabilityDistribution)
    00283       {
    00284         double probRot;
    00285         for(unsigned int i=0; i<objPose.probabilityDistribution.size(); i++)
    00286         {
    00287           if(action.canBeApplied(objPose.probabilityDistribution[i], dynamicWorldState))
    00288           {
    00289             probRot = standardRotation;
    00290             probRot *= getMaxRotationForObject
    00291                              (objPose.probabilityDistribution[i], pose, probRot,
    00292                               dynamicWorldState, staticWorldState, action.objectIdx);
    00293             objPose.probabilityDistribution[i].rotateAround(pose.pos, probRot);
    00294           }
    00295         }
    00296       }
    00297       dynamicWorldState[action.objectIdx]->setPose(objPose);
    00298       if(action.joinAction)
    00299       {
    00300         pose.rotation += rot;
    00301         pose.normRotation();
    00302       }
    00303     }
    00304     else //(action.actionType == MOVE_SELF)
    00305     {
    00306       rot *= getMaxRotationForObject(pose, pose, rot, dynamicWorldState, staticWorldState);
    00307       pose.rotation += rot;
    00308       pose.normRotation();
    00309       if(pose.hasProbabilityDistribution)
    00310       {
    00311         for(unsigned int i=0; i<pose.probabilityDistribution.size(); i++)
    00312         {
    00313           rot = standardRotation;
    00314           rot *= getMaxRotationForObject
    00315                            (pose.probabilityDistribution[i], pose, rot,
    00316                             dynamicWorldState, staticWorldState);
    00317           pose.probabilityDistribution[i].rotation += rot;
    00318           pose.probabilityDistribution[i].normRotation();
    00319         }
    00320       }
    00321     }
    00322   }
    00323 }
    00324
    00325
    00326 double FutureWorldModelGenerator::getMaxTranslationForObject(
    00327                                   const PfPose& objectPose,
    00328                                   const PfVec& translation,
    00329                                   std::vector<PotentialFieldsObject*>& otherObjects,
    00330                                   std::vector<PotentialFieldsObject*>& otherStaticObjects,
    00331                                   int excludedObject)
    00332 {
    00333   Line wayOfObject;
    00334   wayOfObject.p1 = objectPose.pos;
    00335   wayOfObject.p2 = (wayOfObject.p1 + translation);
    00336   PfVec halfTrans(translation * 0.5);
    00337   wayOfObject.position = (wayOfObject.p1 + halfTrans);
    00338   wayOfObject.radiusOfCollisionCircle = halfTrans.length();
    00339   std::vector<PfVec> intersectionPoints;
    00340   unsigned int skip;
    00341   if(excludedObject == -1)
    00342   {
    00343     skip = otherObjects.size();
    00344   }
    00345   else
    00346   {
    00347     skip = static_cast<unsigned int>(excludedObject);
    00348   }
    00349   for(unsigned int i = 0; i<otherObjects.size(); i++)
    00350   {
    00351     if((otherObjects[i]->isActive()) && (i != skip))
    00352     {
    00353       PfieldGeometricObject* geoObj = otherObjects[i]->getAbsGeometry();
    00354       intersectGeometricObjects(&wayOfObject, geoObj, intersectionPoints);
    00355     }
    00356   }
    00357   for(unsigned int j = 0; j<otherStaticObjects.size(); j++)
    00358   {
    00359     PfieldGeometricObject* geoObj = otherStaticObjects[j]->getAbsGeometry();
    00360     intersectGeometricObjects(&wayOfObject, geoObj, intersectionPoints);
    00361   }
    00362   if(intersectionPoints.size()>0)
    00363   {
    00364     //Find nearest point
    00365     PfVec nearestPoint(intersectionPoints[0]);
    00366     double dist = (nearestPoint - wayOfObject.p1).length();
    00367     for(unsigned int j = 1; j<intersectionPoints.size(); j++)
    00368     {
    00369       PfVec testPoint(intersectionPoints[j]);
    00370       double distToTest = (testPoint - wayOfObject.p1).length();
    00371       if(distToTest < dist)
    00372       {
    00373         dist = distToTest;
    00374         nearestPoint = testPoint;
    00375       }
    00376     }
    00377     double transLength = translation.length();
    00378     if(dist >= transLength)
    00379     {
    00380       return 1.0;
    00381     }
    00382     else
    00383     {
    00384       return (dist/transLength);
    00385     }
    00386   }
    00387   else
    00388   {
    00389     return 1.0;
    00390   }
    00391 }
    00392
    00393
    00394 double FutureWorldModelGenerator::getMaxRotationForObject(
    00395                                  const PfPose& objectPose,
    00396                                  const PfPose& rotationPose,
    00397                                  double rotation,
    00398                                  std::vector<PotentialFieldsObject*>& otherObjects,
    00399                                  std::vector<PotentialFieldsObject*>& otherStaticObjects,
    00400                                  int excludedObject)
    00401 {
    00402   double radius((objectPose.pos-rotationPose.pos).length());
    00403   if(radius < EPSILON)
    00404   {
    00405     return 1.0;
    00406   }
    00407   Circle rotationCircle;
    00408   rotationCircle.position = rotationPose.pos;
    00409   rotationCircle.radius = radius;
    00410   rotationCircle.initRadiusOfCollisionCircle();
    00411   std::vector<PfVec> intersectionPoints;
    00412   unsigned int skip;
    00413   if(excludedObject == -1)
    00414   {
    00415     skip = otherObjects.size();
    00416   }
    00417   else
    00418   {
    00419     skip = static_cast<unsigned int>(excludedObject);
    00420   }
    00421   for(unsigned int i = 0; i<otherObjects.size(); i++)
    00422   {
    00423     if((otherObjects[i]->isActive()) && (i != skip))
    00424     {
    00425       PfieldGeometricObject* geoObj = otherObjects[i]->getAbsGeometry();
    00426       intersectGeometricObjects(&rotationCircle, geoObj, intersectionPoints);
    00427     }
    00428   }
    00429   for(unsigned int j = 0; j<otherStaticObjects.size(); j++)
    00430   {
    00431     PfieldGeometricObject* geoObj = otherStaticObjects[j]->getAbsGeometry();
    00432     intersectGeometricObjects(&rotationCircle, geoObj, intersectionPoints);
    00433   }
    00434   if(intersectionPoints.size()>0)
    00435   {
    00436     PfPose sectorBase(rotationPose);
    00437     sectorBase.rotation += rotation/2.0;
    00438     sectorBase.normRotation();
    00439     Sector sector;
    00440     sector.openingAngle = rotation;
    00441     std::vector< PfVec >::const_iterator point;
    00442     double nearestAngle = fabs(rotation);
    00443     for(point = intersectionPoints.begin();
    00444         point != intersectionPoints.end(); ++point)
    00445     {
    00446        if(sector.pointInside(sectorBase, (*point)))
    00447        {
    00448          double angleToPoint(fabs(rotationPose.getAngleTo((*point))));
    00449          if(angleToPoint < nearestAngle)
    00450          {
    00451            nearestAngle = angleToPoint;
    00452          }
    00453        }
    00454     }
    00455     return fabs(nearestAngle/rotation);
    00456   }
    00457   else
    00458   {
    00459     return 1.0;
    00460   }
    00461 }