All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/contrib/rrt_star/src/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/goals/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 ompl::base::PlannerStatus 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         logError("Goal undefined");
00102         return base::PlannerStatus::INVALID_GOAL;
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         logError("There are no valid initial states!");
00115         return base::PlannerStatus::INVALID_START;
00116     }
00117 
00118     if (!sampler_)
00119         sampler_ = si_->allocStateSampler();
00120 
00121     logInform("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     double               stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
00137 
00138     while (ptc() == false)
00139     {
00140         // sample random state (with goal biasing)
00141         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00142             goal_s->sampleGoal(rstate);
00143         else
00144             sampler_->sampleUniform(rstate);
00145 
00146         // find closest state in the tree
00147         Motion *nmotion = nn_->nearest(rmotion);
00148 
00149         base::State *dstate = rstate;
00150 
00151         // find state to add
00152         double d = si_->distance(nmotion->state, rstate);
00153         if (d > maxDistance_)
00154         {
00155             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00156             dstate = xstate;
00157         }
00158 
00159         if (si_->checkMotion(nmotion->state, dstate))
00160         {
00161             // create a motion
00162             double distN = si_->distance(dstate, nmotion->state);
00163             Motion *motion = new Motion(si_);
00164             si_->copyState(motion->state, dstate);
00165             motion->parent = nmotion;
00166             motion->cost = nmotion->cost + distN;
00167 
00168             // find nearby neighbors
00169             double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
00170                                 ballRadiusMax_);
00171 
00172             nn_->nearestR(motion, r, nbh);
00173             rewireTest += nbh.size();
00174 
00175             // cache for distance computations
00176             dists.resize(nbh.size());
00177             // cache for motion validity
00178             valid.resize(nbh.size());
00179             std::fill(valid.begin(), valid.end(), 0);
00180 
00181             if(delayCC_)
00182             {
00183                 // calculate all costs and distances
00184                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00185                     nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
00186 
00187                 // sort the nodes
00188                 std::sort(nbh.begin(), nbh.end(), compareMotion);
00189 
00190                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00191                 {
00192                     dists[i] = si_->distance(nbh[i]->state, dstate);
00193                     nbh[i]->cost -= dists[i];
00194                 }
00195 
00196                 // collision check until a valid motion is found
00197                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00198                 {
00199                     if (nbh[i] != nmotion)
00200                     {
00201                         double c = nbh[i]->cost + dists[i];
00202                         if (c < motion->cost)
00203                         {
00204                             if (si_->checkMotion(nbh[i]->state, dstate))
00205                             {
00206                                 motion->cost = c;
00207                                 motion->parent = nbh[i];
00208                                 valid[i] = 1;
00209                                 break;
00210                             }
00211                             else
00212                                 valid[i] = -1;
00213                         }
00214                     }
00215                     else
00216                     {
00217                         valid[i] = 1;
00218                         dists[i] = distN;
00219                         break;
00220                     }
00221                 }
00222             }
00223             else
00224             {
00225                 // find which one we connect the new state to
00226                 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00227                 {
00228                     if (nbh[i] != nmotion)
00229                     {
00230                         dists[i] = si_->distance(nbh[i]->state, dstate);
00231                         double c = nbh[i]->cost + dists[i];
00232                         if (c < motion->cost)
00233                         {
00234                             if (si_->checkMotion(nbh[i]->state, dstate))
00235                             {
00236                                 motion->cost = c;
00237                                 motion->parent = nbh[i];
00238                                 valid[i] = 1;
00239                             }
00240                             else
00241                                 valid[i] = -1;
00242                         }
00243                     }
00244                     else
00245                     {
00246                         valid[i] = 1;
00247                         dists[i] = distN;
00248                     }
00249                 }
00250             }
00251 
00252             // add motion to the tree
00253             nn_->add(motion);
00254             motion->parent->children.push_back(motion);
00255 
00256             solCheck.resize(1);
00257             solCheck[0] = motion;
00258 
00259             // rewire tree if needed
00260             for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00261                 if (nbh[i] != motion->parent)
00262                 {
00263                     double c = motion->cost + dists[i];
00264                     if (c < nbh[i]->cost)
00265                     {
00266                         bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00267                         if (v)
00268                         {
00269                             // Remove this node from its parent list
00270                             removeFromParent (nbh[i]);
00271                             double delta = c - nbh[i]->cost;
00272 
00273                             // Add this node to the new parent
00274                             nbh[i]->parent = motion;
00275                             nbh[i]->cost = c;
00276                             nbh[i]->parent->children.push_back(nbh[i]);
00277                             solCheck.push_back(nbh[i]);
00278 
00279                             // Update the costs of the node's children
00280                             updateChildCosts(nbh[i], delta);
00281                         }
00282                     }
00283                 }
00284 
00285             // Make sure to check the existing solution for improvement
00286             if (solution)
00287                 solCheck.push_back(solution);
00288 
00289             // check if we found a solution
00290             for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00291             {
00292                 double dist = 0.0;
00293                 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00294                 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00295 
00296                 if (solved)
00297                 {
00298                     if (sufficientlyShort)
00299                     {
00300                         solution = solCheck[i];
00301                         break;
00302                     }
00303                     else if (!solution || (solCheck[i]->cost < solution->cost))
00304                     {
00305                         solution = solCheck[i];
00306                     }
00307                 }
00308                 else if (!solution && dist < approximatedist)
00309                 {
00310                     approximation = solCheck[i];
00311                     approximatedist = dist;
00312                 }
00313             }
00314         }
00315 
00316         // terminate if a sufficient solution is found
00317         if (solution && sufficientlyShort)
00318             break;
00319     }
00320 
00321     double solutionCost;
00322     bool approximate = (solution == NULL);
00323     bool addedSolution = false;
00324     if (approximate)
00325     {
00326         solution = approximation;
00327         solutionCost = approximatedist;
00328     }
00329     else
00330         solutionCost = solution->cost;
00331 
00332     if (solution != NULL)
00333     {
00334         // construct the solution path
00335         std::vector<Motion*> mpath;
00336         while (solution != NULL)
00337         {
00338             mpath.push_back(solution);
00339             solution = solution->parent;
00340         }
00341 
00342         // set the solution path
00343         PathGeometric *path = new PathGeometric(si_);
00344         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00345             path->append(mpath[i]->state);
00346         pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00347         addedSolution = true;
00348     }
00349 
00350     si_->freeState(xstate);
00351     if (rmotion->state)
00352         si_->freeState(rmotion->state);
00353     delete rmotion;
00354 
00355     logInform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00356 
00357     return base::PlannerStatus(addedSolution, approximate);
00358 }
00359 
00360 void ompl::geometric::RRTstar::removeFromParent(Motion *m)
00361 {
00362     std::vector<Motion*>::iterator it = m->parent->children.begin ();
00363     while (it != m->parent->children.end ())
00364     {
00365         if (*it == m)
00366         {
00367             it = m->parent->children.erase(it);
00368             it = m->parent->children.end ();
00369         }
00370         else
00371             ++it;
00372     }
00373 }
00374 
00375 void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
00376 {
00377     for (size_t i = 0; i < m->children.size(); ++i)
00378     {
00379         m->children[i]->cost += delta;
00380         updateChildCosts(m->children[i], delta);
00381     }
00382 }
00383 
00384 void ompl::geometric::RRTstar::freeMemory(void)
00385 {
00386     if (nn_)
00387     {
00388         std::vector<Motion*> motions;
00389         nn_->list(motions);
00390         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00391         {
00392             if (motions[i]->state)
00393                 si_->freeState(motions[i]->state);
00394             delete motions[i];
00395         }
00396     }
00397 }
00398 
00399 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00400 {
00401     Planner::getPlannerData(data);
00402 
00403     std::vector<Motion*> motions;
00404     if (nn_)
00405         nn_->list(motions);
00406 
00407     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00408         data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
00409                       base::PlannerDataVertex (motions[i]->state));
00410 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines