Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeSpacePlannerXYTheta.cc

Go to the documentation of this file.
00001 #include "ShapeSpacePlannerXYTheta.h"
00002 #include "Shared/mathutils.h" // for isnan fix
00003 
00004 //================ RRTNodeXYTheta ================
00005 
00006 const AngSignPi RRTNodeXYTheta::turnLimit = M_PI/12;  // was M_PI/18;
00007 
00008 ostream& operator<<(ostream &os, const RRTNodeXYTheta::NodeValue_t q) {
00009   os << float(q.turn)*180/M_PI << " deg";
00010   os << ( (q.turn > 0) ? "<" : (q.turn < 0) ? ">" : ":" );
00011   os << " [" << q.x << ", " << q.y << "] @ " << float(q.theta)*180/M_PI << " deg.";
00012   return os;
00013 }
00014 
00015 float RRTNodeXYTheta::distance(const NodeValue_t &target) {
00016   return (q.x-target.x)*(q.x-target.x) +
00017          (q.y-target.y)*(q.y-target.y);
00018 }
00019 
00020 void RRTNodeXYTheta::generateSample(const NodeValue_t &lower, 
00021                                     const NodeValue_t &upper,
00022                                     NodeValue_t &sample) {
00023   sample.x = randRange(lower.x, upper.x);
00024   sample.y = randRange(lower.y, upper.y);
00025   sample.theta = std::numeric_limits<float>::quiet_NaN();
00026 }
00027 
00028 RRTNodeBase::Interp_t 
00029 RRTNodeXYTheta::interpolate(const NodeValue_t &start, const NodeValue_t &target, const NodeValue_t &interpStep,
00030                             bool truncate, CollisionChecker *cc, NodeValue_t &reached, bool searchingBackwards) {
00031   AngTwoPi newHeading = atan2(target.y - start.y, target.x - start.x) +
00032     (searchingBackwards ? M_PI : 0);
00033   AngSignPi headingChange = float(newHeading - start.theta) *
00034     (searchingBackwards ? -1 : 1);
00035   AngSignTwoPi turn;
00036   if ( ! safeTurn(start, headingChange, turn, cc) )
00037     return COLLISION;
00038 
00039   // If we get here, either the turn from the start heading to the
00040   // target heading was less than turnLimit, or we found a turn direction that
00041   // avoided collisions during heading interpolation.
00042   reached = start;
00043   reached.theta = newHeading;
00044   // If searching forward, reached.turn is the turn we will make before going to (x,y).
00045   // If searching backward, reached.turn is the turn we will make upon reaching node start
00046   reached.turn = turn;
00047 
00048   float xsteps = std::fabs(target.x - start.x) / interpStep.x;
00049   float ysteps = std::fabs(target.y - start.y) / interpStep.y;
00050   float numSteps = std::max(xsteps,ysteps);
00051   unsigned int safeSteps = max((unsigned int)numSteps, 1u);
00052   if ( safeSteps < numSteps && (!truncate || safeSteps < maxInterpolations ))
00053     ++safeSteps;
00054   float deltaX = (target.x - start.x) / safeSteps;
00055   float deltaY = (target.y - start.y) / safeSteps;
00056   bool truncated = safeSteps > maxInterpolations && truncate;
00057   if ( truncated )
00058     safeSteps = maxInterpolations;
00059   
00060   for (unsigned int t = 0; t < safeSteps; t++) {
00061     reached.x += deltaX;
00062     reached.y += deltaY;
00063     if ( cc->collides(reached) )
00064       return COLLISION;
00065   }
00066 
00067   if ( truncated )
00068     return APPROACHED;
00069   NodeValue_t target2 = target;
00070   target2.theta = reached.theta;
00071   if ( cc->collides(target2) )   // last step might not have taken us all the way to target, so check target here
00072     return COLLISION;
00073   // If target isn't random but instead comes from the other tree,
00074   // see if we can safely turn to the target heading.
00075   if ( ! std::isnan((float)target.theta) ) {
00076     AngSignTwoPi dummyTurn;
00077     if ( safeTurn(target2, AngSignPi(target.theta-newHeading), dummyTurn, cc) )
00078       return REACHED;
00079   }
00080   return APPROACHED;
00081 }
00082 
00083 bool RRTNodeXYTheta::safeTurn(const NodeValue_t &start, const AngSignPi headingChange, AngSignTwoPi &turn, CollisionChecker* cc) {
00084   turn = headingChange;
00085   if ( abs(headingChange) <= turnLimit )
00086     return true;
00087 
00088   // The turn is substantial, so need to check for collisions while turning 
00089   AngSignPi deltaHeading = (headingChange > 0 ) ? turnLimit : AngSignPi(-turnLimit);
00090   int nSteps = abs(float(headingChange) / turnLimit);
00091   int i = 0;
00092   NodeValue_t node = start;
00093   for (; i < nSteps; i++) {
00094     node.theta += float(deltaHeading);
00095     if ( cc->collides(node) )
00096       break; // will try turning the other way
00097   }
00098   if ( i == nSteps )
00099     return true;
00100 
00101   // collision occurrred, so try turning the other way
00102   turn -= (turn > 0) ? 2*M_PI : -2*M_PI;
00103   deltaHeading = -deltaHeading;
00104   nSteps = abs(float(turn) / turnLimit);  // need float so division doesn't return a wrapped AngSignTwoPi
00105   i = 0;
00106   node.theta = start.theta;
00107   for (; i < nSteps; i++) {
00108     node.theta += float(deltaHeading);
00109     if ( cc->collides(node) )
00110       return false;
00111   }
00112   return true;
00113 }
00114 
00115 RRTNodeBase::Interp_t 
00116 RRTNodeXYTheta::snipInterpolate(const NodeValue_t &start, const NodeValue_t &target, const AngTwoPi nextTheta,
00117                                 const NodeValue_t &interpStep, CollisionChecker *cc,
00118                                 NodeValue_t &reached, AngSignTwoPi &nextTurn) {
00119   AngTwoPi newHeading = atan2(target.y - start.y, target.x - start.x);
00120   AngSignPi headingChange = float(newHeading - start.theta);
00121   reached = start;
00122   AngSignTwoPi turn;
00123   if ( ! safeTurn(reached, headingChange, turn, cc) )
00124     return COLLISION;
00125   reached.theta = newHeading;
00126   reached.turn = turn;
00127 
00128   int xsteps = int(fabs(target.x-start.x)/interpStep.x);
00129   int ysteps = int(fabs(target.y-start.y)/interpStep.y);
00130   int numSteps = std::max(xsteps,ysteps);
00131   float deltaX = (target.x-start.x)/float(numSteps);
00132   float deltaY = (target.y-start.y)/float(numSteps);
00133   // Interpolate along the path and check for collisions
00134   for (int t = 0; t < numSteps; t++) {
00135     reached.x += deltaX;
00136     reached.y += deltaY;
00137     if ( cc->collides(reached) )
00138       return COLLISION;
00139   }
00140   
00141   reached.x = target.x;
00142   reached.y = target.y;
00143   if ( cc->collides(reached) )  // because loop may not have gone all the way to the target
00144     return COLLISION;
00145   if ( safeTurn(reached, AngSignPi(nextTheta-newHeading), nextTurn, cc) )
00146     return REACHED;
00147   else
00148     return APPROACHED;
00149 }
00150 
00151 std::string RRTNodeXYTheta::toString() const {
00152   char buff[100];
00153   sprintf(buff, "%7.2f %7.2f %7.2f", q.x, q.y, (float)(q.theta));
00154   return string(buff);
00155 }
00156 
00157 bool RRTNodeXYTheta::CollisionChecker::collides(const NodeValue_t &qnew, GenericRRTBase::PlannerResult2D* result) {
00158   // treat as collision if outside world bounds
00159   if ( worldBounds.isValid() && !worldBounds->isInside(Point(qnew.x, qnew.y)) ) {
00160     if (result) {
00161       ostringstream os;
00162       os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
00163       result->movingObstacle = new HierarchicalObstacle; // there really is none
00164       result->movingObstacle->name = os.str();
00165       ostringstream os2;
00166       os2 << worldBounds->getName() << "-" << worldBounds->getId();
00167       result->collidingObstacle = new HierarchicalObstacle;
00168       result->collidingObstacle->name = os2.str();
00169     }
00170     return true;
00171   }
00172   
00173   body.updatePosition(fmat::pack(qnew.x, qnew.y));
00174   body.updateRotation(fmat::rotation2D(qnew.theta));
00175   
00176   for (size_t i = 0; i < obstacles.size(); i++) {
00177     if ( !obstacles[i]->isBodyObstacle() && obstacles[i]->collides(body)) {
00178       if (result) {
00179         ostringstream os;
00180         os << VRmixin::theAgent->getName() << "-" << VRmixin::theAgent->getId();
00181         result->movingObstacle = dynamic_cast<PlannerObstacle2D*>(body.clone());
00182         result->movingObstacle->name = os.str();
00183         result->collidingObstacle = dynamic_cast<PlannerObstacle2D*>(obstacles[i]->clone());
00184       }
00185       // std::cout << "Collision: " << robot.toString() << " with " << obstacles[i]->toString() << std::endl;
00186       return true;
00187     }
00188   }
00189   return false;
00190 }
00191 
00192 std::vector<PlannerObstacle2D*> RRTNodeXYTheta::CollisionChecker::colliders(const NodeValue_t &qnew) {
00193   body.updatePosition(fmat::pack(qnew.x, qnew.y));
00194   body.updateRotation(fmat::rotation2D(qnew.theta));
00195   std::vector<PlannerObstacle2D*> result;
00196   for (size_t i = 0; i < obstacles.size(); i++)
00197     if ( !obstacles[i]->isBodyObstacle() && obstacles[i]->collides(body))
00198       result.push_back(obstacles[i]);
00199   return result;
00200 }
00201 
00202 
00203 //================ ShapeSpacePlannerXYTheta ================
00204 
00205 ShapeSpacePlannerXYTheta::ShapeSpacePlannerXYTheta(ShapeSpace &shs, const Shape<PolygonData> &worldBounds,
00206                                                    float inflation) :
00207   GenericRRT<NodeType_t, 2>(new NodeType_t::CollisionChecker(shs, worldBounds, inflation)),
00208   targetHeading(), baseOffset(), gateLength()
00209 {
00210   NodeValue_t interp(25,25,0);  // step size in mm
00211   setInterpolation(interp);
00212 }
00213 
00214 void ShapeSpacePlannerXYTheta::initialize(const NodeValue_t &start, std::vector<NodeType_t> &treeStartResult,
00215                                           const NodeValue_t &end, std::vector<NodeType_t> &treeEndResult) {
00216   GenericRRT<NodeType_t, 2>::initialize(start, treeStartResult, end, treeEndResult);
00217   // To enforce a target heading or base offset, define a gate point
00218   fmat::Column<3> endpt = fmat::pack(end.x, end.y, 0);
00219   if ( gateLength == 0 )
00220     gateLength = 0.1; // must have a gate
00221   fmat::Column<3> gateOffset = baseOffset + fmat::pack(gateLength, 0, 0);
00222   if ( ! std::isnan(targetHeading) ) {
00223     fmat::Column<3>  baseEndPt = endpt - fmat::rotationZ(targetHeading) * baseOffset;
00224     treeEndResult[0].q = NodeValue_t(baseEndPt[0], baseEndPt[1], targetHeading);
00225     fmat::Column<3> offsetEndPt = endpt - fmat::rotationZ(targetHeading) * gateOffset;
00226     NodeValue_t qnew(offsetEndPt[0], offsetEndPt[1], targetHeading);
00227     addNode(&treeEndResult, qnew, 0);
00228     treeEndResult[0].parent = 1;  // index of starting node for nearestNode search
00229     return;
00230   }
00231   // No specific targetHeading, but has baseOffset or gate, so define
00232   // a ring of offset target points, with one target aligned with the
00233   // start state heading.
00234   treeEndResult[0].q.theta = std::numeric_limits<float>::quiet_NaN(); // mark root as a dummy node
00235   float tanHeading = tangentHeading(start,end);
00236   // std::cout << "tanHeading = " << tanHeading << " = " << tanHeading*180/M_PI << " deg.\n";
00237   for (float theta = tanHeading; theta < float(tanHeading)+2*M_PI-(1e-5); theta += 2*M_PI/numDivisions) {
00238     fmat::Column<3> offsetEndPt = endpt - fmat::rotationZ(theta) * baseOffset;
00239     NodeValue_t qnew(offsetEndPt[0], offsetEndPt[1], theta);
00240     addNode(&treeEndResult, qnew, 0);
00241   }
00242   treeEndResult[0].parent = numDivisions+1;  // index of starting node for nearestNode search
00243   for (unsigned int parentIndex = 1; parentIndex <= numDivisions; ++parentIndex) {
00244     NodeValue_t offsetTarget = treeEndResult[parentIndex].q;
00245     fmat::Column<3> offsetEndPt = fmat::pack(offsetTarget.x, offsetTarget.y, 0);
00246     float heading = offsetTarget.theta;
00247     fmat::Column<3> gatePt = offsetEndPt - fmat::rotationZ(heading)*fmat::pack(gateLength,0,0);
00248     NodeValue_t qnew(gatePt[0], gatePt[1], heading);
00249     addNode(&treeEndResult, qnew, parentIndex);
00250   }
00251 }
00252 
00253 AngTwoPi ShapeSpacePlannerXYTheta::tangentHeading(const NodeValue_t &start, const NodeValue_t &end) const {
00254   // The equations below come from the Wikipedia article on tangent
00255   // lines between two circles.  We assume the second circle has zero
00256   // radius.
00257   fmat::Column<2> c1 = fmat::pack(start.x,start.y), c2 = fmat::pack(end.x,end.y);
00258   float d = (c2-c1).norm();
00259   float radius = fabs(baseOffset[1]);
00260   float R = -radius/d;
00261   float X = (c2[0] - c1[0]) / d;
00262   float Y = (c2[1] - c1[1]) / d;
00263   float rsq = sqrt(1 - R*R);
00264   float a = R*X - Y*rsq;
00265   float b = R*Y + X*rsq;
00266   AngTwoPi theta = atan2(-a,b);
00267   return theta;
00268 }
00269 
00270 void ShapeSpacePlannerXYTheta::buildPath(const std::vector<NodeType_t> *treeStart, const std::vector<NodeType_t> *treeEnd,
00271                                          std::vector<NodeValue_t> &path) {
00272   
00273   // connection point is last value in each tree
00274   unsigned int n = treeStart->size() - 1;
00275   path.push_back((*treeStart)[n].q);
00276   while (n != 0) {
00277     n = (*treeStart)[n].parent;
00278     path.push_back((*treeStart)[n].q);
00279   }
00280   std::reverse(path.begin(), path.end());
00281   AngTwoPi lastHeading = path.back().theta;
00282 
00283   // Process the end tree unless it consists of nothing but dummy nodes
00284   // created due to an unspecified targetHeading.
00285   // Need to reorder the theta and turn fields since we're reversing the path.
00286   n = treeEnd->size() - 1;
00287   AngTwoPi lastTheta = (*treeEnd)[n].q.theta;
00288   AngSignTwoPi lastTurn = (*treeEnd)[n].q.turn;
00289   path.push_back((*treeEnd)[n].q);
00290   path.back().theta = atan2((*treeEnd)[n].q.y, (*treeEnd)[n].q.x);
00291   path.back().turn = float(AngSignPi(path.back().theta - lastHeading));
00292   // Note: may need to do a safeTurn check for prevLastTurn, but the
00293   // path smoother will probably clean this up anyway.
00294   AngSignTwoPi prevLastTurn = float(AngSignPi(lastTheta - path.back().theta));
00295   while (n != 0) {
00296     n = (*treeEnd)[n].parent;
00297     path.push_back((*treeEnd)[n].q);
00298     path.back().theta = lastTheta;
00299     lastTheta = (*treeEnd)[n].q.theta;
00300     path.back().turn = prevLastTurn;
00301     prevLastTurn = lastTurn;
00302     lastTurn = (*treeEnd)[n].q.turn;
00303   }
00304 
00305   if ( std::isnan(targetHeading) )
00306     path.pop_back();  // remove dummy root node so we don't try to go there
00307 
00308   /*
00309   for ( size_t i = 0; i < path.size(); i++ ) {
00310     Point pt(path[i].x, path[i].y, 0, allocentric);
00311       cout << "path[" << i << "] = " << pt
00312      << " hdg " << float(AngTwoPi(path[i].theta)) * 180/M_PI << " deg."
00313      << " (turn " << float(path[i].turn)*180/M_PI << " deg.)" << endl;
00314   }
00315   std::cout << "====\n";
00316   */
00317 
00318   // Path smoothing: we use two approaches:
00319   //
00320   // 1. Pick random pairs of segments and try to snip the intervening
00321   // segments.  This can potentially snip large sequences quickly.
00322   //
00323   // 2. Systematically try every possible excise combination on the
00324   // segments that remain.
00325 
00326   size_t maxIter = path.size();
00327   AngSignTwoPi nextTurn;
00328   
00329   for (size_t i = 0; i < maxIter; i++) {
00330     unsigned int a = rand() % (path.size()-1);
00331     unsigned int b = rand() % (path.size()-1);
00332     if (a > b)
00333       std::swap(a,b);
00334     else if (a == b)
00335       continue;
00336     if ( b-a < 2 )
00337       continue;
00338     NodeValue_t reached;
00339     AngTwoPi nextTheta = (b < path.size()-1) ? path[b+1].theta : path[b].theta;
00340     if ( NodeType_t::snipInterpolate(path[a], path[b], nextTheta, smoothingInterpolationStep,
00341                                      cc, reached, nextTurn) == RRTNodeBase::REACHED ) {
00342       path[b] = reached;
00343       if ( b < path.size()-1 )
00344         path[b+1].turn = nextTurn;
00345       path.erase(path.begin()+a+1,path.begin()+b);
00346     }
00347   }
00348   
00349   for (size_t i = 0; i+2 < path.size()-1; i++) {
00350     NodeValue_t reached, goodReached;
00351     AngSignTwoPi goodNextTurn;
00352     size_t j;
00353     for (j = i + 2; j < path.size()-1; j++) {
00354       AngTwoPi nextTheta = (j < path.size()-1) ? path[j+1].theta : path[j].theta;
00355       if ( NodeType_t::snipInterpolate(path[i], path[j], nextTheta, smoothingInterpolationStep,
00356                                        cc, reached, nextTurn) == RRTNodeBase::REACHED ) {
00357         goodReached = reached;
00358         goodNextTurn = nextTurn;
00359       }
00360       else
00361         break;
00362     }
00363     if ( --j >= i + 2 ) {
00364       path[j] = goodReached;
00365       if ( j < path.size()-1 )
00366         path[j+1].turn = goodNextTurn;
00367       path.erase(path.begin()+i+1, path.begin()+j);
00368     }
00369   }
00370 
00371   /*
00372   // If no targetHeading, make sure we end up pointing toward the destination
00373   if ( std::isnan(targetHeading) ) {
00374     NodeValue_t fixHeading = path.back();
00375     float newTheta = atan2(dest.y-fixHeading.y, dest.x-fixHeading.x);
00376     fixHeading.turn = float(AngSignPi(newTheta - float(fixHeading.theta)));
00377     fixHeading.theta = newTheta;
00378     path.push_back(fixHeading);
00379   }
00380 */
00381 }
00382 
00383 GenericRRTBase::PlannerResult2D
00384 ShapeSpacePlannerXYTheta::planPath(const Point &startPoint,
00385                                    const fmat::Column<3> &_baseOffset,
00386                                    float _gateLength,
00387                                    const Point &endPoint,
00388                                    const AngTwoPi initialHeading,
00389                                    const AngTwoPi _targetHeading,
00390                                    unsigned int maxIterations,
00391                                    std::vector<NodeValue_t> *pathResult,
00392                                    std::vector<NodeType_t> *treeStartResult,
00393                                    std::vector<NodeType_t> *treeEndResult) {
00394   NodeValue_t lower, upper;
00395   if ( cc->getWorldBounds().isValid() ) {
00396     BoundingBox2D b = cc->getWorldBounds()->getBoundingBox();
00397     lower.x = b.min[0]; lower.y = b.min[1];
00398     upper.x = b.max[0]; upper.y = b.max[1];
00399   } else {
00400     // combine with obstacles + start and end points
00401     BoundingBox2D b = cc->getObstacleBoundingBox();
00402     b.expand(fmat::SubVector<2,const fmat::fmatReal>(startPoint.coords));
00403     b.expand(fmat::SubVector<2,const fmat::fmatReal>(endPoint.coords));
00404     fmat::Column<2> robotBounds = cc->getBodyBoundingBox().getDimensions();
00405     float extra = std::max(robotBounds[0], robotBounds[1]);
00406     lower.x =  b.min[0] - 2*extra;
00407     lower.y = b.min[1] - 2*extra;
00408     upper.x =  b.max[0] + 2*extra;
00409     upper.y = b.max[1] + 2*extra;
00410   }
00411   // std::cout << "World bounds: [" << lower.x << "," << lower.y << "]  to  ["
00412   // << upper.x << "," << upper.y << "]" << std::endl;
00413   setLimits(lower,upper);
00414   
00415   targetHeading = _targetHeading;
00416   float safeTargetHeading = std::isnan((float)_targetHeading) ? 0 : targetHeading;
00417   baseOffset = _baseOffset;
00418   gateLength = _gateLength;
00419   
00420   NodeValue_t startval(startPoint.coordX(), startPoint.coordY(), initialHeading);
00421   fmat::Column<3> endWithOffset = endPoint.getCoords();
00422   NodeValue_t endval(endWithOffset[0], endWithOffset[1], AngTwoPi(safeTargetHeading));
00423   // If no targetHeading specified, endval is a dummy goal node.  The real nodes
00424   // will be added by the initialize() method that planPath calls.
00425   return GenericRRT<NodeType_t, 2>::planPath(startval, endval, maxIterations, 
00426                                              pathResult, treeStartResult, treeEndResult);
00427 }
00428 
00429 void ShapeSpacePlannerXYTheta::plotPath(const std::vector<NodeValue_t> &path,
00430           Shape<GraphicsData> &graphics,
00431           rgb color) {
00432   for ( unsigned int i = 1; i < path.size(); i++ )
00433     graphics->add(new GraphicsData::LineElement("seg",
00434             std::pair<float,float>(path[i-1].x,path[i-1].y),
00435             std::pair<float,float>(path[i].x,path[i].y),
00436             color));
00437 }
00438 
00439 void ShapeSpacePlannerXYTheta::plotTree(const std::vector<NodeType_t> &tree,
00440                                         Shape<GraphicsData> &graphics,
00441                                         rgb color) {
00442   for ( unsigned int i = 1; i < tree.size(); i++ ) { // start from 1 because root has no parent
00443     unsigned int parent = tree[i].parent;
00444     if ( parent != 0 || !std::isnan((float)tree[parent].q.theta ) )
00445       graphics->add(new GraphicsData::LineElement
00446                     ("branch", 
00447                      std::pair<float,float>(tree[parent].q.x,
00448                                             tree[parent].q.y),
00449                      std::pair<float,float>(tree[i].q.x,
00450                                             tree[i].q.y),
00451                      color));
00452     else
00453       graphics->add(new GraphicsData::EllipseElement
00454                     ("end",
00455                      std::pair<float,float>(tree[i].q.x,
00456                                             tree[i].q.y),
00457                      10, 5, tree[i].q.theta, true, color));
00458   }
00459 }

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