Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeSpacePlannerXY.cc

Go to the documentation of this file.
00001 #include <ostream>
00002 
00003 #include "ShapeSpacePlannerXY.h"
00004 
00005 //================ RRTNodeXY ================
00006 
00007 float RRTNodeXY::distance(const RRTNodeXY::NodeValue_t &target) {
00008   return (q.first-target.first)*(q.first-target.first) +
00009   (q.second-target.second)*(q.second-target.second);
00010 }
00011 
00012 void RRTNodeXY::generateSample(const RRTNodeXY::NodeValue_t &lower, 
00013                                const RRTNodeXY::NodeValue_t &upper,
00014                                RRTNodeXY::NodeValue_t &sample) {
00015   sample.first = randRange(lower.first, upper.first);
00016   sample.second = randRange(lower.second, upper.second);
00017 }
00018 
00019 RRTNodeBase::Interp_t 
00020 RRTNodeXY::interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interp, 
00021                        bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool searchBackwards) {
00022   int xsteps = int(std::fabs(target.first-start.first)/interp.first);
00023   int ysteps = int(std::fabs(target.second-start.second)/interp.second);
00024   int numSteps = std::max(xsteps,ysteps);
00025   float deltaX = (target.first-start.first)/float(numSteps);
00026   float deltaY = (target.second-start.second)/float(numSteps);
00027   
00028   bool truncated = (unsigned int)numSteps > maxInterpolations && truncate;
00029   if ( truncated )
00030     numSteps = maxInterpolations;
00031   
00032   // Interpolate along the path and check for collisions
00033   reached = start;
00034   for (int t = 0; t < numSteps; t++) {
00035     reached.first += deltaX;
00036     reached.second += deltaY;
00037     if ( cc->collides(reached) )
00038       return COLLISION;
00039   }
00040   
00041   if ( cc->collides(target) )
00042     return COLLISION;
00043   else if ( !truncated )
00044     return REACHED;
00045   else
00046     return APPROACHED;
00047 }
00048 
00049 std::string RRTNodeXY::toString() const {
00050   char buff[100];
00051   sprintf(buff, "%7.2f %7.2f", q.first, q.second);
00052   return string(buff);
00053 }
00054 
00055 bool RRTNodeXY::CollisionChecker::collides(const NodeValue_t &qnew, ShapeSpacePlannerXY::PlannerResult* result) const {
00056   CircularObstacle robot(qnew.first, qnew.second, robotRadius);
00057   // treat as collision if outside world bounds
00058   if ( worldBounds.isValid() && !worldBounds->isInside(Point(qnew.first, qnew.second)) ) {
00059     if (result) {
00060       ostringstream os;
00061       os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
00062       result->movingObstacle = new CircularObstacle(robot);
00063       result->movingObstacle->name = os.str();
00064       ostringstream os2;
00065       os2 << worldBounds->getName() << "-" << worldBounds->getId();
00066       result->collidingObstacle = new HierarchicalObstacle;
00067       result->collidingObstacle->name = os2.str();
00068     }
00069     return true;
00070   }
00071   
00072   for (size_t i = 0; i < obstacles.size(); i++)
00073     if ( !obstacles[i]->isBodyObstacle() && obstacles[i]->collides(robot)) {
00074       //std::cout << "Collision: " << robot.toString() << " with " << obstacles[i]->toString() << std::endl;
00075       if (result) {
00076   ostringstream os;
00077   os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
00078         result->movingObstacle = new CircularObstacle(robot);
00079   result->movingObstacle->name = os.str();
00080   result->collidingObstacle = dynamic_cast<PlannerObstacle2D*>(obstacles[i]->clone());
00081       }
00082       return true;
00083     }
00084   return false;
00085 }
00086 
00087 //================ ShapeSpacePlannerXY ================
00088 
00089 ShapeSpacePlannerXY::ShapeSpacePlannerXY(ShapeSpace &shs, const Shape<PolygonData> &worldBounds,
00090                                          float inflation, float _robotRadius)
00091   : GenericRRT<NodeType_t, 2>(new NodeType_t::CollisionChecker(shs, worldBounds, inflation, _robotRadius)),
00092     robotRadius(_robotRadius) {
00093   NodeValue_t interp;
00094   interp.first = interp.second = 25; // step size in mm
00095   setInterpolation(interp);
00096 }
00097 
00098 ShapeSpacePlannerXY::PlannerResult
00099 ShapeSpacePlannerXY::planPath(const Point& start, const Point &end,
00100                               unsigned int maxIterations,
00101                               std::vector<NodeValue_t> *pathResult,
00102                               std::vector<NodeType_t> *treeStartResult,
00103                               std::vector<NodeType_t> *treeEndResult) {
00104   NodeValue_t lower, upper;
00105   if ( cc->getWorldBounds().isValid() ) {
00106     BoundingBox2D b = cc->getWorldBounds()->getBoundingBox();
00107     lower.first = b.min[0]; lower.second = b.min[1];
00108     upper.first = b.max[0]; upper.second = b.max[1];
00109   } else {
00110     // combine with obstacles + start and end points
00111     BoundingBox2D b(cc->getObstacleBoundingBox());
00112     b.expand(start.coords);
00113     b.expand(end.coords);
00114     lower.first =  b.min[0] - 2*robotRadius;
00115     lower.second = b.min[1] - 2*robotRadius;
00116     upper.first =  b.max[0] + 2*robotRadius;
00117     upper.second = b.max[1] + 2*robotRadius;
00118   }
00119   std::cout << "World bounds: [" << lower.first << "," << lower.second
00120       << "]  to  [" << upper.first << "," << upper.second << "]" << std::endl;
00121   setLimits(lower,upper);
00122   
00123   NodeValue_t startval(start.coordX(), start.coordY());
00124   NodeValue_t endval(end.coordX(), end.coordY());
00125   return GenericRRT<NodeType_t, 2>::planPath(startval, endval, maxIterations, 
00126                                              pathResult, treeStartResult, treeEndResult);
00127 }
00128 
00129 void ShapeSpacePlannerXY::plotPath(const std::vector<NodeValue_t> &path,
00130            Shape<GraphicsData> &graphics,
00131            rgb color) {
00132   for ( unsigned int i = 1; i < path.size(); i++ )
00133     graphics->add(new GraphicsData::LineElement("seg", path[i-1], path[i], color));
00134 }
00135 
00136 void ShapeSpacePlannerXY::plotTree(const std::vector<NodeType_t> &tree,
00137                                    Shape<GraphicsData> &graphics,
00138                                    rgb color) {
00139   for ( unsigned int i = 1; i < tree.size(); i++ ) // start from 1 because root has no parent
00140     graphics->add(new GraphicsData::LineElement("branch", tree[tree[i].parent].q, tree[i].q, color));
00141 }
00142 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:51 2016 by Doxygen 1.6.3