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 ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTstar")
00046 {
00047     specs_.approximateSolutions = true;
00048     specs_.optimizingPaths = true;
00049 
00050     goalBias_ = 0.05;
00051     maxDistance_ = 0.0;
00052     ballRadiusMax_ = 0.0;
00053     ballRadiusConst_ = 1.0;
00054     delayCC_ = true;
00055 
00056     Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange);
00057     Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias);
00058     Planner::declareParam<double>("ball_radius_constant", this, &RRTstar::setBallRadiusConstant, &RRTstar::getBallRadiusConstant);
00059     Planner::declareParam<double>("max_ball_radius", this, &RRTstar::setMaxBallRadius, &RRTstar::getMaxBallRadius);
00060     Planner::declareParam<bool>("delay_cc", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC);
00061 }
00062 
00063 ompl::geometric::RRTstar::~RRTstar(void)
00064 {
00065     freeMemory();
00066 }
00067 
00068 void ompl::geometric::RRTstar::setup(void)
00069 {
00070     Planner::setup();
00071     tools::SelfConfig sc(si_, getName());
00072     sc.configurePlannerRange(maxDistance_);
00073 
00074     ballRadiusMax_ = si_->getMaximumExtent();
00075     ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
00076 
00077     delayCC_ = true;
00078 
00079     if (!nn_)
00080         nn_.reset(new NearestNeighborsGNAT<Motion*>());
00081     nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
00082 }
00083 
00084 void ompl::geometric::RRTstar::clear(void)
00085 {
00086     Planner::clear();
00087     sampler_.reset();
00088     freeMemory();
00089     if (nn_)
00090         nn_->clear();
00091 }
00092 
00093 bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
00094 {
00095     checkValidity();
00096     base::Goal                 *goal   = pdef_->getGoal().get();
00097     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00098 
00099     if (!goal)
00100     {
00101         msg_.error("Goal undefined");
00102         return false;
00103     }
00104 
00105     while (const base::State *st = pis_.nextStart())
00106     {
00107         Motion *motion = new Motion(si_);
00108         si_->copyState(motion->state, st);
00109         nn_->add(motion);
00110     }
00111 
00112     if (nn_->size() == 0)
00113     {
00114         msg_.error("There are no valid initial states!");
00115         return false;
00116     }
00117 
00118     if (!sampler_)
00119         sampler_ = si_->allocStateSampler();
00120 
00121     msg_.inform("Starting with %u states", nn_->size());
00122 
00123     Motion *solution       = NULL;
00124     Motion *approximation  = NULL;
00125     double approximatedist = std::numeric_limits<double>::infinity();
00126     bool sufficientlyShort = false;
00127 
00128     Motion *rmotion     = new Motion(si_);
00129     base::State *rstate = rmotion->state;
00130     base::State *xstate = si_->allocState();
00131     std::vector<Motion*> solCheck;
00132     std::vector<Motion*> nbh;
00133     std::vector<double>  dists;
00134     std::vector<int>     valid;
00135     unsigned int         rewireTest = 0;
00136 
00137     while (ptc() == false)
00138     {
00139         // sample random state (with goal biasing)
00140         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00141             goal_s->sampleGoal(rstate);
00142         else
00143             sampler_->sampleUniform(rstate);
00144 
00145         // find closest state in the tree
00146         Motion *nmotion = nn_->nearest(rmotion);
00147 
00148         base::State *dstate = rstate;
00149 
00150         // find state to add
00151         double d = si_->distance(nmotion->state, rstate);
00152         if (d > maxDistance_)
00153         {
00154             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00155             dstate = xstate;
00156         }
00157 
00158         if (si_->checkMotion(nmotion->state, dstate))
00159         {
00160             // create a motion
00161             double distN = si_->distance(dstate, nmotion->state);
00162             Motion *motion = new Motion(si_);
00163             si_->copyState(motion->state, dstate);
00164             motion->parent = nmotion;
00165             motion->cost = nmotion->cost + distN;
00166 
00167             // find nearby neighbors
00168             double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00169                                 ballRadiusMax_);
00170 
00171             nn_->nearestR(motion, r, nbh);
00172             rewireTest += nbh.size();
00173 
00174             // cache for distance computations
00175             dists.resize(nbh.size());
00176             // cache for motion validity
00177             valid.resize(nbh.size());
00178             std::fill(valid.begin(), valid.end(), 0);
00179 
00180             if(delayCC_)
00181             {
00182                 // calculate all costs and distances
00183                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00184                     nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
00185 
00186                 // sort the nodes
00187                 std::sort(nbh.begin(), nbh.end(), compareMotion);
00188 
00189                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00190                 {
00191                     dists[i] = si_->distance(nbh[i]->state, dstate);
00192                     nbh[i]->cost -= dists[i];
00193                 }
00194 
00195                 // collision check until a valid motion is found
00196                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00197                 {
00198                     if (nbh[i] != nmotion)
00199                     {
00200                         double c = nbh[i]->cost + dists[i];
00201                         if (c < motion->cost)
00202                         {
00203                             if (si_->checkMotion(nbh[i]->state, dstate))
00204                             {
00205                                 motion->cost = c;
00206                                 motion->parent = nbh[i];
00207                                 valid[i] = 1;
00208                                 break;
00209                             }
00210                             else
00211                                 valid[i] = -1;
00212                         }
00213                     }
00214                     else
00215                     {
00216                         valid[i] = 1;
00217                         dists[i] = distN;
00218                         break;
00219                     }
00220                 }
00221             }
00222             else
00223             {
00224                 // find which one we connect the new state to
00225                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00226                 {
00227                     if (nbh[i] != nmotion)
00228                     {
00229                         dists[i] = si_->distance(nbh[i]->state, dstate);
00230                         double c = nbh[i]->cost + dists[i];
00231                         if (c < motion->cost)
00232                         {
00233                             if (si_->checkMotion(nbh[i]->state, dstate))
00234                             {
00235                                 motion->cost = c;
00236                                 motion->parent = nbh[i];
00237                                 valid[i] = 1;
00238                             }
00239                             else
00240                                 valid[i] = -1;
00241                         }
00242                     }
00243                     else
00244                     {
00245                         valid[i] = 1;
00246                         dists[i] = distN;
00247                     }
00248                 }
00249             }
00250 
00251             // add motion to the tree
00252             nn_->add(motion);
00253             motion->parent->children.push_back(motion);
00254 
00255             solCheck.resize(1);
00256             solCheck[0] = motion;
00257 
00258             // rewire tree if needed
00259             for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00260                 if (nbh[i] != motion->parent)
00261                 {
00262                     double c = motion->cost + dists[i];
00263                     if (c < nbh[i]->cost)
00264                     {
00265                         bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00266                         if (v)
00267                         {
00268                             // Remove this node from its parent list
00269                             removeFromParent (nbh[i]);
00270                             double delta = c - nbh[i]->cost;
00271 
00272                             // Add this node to the new parent
00273                             nbh[i]->parent = motion;
00274                             nbh[i]->cost = c;
00275                             nbh[i]->parent->children.push_back(nbh[i]);
00276                             solCheck.push_back(nbh[i]);
00277 
00278                             // Update the costs of the node's children
00279                             updateChildCosts(nbh[i], delta);
00280                         }
00281                     }
00282                 }
00283 
00284             // check if we found a solution
00285             for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00286             {
00287                 double dist = 0.0;
00288                 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00289                 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00290 
00291                 if (solved)
00292                 {
00293                     if (sufficientlyShort)
00294                     {
00295                         solution = solCheck[i];
00296                         break;
00297                     }
00298                     else if (!solution || (solCheck[i]->cost < solution->cost))
00299                     {
00300                         solution = solCheck[i];
00301                     }
00302                 }
00303                 else if (!solution && dist < approximatedist)
00304                 {
00305                     approximation = solCheck[i];
00306                     approximatedist = dist;
00307                 }
00308             }
00309         }
00310 
00311         // terminate if a sufficient solution is found
00312         if (solution && sufficientlyShort)
00313             break;
00314     }
00315 
00316     double solutionCost;
00317     bool approximate = (solution == NULL);
00318     bool addedSolution = false;
00319     if (approximate)
00320     {
00321         solution = approximation;
00322         solutionCost = approximatedist;
00323     }
00324     else
00325         solutionCost = solution->cost;
00326 
00327     if (solution != NULL)
00328     {
00329         // construct the solution path
00330         std::vector<Motion*> mpath;
00331         while (solution != NULL)
00332         {
00333             mpath.push_back(solution);
00334             solution = solution->parent;
00335         }
00336 
00337         // set the solution path
00338         PathGeometric *path = new PathGeometric(si_);
00339         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00340             path->append(mpath[i]->state);
00341         goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00342         addedSolution = true;
00343     }
00344 
00345     si_->freeState(xstate);
00346     if (rmotion->state)
00347         si_->freeState(rmotion->state);
00348     delete rmotion;
00349 
00350     msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00351 
00352     return addedSolution;
00353 }
00354 
00355 void ompl::geometric::RRTstar::removeFromParent(Motion *m)
00356 {
00357     std::vector<Motion*>::iterator it = m->parent->children.begin ();
00358     while (it != m->parent->children.end ())
00359     {
00360         if (*it == m)
00361         {
00362             it = m->parent->children.erase(it);
00363             it = m->parent->children.end ();
00364         }
00365         else
00366             ++it;
00367     }
00368 }
00369 
00370 void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
00371 {
00372     for (size_t i = 0; i < m->children.size(); ++i)
00373     {
00374         m->children[i]->cost += delta;
00375         updateChildCosts(m->children[i], delta);
00376     }
00377 }
00378 
00379 void ompl::geometric::RRTstar::freeMemory(void)
00380 {
00381     if (nn_)
00382     {
00383         std::vector<Motion*> motions;
00384         nn_->list(motions);
00385         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00386         {
00387             if (motions[i]->state)
00388                 si_->freeState(motions[i]->state);
00389             delete motions[i];
00390         }
00391     }
00392 }
00393 
00394 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00395 {
00396     Planner::getPlannerData(data);
00397 
00398     std::vector<Motion*> motions;
00399     if (nn_)
00400         nn_->list(motions);
00401 
00402     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00403         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00404 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends