All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
RRTstar.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Authors: Alejandro Perez, Sertac Karaman, Ioan Sucan */
00036 
00037 #include "ompl/contrib/rrt_star/RRTstar.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <algorithm>
00042 #include <limits>
00043 #include <map>
00044 
00045 void ompl::geometric::RRTstar::setup(void)
00046 {
00047     Planner::setup();
00048     SelfConfig sc(si_, getName());
00049     sc.configurePlannerRange(maxDistance_);
00050 
00051     ballRadiusMax_ = si_->getMaximumExtent();
00052     ballRadiusConst_ = maxDistance_ * sqrt(si_->getStateSpace()->getDimension());
00053 
00054     delayCC_ = true;
00055 
00056     if (!nn_)
00057         nn_.reset(new NearestNeighborsGNAT<Motion*>());
00058     nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
00059 }
00060 
00061 void ompl::geometric::RRTstar::clear(void)
00062 {
00063     Planner::clear();
00064     sampler_.reset();
00065     freeMemory();
00066     if (nn_)
00067         nn_->clear();
00068 }
00069 
00070 bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
00071 {
00072     checkValidity();
00073     base::Goal                 *goal   = pdef_->getGoal().get();
00074     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00075 
00076     if (!goal)
00077     {
00078         msg_.error("Goal undefined");
00079         return false; }
00080     while (const base::State *st = pis_.nextStart())
00081     {
00082         Motion *motion = new Motion(si_);
00083         si_->copyState(motion->state, st);
00084         nn_->add(motion);
00085     }
00086 
00087     if (nn_->size() == 0)
00088     {
00089         msg_.error("There are no valid initial states!");
00090         return false;
00091     }
00092 
00093     if (!sampler_)
00094         sampler_ = si_->allocStateSampler();
00095 
00096     msg_.inform("Starting with %u states", nn_->size());
00097 
00098     Motion *solution     = NULL;
00099     Motion *approxsol    = NULL;
00100     double  approxdif    = std::numeric_limits<double>::infinity();
00101     bool    approxsolved = false;
00102 
00103     Motion *rmotion   = new Motion(si_);
00104     base::State *rstate = rmotion->state;
00105     base::State *xstate = si_->allocState();
00106     std::vector<Motion*> solCheck;
00107     std::vector<Motion*> nbh;
00108     std::vector<double>  dists;
00109     std::vector<int>     valid;
00110     long unsigned int    rewireTest = 0;
00111 
00112     while (ptc() == false)
00113     {
00114         /* sample random state (with goal biasing) */
00115         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00116             goal_s->sampleGoal(rstate);
00117         else
00118             sampler_->sampleUniform(rstate);
00119 
00120         /* find closest state in the tree */
00121         Motion *nmotion = nn_->nearest(rmotion);
00122 
00123         base::State *dstate = rstate;
00124 
00125         /* find state to add */
00126         double d = si_->distance(nmotion->state, rstate);
00127         if (d > maxDistance_)
00128         {
00129             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00130             dstate = xstate;
00131         }
00132 
00133         if (si_->checkMotion(nmotion->state, dstate))
00134         {
00135             /* create a motion */
00136             double distN = si_->distance(dstate, nmotion->state);
00137             Motion *motion = new Motion(si_);
00138             si_->copyState(motion->state, dstate);
00139             motion->parent = nmotion;
00140             motion->cost = nmotion->cost + distN;
00141 
00142             /* find nearby neighbors */
00143             double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00144                                 ballRadiusMax_);
00145 
00146             nn_->nearestR(motion, r, nbh);
00147             rewireTest += nbh.size();
00148 
00149             // cache for distance computations
00150             dists.resize(nbh.size());
00151             // cache for motion validity
00152             valid.resize(nbh.size());
00153             std::fill(valid.begin(), valid.end(), 0);
00154 
00155             if(delayCC_)
00156             {
00157                 // calculate all costs and distances
00158                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00159                     if (nbh[i] != nmotion)
00160                     {
00161                         double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
00162                         nbh[i]->cost = c;
00163                     }
00164 
00165                 // sort the nodes
00166                 std::sort(nbh.begin(), nbh.end(), compareMotion);
00167 
00168                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00169                     if (nbh[i] != nmotion)
00170                     {
00171                         dists[i] = si_->distance(nbh[i]->state, dstate);
00172                         nbh[i]->cost -= dists[i];
00173                     }
00174 
00175                 // collision check until a valid motion is found
00176                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00177                     if (nbh[i] != nmotion)
00178                     {
00179 
00180                         dists[i] = si_->distance(nbh[i]->state, dstate);
00181                         double c = nbh[i]->cost + dists[i];
00182                         if (c < motion->cost)
00183                         {
00184                             if (si_->checkMotion(nbh[i]->state, dstate))
00185                             {
00186                                 motion->cost = c;
00187                                 motion->parent = nbh[i];
00188                                 valid[i] = 1;
00189                                 break;
00190                             }
00191                             else
00192                                 valid[i] = -1;
00193                         }
00194                     }
00195                     else
00196                     {
00197                         valid[i] = 1;
00198                         dists[i] = distN;
00199                         break;
00200                     }
00201 
00202             }
00203             else
00204             {
00205                 /* find which one we connect the new state to*/
00206                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00207                     if (nbh[i] != nmotion)
00208                     {
00209 
00210                         dists[i] = si_->distance(nbh[i]->state, dstate);
00211                         double c = nbh[i]->cost + dists[i];
00212                         if (c < motion->cost)
00213                         {
00214                             if (si_->checkMotion(nbh[i]->state, dstate))
00215                             {
00216                                 motion->cost = c;
00217                                 motion->parent = nbh[i];
00218                                 valid[i] = 1;
00219                             }
00220                             else
00221                                 valid[i] = -1;
00222                         }
00223                     }
00224                     else
00225                     {
00226                         valid[i] = 1;
00227                         dists[i] = distN;
00228                     }
00229 
00230             }
00231 
00232             /* add motion to tree */
00233             nn_->add(motion);
00234 
00235             solCheck.resize(1);
00236             solCheck[0] = motion;
00237 
00238             /* rewire tree if needed */
00239             for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00240                 if (nbh[i] != motion->parent)
00241                 {
00242                     double c = motion->cost + dists[i];
00243                     if (c < nbh[i]->cost)
00244                     {
00245                         bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00246                         if (v)
00247                         {
00248                             nbh[i]->parent = motion;
00249                             nbh[i]->cost = c;
00250                             solCheck.push_back(nbh[i]);
00251                         }
00252                     }
00253                 }
00254 
00255             /* check if  we found a solution */
00256             for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00257             {
00258                 double dist = 0.0;
00259                 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00260                 bool sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00261 
00262                 if (solved)
00263                 {
00264                     if (sufficientlyShort)
00265                     {
00266                         solution = solCheck[i];
00267                         break;
00268                     }
00269                     else
00270                     {
00271                         if (approxsolved)
00272                         {
00273                             if (dist < approxdif)
00274                             {
00275                                 approxdif = dist;
00276                                 approxsol = solCheck[i];
00277                             }
00278                         }
00279                         else
00280                         {
00281                             approxsolved = true;
00282                             approxdif = dist;
00283                             approxsol = solCheck[i];
00284                         }
00285                     }
00286                 }
00287                 else
00288                     if (!approxsolved && dist < approxdif)
00289                     {
00290                         approxdif = dist;
00291                         approxsol = solCheck[i];
00292                     }
00293             }
00294 
00295             /* terminate if a solution was found */
00296             if (solution != NULL)
00297                 break;
00298         }
00299     }
00300 
00301     bool approximate = false;
00302     if (solution == NULL)
00303     {
00304         solution = approxsol;
00305         approximate = true;
00306     }
00307 
00308     if (solution != NULL)
00309     {
00310         /* construct the solution path */
00311         std::vector<Motion*> mpath;
00312         while (solution != NULL)
00313         {
00314             mpath.push_back(solution);
00315             solution = solution->parent;
00316         }
00317 
00318         /* set the solution path */
00319         PathGeometric *path = new PathGeometric(si_);
00320         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00321             path->states.push_back(si_->cloneState(mpath[i]->state));
00322         goal->setDifference(approxdif);
00323         goal->setSolutionPath(base::PathPtr(path), approximate);
00324 
00325         if (approximate)
00326             msg_.warn("Found approximate solution");
00327     }
00328 
00329     si_->freeState(xstate);
00330     if (rmotion->state)
00331         si_->freeState(rmotion->state);
00332     delete rmotion;
00333 
00334     msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00335 
00336     return goal->isAchieved();
00337 }
00338 
00339 void ompl::geometric::RRTstar::freeMemory(void)
00340 {
00341     if (nn_)
00342     {
00343         std::vector<Motion*> motions;
00344         nn_->list(motions);
00345         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00346         {
00347             if (motions[i]->state)
00348                 si_->freeState(motions[i]->state);
00349             delete motions[i];
00350         }
00351     }
00352 }
00353 
00354 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00355 {
00356     Planner::getPlannerData(data);
00357 
00358     std::vector<Motion*> motions;
00359     if (nn_)
00360         nn_->list(motions);
00361 
00362     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00363         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00364 }