ompl/geometric/planners/rrt/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, Ryan Luna, Luis G. Torres, Ioan Sucan, Javier V Gomez */
00036 
00037 #include "ompl/geometric/planners/rrt/RRTstar.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00041 #include <algorithm>
00042 #include <limits>
00043 #include <map>
00044 #include <queue>
00045 #include <boost/math/constants/constants.hpp>
00046 
00047 ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
00048     base::Planner(si, "RRTstar"),
00049     goalBias_(0.05),
00050     maxDistance_(0.0),
00051     delayCC_(true),
00052     lastGoalMotion_(NULL),
00053     prune_(false),
00054     pruneStatesThreshold_(0.95),
00055     iterations_(0),
00056     bestCost_(std::numeric_limits<double>::quiet_NaN())
00057 {
00058     specs_.approximateSolutions = true;
00059     specs_.optimizingPaths = true;
00060     specs_.canReportIntermediateSolutions = true;
00061 
00062     Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
00063     Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
00064     Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
00065     Planner::declareParam<bool>("prune", this, &RRTstar::setPrune, &RRTstar::getPrune, "0,1");
00066     Planner::declareParam<double>("prune_states_threshold", this, &RRTstar::setPruneStatesImprovementThreshold, &RRTstar::getPruneStatesImprovementThreshold, "0.:.01:1.");
00067 
00068     addPlannerProgressProperty("iterations INTEGER",
00069                                boost::bind(&RRTstar::getIterationCount, this));
00070     addPlannerProgressProperty("best cost REAL",
00071                                boost::bind(&RRTstar::getBestCost, this));
00072 }
00073 
00074 ompl::geometric::RRTstar::~RRTstar()
00075 {
00076     freeMemory();
00077 }
00078 
00079 void ompl::geometric::RRTstar::setup()
00080 {
00081     Planner::setup();
00082     tools::SelfConfig sc(si_, getName());
00083     sc.configurePlannerRange(maxDistance_);
00084     if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
00085     {
00086         OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
00087     }
00088 
00089     if (!nn_)
00090         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
00091     nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
00092 
00093     // Setup optimization objective
00094     //
00095     // If no optimization objective was specified, then default to
00096     // optimizing path length as computed by the distance() function
00097     // in the state space.
00098     if (pdef_)
00099     {
00100         if (pdef_->hasOptimizationObjective())
00101             opt_ = pdef_->getOptimizationObjective();
00102         else
00103         {
00104             OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
00105             opt_.reset(new base::PathLengthOptimizationObjective(si_));
00106         }
00107     }
00108     else
00109     {
00110         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
00111         setup_ = false;
00112     }
00113 }
00114 
00115 void ompl::geometric::RRTstar::clear()
00116 {
00117     Planner::clear();
00118     sampler_.reset();
00119     freeMemory();
00120     if (nn_)
00121         nn_->clear();
00122 
00123     lastGoalMotion_ = NULL;
00124     goalMotions_.clear();
00125 
00126     iterations_ = 0;
00127     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00128 }
00129 
00130 ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
00131 {
00132     checkValidity();
00133     base::Goal                  *goal   = pdef_->getGoal().get();
00134     base::GoalSampleableRegion  *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00135 
00136     bool symCost = opt_->isSymmetric();
00137 
00138     while (const base::State *st = pis_.nextStart())
00139     {
00140         Motion *motion = new Motion(si_);
00141         si_->copyState(motion->state, st);
00142         motion->cost = opt_->identityCost();
00143         nn_->add(motion);
00144         startMotion_ = motion;
00145     }
00146 
00147     if (nn_->size() == 0)
00148     {
00149         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00150         return base::PlannerStatus::INVALID_START;
00151     }
00152 
00153     if (!sampler_)
00154         sampler_ = si_->allocStateSampler();
00155 
00156     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
00157 
00158     if (prune_ && !si_->getStateSpace()->isMetricSpace())
00159         OMPL_WARN("%s: tree pruning was activated but since the state space %s is not a metric space, "
00160                   "the optimization objective might not satisfy the triangle inequality. You may need to disable pruning."
00161                   , getName().c_str(), si_->getStateSpace()->getName().c_str());
00162 
00163     const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
00164 
00165     Motion *solution       = lastGoalMotion_;
00166 
00167     // \TODO Make this variable unnecessary, or at least have it
00168     // persist across solve runs
00169     base::Cost bestCost    = opt_->infiniteCost();
00170 
00171     bestCost_ = opt_->infiniteCost();
00172 
00173     Motion *approximation  = NULL;
00174     double approximatedist = std::numeric_limits<double>::infinity();
00175     bool sufficientlyShort = false;
00176 
00177     Motion *rmotion        = new Motion(si_);
00178     base::State *rstate    = rmotion->state;
00179     base::State *xstate    = si_->allocState();
00180 
00181     // e+e/d.  K-nearest RRT*
00182     double k_rrg           = boost::math::constants::e<double>() +
00183                              (boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension());
00184 
00185     std::vector<Motion*>       nbh;
00186 
00187     std::vector<base::Cost>    costs;
00188     std::vector<base::Cost>    incCosts;
00189     std::vector<std::size_t>   sortedCostIndices;
00190 
00191     std::vector<int>           valid;
00192     unsigned int               rewireTest = 0;
00193     unsigned int               statesGenerated = 0;
00194 
00195     if (solution)
00196         OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(), solution->cost.value());
00197     OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(), (unsigned int)std::ceil(k_rrg * log((double)(nn_->size() + 1))));
00198 
00199     // our functor for sorting nearest neighbors
00200     CostIndexCompare compareFn(costs, *opt_);
00201 
00202     while (ptc == false)
00203     {
00204         iterations_++;
00205 
00206         // sample random state (with goal biasing)
00207         // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
00208         if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
00209             goal_s->sampleGoal(rstate);
00210         else
00211         {
00212             sampler_->sampleUniform(rstate);
00213 
00214             if (prune_ && opt_->isCostBetterThan(bestCost_, costToGo(rmotion)))
00215                 continue;
00216         }
00217 
00218         // find closest state in the tree
00219         Motion *nmotion = nn_->nearest(rmotion);
00220 
00221         if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
00222             continue;
00223 
00224         base::State *dstate = rstate;
00225 
00226         // find state to add to the tree
00227         double d = si_->distance(nmotion->state, rstate);
00228         if (d > maxDistance_)
00229         {
00230             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00231             dstate = xstate;
00232         }
00233 
00234         // Check if the motion between the nearest state and the state to add is valid
00235         if (si_->checkMotion(nmotion->state, dstate))
00236         {
00237             // create a motion
00238             Motion *motion = new Motion(si_);
00239             si_->copyState(motion->state, dstate);
00240             motion->parent = nmotion;
00241             motion->incCost = opt_->motionCost(nmotion->state, motion->state);
00242             motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
00243 
00244             // Find nearby neighbors of the new motion - k-nearest RRT*
00245             unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
00246             nn_->nearestK(motion, k, nbh);
00247             rewireTest += nbh.size();
00248             statesGenerated++;
00249 
00250             // cache for distance computations
00251             //
00252             // Our cost caches only increase in size, so they're only
00253             // resized if they can't fit the current neighborhood
00254             if (costs.size() < nbh.size())
00255             {
00256                 costs.resize(nbh.size());
00257                 incCosts.resize(nbh.size());
00258                 sortedCostIndices.resize(nbh.size());
00259             }
00260 
00261             // cache for motion validity (only useful in a symmetric space)
00262             //
00263             // Our validity caches only increase in size, so they're
00264             // only resized if they can't fit the current neighborhood
00265             if (valid.size() < nbh.size())
00266                 valid.resize(nbh.size());
00267             std::fill(valid.begin(), valid.begin() + nbh.size(), 0);
00268 
00269             // Finding the nearest neighbor to connect to
00270             // By default, neighborhood states are sorted by cost, and collision checking
00271             // is performed in increasing order of cost
00272             if (delayCC_)
00273             {
00274                 // calculate all costs and distances
00275                 for (std::size_t i = 0 ; i < nbh.size(); ++i)
00276                 {
00277                     incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
00278                     costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
00279                 }
00280 
00281                 // sort the nodes
00282                 //
00283                 // we're using index-value pairs so that we can get at
00284                 // original, unsorted indices
00285                 for (std::size_t i = 0; i < nbh.size(); ++i)
00286                     sortedCostIndices[i] = i;
00287                 std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + nbh.size(),
00288                           compareFn);
00289 
00290                 // collision check until a valid motion is found
00291                 //
00292                 // ASYMMETRIC CASE: it's possible that none of these
00293                 // neighbors are valid. This is fine, because motion
00294                 // already has a connection to the tree through
00295                 // nmotion (with populated cost fields!).
00296                 for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
00297                      i != sortedCostIndices.begin() + nbh.size();
00298                      ++i)
00299                 {
00300                     if (nbh[*i] == nmotion || si_->checkMotion(nbh[*i]->state, motion->state))
00301                     {
00302                         motion->incCost = incCosts[*i];
00303                         motion->cost = costs[*i];
00304                         motion->parent = nbh[*i];
00305                         valid[*i] = 1;
00306                         break;
00307                     }
00308                     else valid[*i] = -1;
00309                 }
00310             }
00311             else // if not delayCC
00312             {
00313                 motion->incCost = opt_->motionCost(nmotion->state, motion->state);
00314                 motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
00315                 // find which one we connect the new state to
00316                 for (std::size_t i = 0 ; i < nbh.size(); ++i)
00317                 {
00318                     if (nbh[i] != nmotion)
00319                     {
00320                         incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
00321                         costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
00322                         if (opt_->isCostBetterThan(costs[i], motion->cost))
00323                         {
00324                             if (si_->checkMotion(nbh[i]->state, motion->state))
00325                             {
00326                                 motion->incCost = incCosts[i];
00327                                 motion->cost = costs[i];
00328                                 motion->parent = nbh[i];
00329                                 valid[i] = 1;
00330                             }
00331                             else valid[i] = -1;
00332                         }
00333                     }
00334                     else
00335                     {
00336                         incCosts[i] = motion->incCost;
00337                         costs[i] = motion->cost;
00338                         valid[i] = 1;
00339                     }
00340                 }
00341             }
00342 
00343             if (prune_)
00344             {
00345                 if (opt_->isCostBetterThan(costToGo(motion, false), bestCost_))
00346                 {
00347                     nn_->add(motion);
00348                     motion->parent->children.push_back(motion);
00349                 }
00350                 else // If the new motion does not improve the best cost it is ignored.
00351                 {
00352                     --statesGenerated;
00353                     si_->freeState(motion->state);
00354                     delete motion;
00355                     continue;
00356                 }
00357             }
00358             else
00359             {
00360                 // add motion to the tree
00361                 nn_->add(motion);
00362                 motion->parent->children.push_back(motion);
00363             }
00364 
00365             bool checkForSolution = false;
00366             for (std::size_t i = 0; i < nbh.size(); ++i)
00367             {
00368                 if (nbh[i] != motion->parent)
00369                 {
00370                     base::Cost nbhIncCost;
00371                     if (symCost)
00372                         nbhIncCost = incCosts[i];
00373                     else
00374                         nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state);
00375                     base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost);
00376                     if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost))
00377                     {
00378                         bool motionValid;
00379                         if (valid[i] == 0)
00380                             motionValid = si_->checkMotion(motion->state, nbh[i]->state);
00381                         else
00382                             motionValid = (valid[i] == 1);
00383 
00384                         if (motionValid)
00385                         {
00386                             // Remove this node from its parent list
00387                             removeFromParent (nbh[i]);
00388 
00389                             // Add this node to the new parent
00390                             nbh[i]->parent = motion;
00391                             nbh[i]->incCost = nbhIncCost;
00392                             nbh[i]->cost = nbhNewCost;
00393                             nbh[i]->parent->children.push_back(nbh[i]);
00394 
00395                             // Update the costs of the node's children
00396                             updateChildCosts(nbh[i]);
00397 
00398                             checkForSolution = true;
00399                         }
00400                     }
00401                 }
00402             }
00403 
00404             // Add the new motion to the goalMotion_ list, if it satisfies the goal
00405             double distanceFromGoal;
00406             if (goal->isSatisfied(motion->state, &distanceFromGoal))
00407             {
00408                 goalMotions_.push_back(motion);
00409                 checkForSolution = true;
00410             }
00411 
00412             // Checking for solution or iterative improvement
00413             if (checkForSolution)
00414             {
00415                 bool updatedSolution = false;
00416                 for (size_t i = 0; i < goalMotions_.size(); ++i)
00417                 {
00418                     if (opt_->isCostBetterThan(goalMotions_[i]->cost, bestCost))
00419                     {
00420                         bestCost = goalMotions_[i]->cost;
00421                         bestCost_ = bestCost;
00422                         updatedSolution = true;
00423                     }
00424 
00425                     sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->cost);
00426                     if (sufficientlyShort)
00427                      {
00428                          solution = goalMotions_[i];
00429                          break;
00430                      }
00431                      else if (!solution ||
00432                          opt_->isCostBetterThan(goalMotions_[i]->cost,solution->cost))
00433                     {
00434                         solution = goalMotions_[i];
00435                         updatedSolution = true;
00436                     }
00437                 }
00438 
00439                 if (updatedSolution)
00440                 {
00441                     if (prune_)
00442                     {
00443                         int n = pruneTree(bestCost_);
00444                         statesGenerated -= n;
00445                     }
00446 
00447                     if (intermediateSolutionCallback)
00448                     {
00449                         std::vector<const base::State *> spath;
00450                         Motion *intermediate_solution = solution->parent; // Do not include goal state to simplify code.
00451 
00452                         do
00453                         {
00454                             spath.push_back(intermediate_solution->state);
00455                             intermediate_solution = intermediate_solution->parent;
00456                         } while (intermediate_solution->parent != 0); // Do not include the start state.
00457 
00458                         intermediateSolutionCallback(this, spath, bestCost_);
00459                     }
00460                 }
00461             }
00462 
00463             // Checking for approximate solution (closest state found to the goal)
00464             if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
00465             {
00466                 approximation = motion;
00467                 approximatedist = distanceFromGoal;
00468             }
00469         }
00470 
00471         // terminate if a sufficient solution is found
00472         if (solution && sufficientlyShort)
00473             break;
00474     }
00475 
00476     bool approximate = (solution == NULL);
00477     bool addedSolution = false;
00478     if (approximate)
00479         solution = approximation;
00480     else
00481         lastGoalMotion_ = solution;
00482 
00483     if (solution != NULL)
00484     {
00485         ptc.terminate();
00486         // construct the solution path
00487         std::vector<Motion*> mpath;
00488         while (solution != NULL)
00489         {
00490             mpath.push_back(solution);
00491             solution = solution->parent;
00492         }
00493 
00494         // set the solution path
00495         PathGeometric *geoPath = new PathGeometric(si_);
00496         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00497             geoPath->append(mpath[i]->state);
00498 
00499         base::PathPtr path(geoPath);
00500         // Add the solution path.
00501         base::PlannerSolution psol(path);
00502         psol.setPlannerName(getName());
00503         if (approximate)
00504             psol.setApproximate(approximatedist);
00505         // Does the solution satisfy the optimization objective?
00506         psol.setOptimized(opt_, bestCost, sufficientlyShort);
00507         pdef_->addSolutionPath(psol);
00508 
00509         addedSolution = true;
00510     }
00511 
00512     si_->freeState(xstate);
00513     if (rmotion->state)
00514         si_->freeState(rmotion->state);
00515     delete rmotion;
00516 
00517     OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree.", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size());
00518 
00519     return base::PlannerStatus(addedSolution, approximate);
00520 }
00521 
00522 void ompl::geometric::RRTstar::removeFromParent(Motion *m)
00523 {
00524     for (std::vector<Motion*>::iterator it = m->parent->children.begin ();
00525         it != m->parent->children.end (); ++it)
00526         if (*it == m)
00527         {
00528             m->parent->children.erase(it);
00529             break;
00530         }
00531 }
00532 
00533 void ompl::geometric::RRTstar::updateChildCosts(Motion *m)
00534 {
00535     for (std::size_t i = 0; i < m->children.size(); ++i)
00536     {
00537         m->children[i]->cost = opt_->combineCosts(m->cost, m->children[i]->incCost);
00538         updateChildCosts(m->children[i]);
00539     }
00540 }
00541 
00542 void ompl::geometric::RRTstar::freeMemory()
00543 {
00544     if (nn_)
00545     {
00546         std::vector<Motion*> motions;
00547         nn_->list(motions);
00548         for (std::size_t i = 0 ; i < motions.size() ; ++i)
00549         {
00550             if (motions[i]->state)
00551                 si_->freeState(motions[i]->state);
00552             delete motions[i];
00553         }
00554     }
00555 }
00556 
00557 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00558 {
00559     Planner::getPlannerData(data);
00560 
00561     std::vector<Motion*> motions;
00562     if (nn_)
00563         nn_->list(motions);
00564 
00565     if (lastGoalMotion_)
00566         data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
00567 
00568     for (std::size_t i = 0 ; i < motions.size() ; ++i)
00569     {
00570         if (motions[i]->parent == NULL)
00571             data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
00572         else
00573             data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
00574                          base::PlannerDataVertex(motions[i]->state));
00575     }
00576 }
00577 
00578 int ompl::geometric::RRTstar::pruneTree(const base::Cost pruneTreeCost)
00579 {
00580     const int tree_size = nn_->size();
00581     pruneScratchSpace_.newTree.reserve(tree_size);
00582     pruneScratchSpace_.newTree.clear();
00583     pruneScratchSpace_.toBePruned.reserve(tree_size);
00584     pruneScratchSpace_.toBePruned.clear();
00585     pruneScratchSpace_.candidates.clear();
00586     pruneScratchSpace_.candidates.push_back(startMotion_);
00587     std::size_t j = 0;
00588     while (j != pruneScratchSpace_.candidates.size())
00589     {
00590         Motion *candidate = pruneScratchSpace_.candidates[j++];
00591         if (opt_->isCostBetterThan(pruneTreeCost, costToGo(candidate)))
00592             pruneScratchSpace_.toBePruned.push_back(candidate);
00593         else
00594         {
00595             pruneScratchSpace_.newTree.push_back(candidate);
00596             pruneScratchSpace_.candidates.insert(pruneScratchSpace_.candidates.end(),
00597                 candidate->children.begin(), candidate->children.end());
00598         }
00599     }
00600 
00601     // To create the new nn takes one order of magnitude in time more than just checking how many
00602     // states would be pruned. Therefore, only prune if it removes a significant amount of states.
00603     if ((double)pruneScratchSpace_.newTree.size() / tree_size < pruneStatesThreshold_)
00604     {
00605         for (std::size_t i = 0; i < pruneScratchSpace_.toBePruned.size(); ++i)
00606             deleteBranch(pruneScratchSpace_.toBePruned[i]);
00607 
00608         nn_->clear();
00609         nn_->add(pruneScratchSpace_.newTree);
00610 
00611         return (tree_size - pruneScratchSpace_.newTree.size());
00612     }
00613     return 0;
00614 }
00615 
00616 void ompl::geometric::RRTstar::deleteBranch(Motion *motion)
00617 {
00618     removeFromParent(motion);
00619 
00620     std::vector<Motion *>& toDelete = pruneScratchSpace_.candidates;
00621     toDelete.clear();
00622     toDelete.push_back(motion);
00623 
00624     while (!toDelete.empty())
00625     {
00626         Motion *mto_delete = toDelete.back();
00627         toDelete.pop_back();
00628 
00629         for(std::size_t i = 0; i < mto_delete->children.size(); ++i)
00630             toDelete.push_back(mto_delete->children[i]);
00631 
00632         si_->freeState(mto_delete->state);
00633         delete mto_delete;
00634     }
00635 }
00636 
00637 ompl::base::Cost ompl::geometric::RRTstar::costToGo(const Motion *motion, const bool shortest) const
00638 {
00639     base::Cost costToCome;
00640     if (shortest)
00641         costToCome = opt_->motionCost(startMotion_->state, motion->state); // h_s
00642     else
00643         costToCome = motion->cost; //d_s
00644 
00645     const base::Cost costToGo = base::goalRegionCostToGo(motion->state, pdef_->getGoal().get()); // h_g
00646     return opt_->combineCosts(costToCome, costToGo); // h_s + h_g
00647 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines