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 }