Loading...
Searching...
No Matches
RRTstar.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Luis G. Torres, Ioan Sucan, Javier V Gomez, Jonathan Gammell */
36
37#include "ompl/geometric/planners/rrt/RRTstar.h"
38#include <algorithm>
39#include <boost/math/constants/constants.hpp>
40#include <limits>
41#include <vector>
42#include "ompl/base/Goal.h"
43#include "ompl/base/goals/GoalSampleableRegion.h"
44#include "ompl/base/goals/GoalState.h"
45#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
46#include "ompl/base/samplers/InformedStateSampler.h"
47#include "ompl/base/samplers/informed/RejectionInfSampler.h"
48#include "ompl/base/samplers/informed/OrderedInfSampler.h"
49#include "ompl/tools/config/SelfConfig.h"
50#include "ompl/util/GeometricEquations.h"
51
52ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si)
53 : base::Planner(si, "RRTstar")
54{
55 specs_.approximateSolutions = true;
56 specs_.optimizingPaths = true;
57 specs_.canReportIntermediateSolutions = true;
58
59 Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
60 Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
61 Planner::declareParam<double>("rewire_factor", this, &RRTstar::setRewireFactor, &RRTstar::getRewireFactor,
62 "1.0:0.01:2.0");
63 Planner::declareParam<bool>("use_k_nearest", this, &RRTstar::setKNearest, &RRTstar::getKNearest, "0,1");
64 Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
65 Planner::declareParam<bool>("tree_pruning", this, &RRTstar::setTreePruning, &RRTstar::getTreePruning, "0,1");
66 Planner::declareParam<double>("prune_threshold", this, &RRTstar::setPruneThreshold, &RRTstar::getPruneThreshold,
67 "0.:.01:1.");
68 Planner::declareParam<bool>("pruned_measure", this, &RRTstar::setPrunedMeasure, &RRTstar::getPrunedMeasure, "0,1");
69 Planner::declareParam<bool>("informed_sampling", this, &RRTstar::setInformedSampling, &RRTstar::getInformedSampling,
70 "0,1");
71 Planner::declareParam<bool>("sample_rejection", this, &RRTstar::setSampleRejection, &RRTstar::getSampleRejection,
72 "0,1");
73 Planner::declareParam<bool>("new_state_rejection", this, &RRTstar::setNewStateRejection,
74 &RRTstar::getNewStateRejection, "0,1");
75 Planner::declareParam<bool>("use_admissible_heuristic", this, &RRTstar::setAdmissibleCostToCome,
76 &RRTstar::getAdmissibleCostToCome, "0,1");
77 Planner::declareParam<bool>("ordered_sampling", this, &RRTstar::setOrderedSampling, &RRTstar::getOrderedSampling,
78 "0,1");
79 Planner::declareParam<unsigned int>("ordering_batch_size", this, &RRTstar::setBatchSize, &RRTstar::getBatchSize,
80 "1:100:1000000");
81 Planner::declareParam<bool>("focus_search", this, &RRTstar::setFocusSearch, &RRTstar::getFocusSearch, "0,1");
82 Planner::declareParam<unsigned int>("number_sampling_attempts", this, &RRTstar::setNumSamplingAttempts,
83 &RRTstar::getNumSamplingAttempts, "10:10:100000");
84
85 addPlannerProgressProperty("iterations INTEGER", [this] { return numIterationsProperty(); });
86 addPlannerProgressProperty("best cost REAL", [this] { return bestCostProperty(); });
87}
88
89ompl::geometric::RRTstar::~RRTstar()
90{
91 freeMemory();
92}
93
95{
96 Planner::setup();
97 tools::SelfConfig sc(si_, getName());
98 sc.configurePlannerRange(maxDistance_);
99 if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
100 {
101 OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
102 }
103
104 if (!nn_)
106 nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
107
108 // Setup optimization objective
109 //
110 // If no optimization objective was specified, then default to
111 // optimizing path length as computed by the distance() function
112 // in the state space.
113 if (pdef_)
114 {
115 if (pdef_->hasOptimizationObjective())
116 opt_ = pdef_->getOptimizationObjective();
117 else
118 {
119 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
120 "planning time.",
121 getName().c_str());
122 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
123
124 // Store the new objective in the problem def'n
125 pdef_->setOptimizationObjective(opt_);
126 }
127
128 // Set the bestCost_ and prunedCost_ as infinite
129 bestCost_ = opt_->infiniteCost();
130 prunedCost_ = opt_->infiniteCost();
131 }
132 else
133 {
134 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
135 setup_ = false;
136 }
137
138 // Get the measure of the entire space:
139 prunedMeasure_ = si_->getSpaceMeasure();
140
141 // Calculate some constants:
142 calculateRewiringLowerBounds();
143}
144
146{
147 setup_ = false;
148 Planner::clear();
149 sampler_.reset();
150 infSampler_.reset();
151 freeMemory();
152 if (nn_)
153 nn_->clear();
154
155 bestGoalMotion_ = nullptr;
156 goalMotions_.clear();
157 startMotions_.clear();
158
159 iterations_ = 0;
160 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
161 prunedCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
162 prunedMeasure_ = 0.0;
163}
164
166{
167 checkValidity();
168 base::Goal *goal = pdef_->getGoal().get();
169 auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
170
171 bool symCost = opt_->isSymmetric();
172
173 // Check if there are more starts
174 if (pis_.haveMoreStartStates() == true)
175 {
176 // There are, add them
177 while (const base::State *st = pis_.nextStart())
178 {
179 auto *motion = new Motion(si_);
180 si_->copyState(motion->state, st);
181 motion->cost = opt_->identityCost();
182 nn_->add(motion);
183 startMotions_.push_back(motion);
184 }
185
186 // And assure that, if we're using an informed sampler, it's reset
187 infSampler_.reset();
188 }
189 // No else
190
191 if (nn_->size() == 0)
192 {
193 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
195 }
196
197 // Allocate a sampler if necessary
198 if (!sampler_ && !infSampler_)
199 {
200 allocSampler();
201 }
202
203 OMPL_INFORM("%s: Started planning with %u states. Seeking a solution better than %.5f.", getName().c_str(), nn_->size(), opt_->getCostThreshold().value());
204
205 if ((useTreePruning_ || useRejectionSampling_ || useInformedSampling_ || useNewStateRejection_) &&
206 !si_->getStateSpace()->isMetricSpace())
207 OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
208 "the triangle inequality. "
209 "You may need to disable pruning or rejection.",
210 getName().c_str(), si_->getStateSpace()->getName().c_str());
211
212 const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
213
214 Motion *approxGoalMotion = nullptr;
215 double approxDist = std::numeric_limits<double>::infinity();
216
217 auto *rmotion = new Motion(si_);
218 base::State *rstate = rmotion->state;
219 base::State *xstate = si_->allocState();
220
221 std::vector<Motion *> nbh;
222
223 std::vector<base::Cost> costs;
224 std::vector<base::Cost> incCosts;
225 std::vector<std::size_t> sortedCostIndices;
226
227 std::vector<int> valid;
228 unsigned int rewireTest = 0;
229 unsigned int statesGenerated = 0;
230
231 if (bestGoalMotion_)
232 OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
233 bestCost_.value());
234
235 if (useKNearest_)
236 OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
237 (unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
238 else
240 "%s: Initial rewiring radius of %.2f", getName().c_str(),
241 std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
242 1 / (double)(si_->getStateDimension()))));
243
244 // our functor for sorting nearest neighbors
245 CostIndexCompare compareFn(costs, *opt_);
246
247 while (ptc == false)
248 {
249 iterations_++;
250
251 // sample random state (with goal biasing)
252 // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
253 // states.
254 if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
255 goal_s->canSample())
256 goal_s->sampleGoal(rstate);
257 else
258 {
259 // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
260 // loop and return to try again
261 if (!sampleUniform(rstate))
262 continue;
263 }
264
265 // find closest state in the tree
266 Motion *nmotion = nn_->nearest(rmotion);
267
268 if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
269 continue;
270
271 base::State *dstate = rstate;
272
273 // find state to add to the tree
274 double d = si_->distance(nmotion->state, rstate);
275 if (d > maxDistance_)
276 {
277 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
278 dstate = xstate;
279 }
280
281 // Check if the motion between the nearest state and the state to add is valid
282 if (si_->checkMotion(nmotion->state, dstate))
283 {
284 // create a motion
285 auto *motion = new Motion(si_);
286 si_->copyState(motion->state, dstate);
287 motion->parent = nmotion;
288 motion->incCost = opt_->motionCost(nmotion->state, motion->state);
289 motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
290
291 // Find nearby neighbors of the new motion
292 getNeighbors(motion, nbh);
293
294 rewireTest += nbh.size();
295 ++statesGenerated;
296
297 // cache for distance computations
298 //
299 // Our cost caches only increase in size, so they're only
300 // resized if they can't fit the current neighborhood
301 if (costs.size() < nbh.size())
302 {
303 costs.resize(nbh.size());
304 incCosts.resize(nbh.size());
305 sortedCostIndices.resize(nbh.size());
306 }
307
308 // cache for motion validity (only useful in a symmetric space)
309 //
310 // Our validity caches only increase in size, so they're
311 // only resized if they can't fit the current neighborhood
312 if (valid.size() < nbh.size())
313 valid.resize(nbh.size());
314 std::fill(valid.begin(), valid.begin() + nbh.size(), 0);
315
316 // Finding the nearest neighbor to connect to
317 // By default, neighborhood states are sorted by cost, and collision checking
318 // is performed in increasing order of cost
319 if (delayCC_)
320 {
321 // calculate all costs and distances
322 for (std::size_t i = 0; i < nbh.size(); ++i)
323 {
324 incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
325 costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
326 }
327
328 // sort the nodes
329 //
330 // we're using index-value pairs so that we can get at
331 // original, unsorted indices
332 for (std::size_t i = 0; i < nbh.size(); ++i)
333 sortedCostIndices[i] = i;
334 std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + nbh.size(), compareFn);
335
336 // collision check until a valid motion is found
337 //
338 // ASYMMETRIC CASE: it's possible that none of these
339 // neighbors are valid. This is fine, because motion
340 // already has a connection to the tree through
341 // nmotion (with populated cost fields!).
342 for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
343 i != sortedCostIndices.begin() + nbh.size(); ++i)
344 {
345 if (nbh[*i] == nmotion ||
346 ((!useKNearest_ || si_->distance(nbh[*i]->state, motion->state) < maxDistance_) &&
347 si_->checkMotion(nbh[*i]->state, motion->state)))
348 {
349 motion->incCost = incCosts[*i];
350 motion->cost = costs[*i];
351 motion->parent = nbh[*i];
352 valid[*i] = 1;
353 break;
354 }
355 else
356 valid[*i] = -1;
357 }
358 }
359 else // if not delayCC
360 {
361 motion->incCost = opt_->motionCost(nmotion->state, motion->state);
362 motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
363 // find which one we connect the new state to
364 for (std::size_t i = 0; i < nbh.size(); ++i)
365 {
366 if (nbh[i] != nmotion)
367 {
368 incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
369 costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
370 if (opt_->isCostBetterThan(costs[i], motion->cost))
371 {
372 if ((!useKNearest_ || si_->distance(nbh[i]->state, motion->state) < maxDistance_) &&
373 si_->checkMotion(nbh[i]->state, motion->state))
374 {
375 motion->incCost = incCosts[i];
376 motion->cost = costs[i];
377 motion->parent = nbh[i];
378 valid[i] = 1;
379 }
380 else
381 valid[i] = -1;
382 }
383 }
384 else
385 {
386 incCosts[i] = motion->incCost;
387 costs[i] = motion->cost;
388 valid[i] = 1;
389 }
390 }
391 }
392
393 if (useNewStateRejection_)
394 {
395 if (opt_->isCostBetterThan(solutionHeuristic(motion), bestCost_))
396 {
397 nn_->add(motion);
398 motion->parent->children.push_back(motion);
399 }
400 else // If the new motion does not improve the best cost it is ignored.
401 {
402 si_->freeState(motion->state);
403 delete motion;
404 continue;
405 }
406 }
407 else
408 {
409 // add motion to the tree
410 nn_->add(motion);
411 motion->parent->children.push_back(motion);
412 }
413
414 bool checkForSolution = false;
415 for (std::size_t i = 0; i < nbh.size(); ++i)
416 {
417 if (nbh[i] != motion->parent)
418 {
419 base::Cost nbhIncCost;
420 if (symCost)
421 nbhIncCost = incCosts[i];
422 else
423 nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state);
424 base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost);
425 if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost))
426 {
427 bool motionValid;
428 if (valid[i] == 0)
429 {
430 motionValid =
431 (!useKNearest_ || si_->distance(nbh[i]->state, motion->state) < maxDistance_) &&
432 si_->checkMotion(motion->state, nbh[i]->state);
433 }
434 else
435 {
436 motionValid = (valid[i] == 1);
437 }
438
439 if (motionValid)
440 {
441 // Remove this node from its parent list
442 removeFromParent(nbh[i]);
443
444 // Add this node to the new parent
445 nbh[i]->parent = motion;
446 nbh[i]->incCost = nbhIncCost;
447 nbh[i]->cost = nbhNewCost;
448 nbh[i]->parent->children.push_back(nbh[i]);
449
450 // Update the costs of the node's children
451 updateChildCosts(nbh[i]);
452
453 checkForSolution = true;
454 }
455 }
456 }
457 }
458
459 // Add the new motion to the goalMotion_ list, if it satisfies the goal
460 double distanceFromGoal;
461 if (goal->isSatisfied(motion->state, &distanceFromGoal))
462 {
463 motion->inGoal = true;
464 goalMotions_.push_back(motion);
465 checkForSolution = true;
466 }
467
468 // Checking for solution or iterative improvement
469 if (checkForSolution)
470 {
471 bool updatedSolution = false;
472 if (!bestGoalMotion_ && !goalMotions_.empty())
473 {
474 // We have found our first solution, store it as the best. We only add one
475 // vertex at a time, so there can only be one goal vertex at this moment.
476 bestGoalMotion_ = goalMotions_.front();
477 bestCost_ = bestGoalMotion_->cost;
478 updatedSolution = true;
479
480 OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
481 "vertices in the graph)",
482 getName().c_str(), bestCost_.value(), iterations_, nn_->size());
483 }
484 else
485 {
486 // We already have a solution, iterate through the list of goal vertices
487 // and see if there's any improvement.
488 for (auto &goalMotion : goalMotions_)
489 {
490 // Is this goal motion better than the (current) best?
491 if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
492 {
493 bestGoalMotion_ = goalMotion;
494 bestCost_ = bestGoalMotion_->cost;
495 updatedSolution = true;
496
497 // Check if it satisfies the optimization objective, if it does, break the for loop
498 if (opt_->isSatisfied(bestCost_))
499 {
500 break;
501 }
502 }
503 }
504 }
505
506 if (updatedSolution)
507 {
508 if (useTreePruning_)
509 {
510 pruneTree(bestCost_);
511 }
512
513 if (intermediateSolutionCallback)
514 {
515 std::vector<const base::State *> spath;
516 Motion *intermediate_solution =
517 bestGoalMotion_->parent; // Do not include goal state to simplify code.
518
519 // Push back until we find the start, but not the start itself
520 while (intermediate_solution->parent != nullptr)
521 {
522 spath.push_back(intermediate_solution->state);
523 intermediate_solution = intermediate_solution->parent;
524 }
525
526 intermediateSolutionCallback(this, spath, bestCost_);
527 }
528 }
529 }
530
531 // Checking for approximate solution (closest state found to the goal)
532 if (goalMotions_.size() == 0 && distanceFromGoal < approxDist)
533 {
534 approxGoalMotion = motion;
535 approxDist = distanceFromGoal;
536 }
537 }
538
539 // terminate if a sufficient solution is found
540 if (bestGoalMotion_ && opt_->isSatisfied(bestCost_))
541 break;
542 }
543
544 // Add our solution (if it exists)
545 Motion *newSolution = nullptr;
546 if (bestGoalMotion_)
547 {
548 // We have an exact solution
549 newSolution = bestGoalMotion_;
550 }
551 else if (approxGoalMotion)
552 {
553 // We don't have a solution, but we do have an approximate solution
554 newSolution = approxGoalMotion;
555 }
556 // No else, we have nothing
557
558 // Add what we found
559 if (newSolution)
560 {
561 ptc.terminate();
562 // construct the solution path
563 std::vector<Motion *> mpath;
564 Motion *iterMotion = newSolution;
565 while (iterMotion != nullptr)
566 {
567 mpath.push_back(iterMotion);
568 iterMotion = iterMotion->parent;
569 }
570
571 // set the solution path
572 auto path(std::make_shared<PathGeometric>(si_));
573 for (int i = mpath.size() - 1; i >= 0; --i)
574 path->append(mpath[i]->state);
575
576 // Add the solution path.
577 base::PlannerSolution psol(path);
578 psol.setPlannerName(getName());
579
580 // If we don't have a goal motion, the solution is approximate
581 if (!bestGoalMotion_)
582 psol.setApproximate(approxDist);
583
584 // Does the solution satisfy the optimization objective?
585 psol.setOptimized(opt_, newSolution->cost, opt_->isSatisfied(bestCost_));
586 pdef_->addSolutionPath(psol);
587 }
588 // No else, we have nothing
589
590 si_->freeState(xstate);
591 if (rmotion->state)
592 si_->freeState(rmotion->state);
593 delete rmotion;
594
595 OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
596 "%.3f",
597 getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
598
599 // We've added a solution if newSolution == true, and it is an approximate solution if bestGoalMotion_ == false
600 return {newSolution != nullptr, bestGoalMotion_ == nullptr};
601}
602
603void ompl::geometric::RRTstar::getNeighbors(Motion *motion, std::vector<Motion *> &nbh) const
604{
605 auto cardDbl = static_cast<double>(nn_->size() + 1u);
606 if (useKNearest_)
607 {
608 //- k-nearest RRT*
609 unsigned int k = std::ceil(k_rrt_ * log(cardDbl));
610 nn_->nearestK(motion, k, nbh);
611 }
612 else
613 {
614 double r = std::min(
615 maxDistance_, r_rrt_ * std::pow(log(cardDbl) / cardDbl, 1 / static_cast<double>(si_->getStateDimension())));
616 nn_->nearestR(motion, r, nbh);
617 }
618}
619
621{
622 for (auto it = m->parent->children.begin(); it != m->parent->children.end(); ++it)
623 {
624 if (*it == m)
625 {
626 m->parent->children.erase(it);
627 break;
628 }
629 }
630}
631
633{
634 for (std::size_t i = 0; i < m->children.size(); ++i)
635 {
636 m->children[i]->cost = opt_->combineCosts(m->cost, m->children[i]->incCost);
637 updateChildCosts(m->children[i]);
638 }
639}
640
642{
643 if (nn_)
644 {
645 std::vector<Motion *> motions;
646 nn_->list(motions);
647 for (auto &motion : motions)
648 {
649 if (motion->state)
650 si_->freeState(motion->state);
651 delete motion;
652 }
653 }
654}
655
657{
658 Planner::getPlannerData(data);
659
660 std::vector<Motion *> motions;
661 if (nn_)
662 nn_->list(motions);
663
664 if (bestGoalMotion_)
665 data.addGoalVertex(base::PlannerDataVertex(bestGoalMotion_->state));
666
667 for (auto &motion : motions)
668 {
669 if (motion->parent == nullptr)
670 data.addStartVertex(base::PlannerDataVertex(motion->state));
671 else
672 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
673 }
674}
675
677{
678 // Variable
679 // The percent improvement (expressed as a [0,1] fraction) in cost
680 double fracBetter;
681 // The number pruned
682 int numPruned = 0;
683
684 if (opt_->isFinite(prunedCost_))
685 {
686 fracBetter = std::abs((pruneTreeCost.value() - prunedCost_.value()) / prunedCost_.value());
687 }
688 else
689 {
690 fracBetter = 1.0;
691 }
692
693 if (fracBetter > pruneThreshold_)
694 {
695 // We are only pruning motions if they, AND all descendents, have a estimated cost greater than pruneTreeCost
696 // The easiest way to do this is to find leaves that should be pruned and ascend up their ancestry until a
697 // motion is found that is kept.
698 // To avoid making an intermediate copy of the NN structure, we process the tree by descending down from the
699 // start(s).
700 // In the first pass, all Motions with a cost below pruneTreeCost, or Motion's with children with costs below
701 // pruneTreeCost are added to the replacement NN structure,
702 // while all other Motions are stored as either a 'leaf' or 'chain' Motion. After all the leaves are
703 // disconnected and deleted, we check
704 // if any of the the chain Motions are now leaves, and repeat that process until done.
705 // This avoids (1) copying the NN structure into an intermediate variable and (2) the use of the expensive
706 // NN::remove() method.
707
708 // Variable
709 // The queue of Motions to process:
710 std::queue<Motion *, std::deque<Motion *>> motionQueue;
711 // The list of leaves to prune
712 std::queue<Motion *, std::deque<Motion *>> leavesToPrune;
713 // The list of chain vertices to recheck after pruning
714 std::list<Motion *> chainsToRecheck;
715
716 // Clear the NN structure:
717 nn_->clear();
718
719 // Put all the starts into the NN structure and their children into the queue:
720 // We do this so that start states are never pruned.
721 for (auto &startMotion : startMotions_)
722 {
723 // Add to the NN
724 nn_->add(startMotion);
725
726 // Add their children to the queue:
727 addChildrenToList(&motionQueue, startMotion);
728 }
729
730 while (motionQueue.empty() == false)
731 {
732 // Test, can the current motion ever provide a better solution?
733 if (keepCondition(motionQueue.front(), pruneTreeCost))
734 {
735 // Yes it can, so it definitely won't be pruned
736 // Add it back into the NN structure
737 nn_->add(motionQueue.front());
738
739 // Add it's children to the queue
740 addChildrenToList(&motionQueue, motionQueue.front());
741 }
742 else
743 {
744 // No it can't, but does it have children?
745 if (motionQueue.front()->children.empty() == false)
746 {
747 // Yes it does.
748 // We can minimize the number of intermediate chain motions if we check their children
749 // If any of them won't be pruned, then this motion won't either. This intuitively seems
750 // like a nice balance between following the descendents forever.
751
752 // Variable
753 // Whether the children are definitely to be kept.
754 bool keepAChild = false;
755
756 // Find if any child is definitely not being pruned.
757 for (unsigned int i = 0u; keepAChild == false && i < motionQueue.front()->children.size(); ++i)
758 {
759 // Test if the child can ever provide a better solution
760 keepAChild = keepCondition(motionQueue.front()->children.at(i), pruneTreeCost);
761 }
762
763 // Are we *definitely* keeping any of the children?
764 if (keepAChild)
765 {
766 // Yes, we are, so we are not pruning this motion
767 // Add it back into the NN structure.
768 nn_->add(motionQueue.front());
769 }
770 else
771 {
772 // No, we aren't. This doesn't mean we won't though
773 // Move this Motion to the temporary list
774 chainsToRecheck.push_back(motionQueue.front());
775 }
776
777 // Either way. add it's children to the queue
778 addChildrenToList(&motionQueue, motionQueue.front());
779 }
780 else
781 {
782 // No, so we will be pruning this motion:
783 leavesToPrune.push(motionQueue.front());
784 }
785 }
786
787 // Pop the iterator, std::list::erase returns the next iterator
788 motionQueue.pop();
789 }
790
791 // We now have a list of Motions to definitely remove, and a list of Motions to recheck
792 // Iteratively check the two lists until there is nothing to to remove
793 while (leavesToPrune.empty() == false)
794 {
795 // First empty the current leaves-to-prune
796 while (leavesToPrune.empty() == false)
797 {
798 // If this leaf is a goal, remove it from the goal set
799 if (leavesToPrune.front()->inGoal == true)
800 {
801 // Warn if pruning the _best_ goal
802 if (leavesToPrune.front() == bestGoalMotion_)
803 {
804 OMPL_ERROR("%s: Pruning the best goal.", getName().c_str());
805 }
806 // Remove it
807 goalMotions_.erase(std::remove(goalMotions_.begin(), goalMotions_.end(), leavesToPrune.front()),
808 goalMotions_.end());
809 }
810
811 // Remove the leaf from its parent
812 removeFromParent(leavesToPrune.front());
813
814 // Erase the actual motion
815 // First free the state
816 si_->freeState(leavesToPrune.front()->state);
817
818 // then delete the pointer
819 delete leavesToPrune.front();
820
821 // And finally remove it from the list, erase returns the next iterator
822 leavesToPrune.pop();
823
824 // Update our counter
825 ++numPruned;
826 }
827
828 // Now, we need to go through the list of chain vertices and see if any are now leaves
829 auto mIter = chainsToRecheck.begin();
830 while (mIter != chainsToRecheck.end())
831 {
832 // Is the Motion a leaf?
833 if ((*mIter)->children.empty() == true)
834 {
835 // It is, add to the removal queue
836 leavesToPrune.push(*mIter);
837
838 // Remove from this queue, getting the next
839 mIter = chainsToRecheck.erase(mIter);
840 }
841 else
842 {
843 // Is isn't, skip to the next
844 ++mIter;
845 }
846 }
847 }
848
849 // Now finally add back any vertices left in chainsToReheck.
850 // These are chain vertices that have descendents that we want to keep
851 for (const auto &r : chainsToRecheck)
852 // Add the motion back to the NN struct:
853 nn_->add(r);
854
855 // All done pruning.
856 // Update the cost at which we've pruned:
857 prunedCost_ = pruneTreeCost;
858
859 // And if we're using the pruned measure, the measure to which we've pruned
860 if (usePrunedMeasure_)
861 {
862 prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
863
864 if (useKNearest_ == false)
865 {
866 calculateRewiringLowerBounds();
867 }
868 }
869 // No else, prunedMeasure_ is the si_ measure by default.
870 }
871
872 return numPruned;
873}
874
875void ompl::geometric::RRTstar::addChildrenToList(std::queue<Motion *, std::deque<Motion *>> *motionList, Motion *motion)
876{
877 for (auto &child : motion->children)
878 {
879 motionList->push(child);
880 }
881}
882
883bool ompl::geometric::RRTstar::keepCondition(const Motion *motion, const base::Cost &threshold) const
884{
885 // We keep if the cost-to-come-heuristic of motion is <= threshold, by checking
886 // if !(threshold < heuristic), as if b is not better than a, then a is better than, or equal to, b
887 if (bestGoalMotion_ && motion == bestGoalMotion_)
888 {
889 // If the threshold is the theoretical minimum, the bestGoalMotion_ will sometimes fail the test due to floating point precision. Avoid that.
890 return true;
891 }
892
893 return !opt_->isCostBetterThan(threshold, solutionHeuristic(motion));
894}
895
897{
898 base::Cost costToCome;
899 if (useAdmissibleCostToCome_)
900 {
901 // Start with infinite cost
902 costToCome = opt_->infiniteCost();
903
904 // Find the min from each start
905 for (auto &startMotion : startMotions_)
906 {
907 costToCome = opt_->betterCost(
908 costToCome, opt_->motionCost(startMotion->state,
909 motion->state)); // lower-bounding cost from the start to the state
910 }
911 }
912 else
913 {
914 costToCome = motion->cost; // current cost from the state to the goal
915 }
916
917 const base::Cost costToGo =
918 opt_->costToGo(motion->state, pdef_->getGoal().get()); // lower-bounding cost from the state to the goal
919 return opt_->combineCosts(costToCome, costToGo); // add the two costs
920}
921
923{
924 if (static_cast<bool>(opt_) == true)
925 {
926 if (opt_->hasCostToGoHeuristic() == false)
927 {
928 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
929 }
930 }
931
932 // If we just disabled tree pruning, but we wee using prunedMeasure, we need to disable that as it required myself
933 if (prune == false && getPrunedMeasure() == true)
934 {
935 setPrunedMeasure(false);
936 }
937
938 // Store
939 useTreePruning_ = prune;
940}
941
943{
944 if (static_cast<bool>(opt_) == true)
945 {
946 if (opt_->hasCostToGoHeuristic() == false)
947 {
948 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
949 }
950 }
951
952 // This option only works with informed sampling
953 if (informedMeasure == true && (useInformedSampling_ == false || useTreePruning_ == false))
954 {
955 OMPL_ERROR("%s: InformedMeasure requires InformedSampling and TreePruning.", getName().c_str());
956 }
957
958 // Check if we're changed and update parameters if we have:
959 if (informedMeasure != usePrunedMeasure_)
960 {
961 // Store the setting
962 usePrunedMeasure_ = informedMeasure;
963
964 // Update the prunedMeasure_ appropriately, if it has been configured.
965 if (setup_ == true)
966 {
967 if (usePrunedMeasure_)
968 {
969 prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
970 }
971 else
972 {
973 prunedMeasure_ = si_->getSpaceMeasure();
974 }
975 }
976
977 // And either way, update the rewiring radius if necessary
978 if (useKNearest_ == false)
979 {
980 calculateRewiringLowerBounds();
981 }
982 }
983}
984
986{
987 if (static_cast<bool>(opt_) == true)
988 {
989 if (opt_->hasCostToGoHeuristic() == false)
990 {
991 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
992 }
993 }
994
995 // This option is mutually exclusive with setSampleRejection, assert that:
996 if (informedSampling == true && useRejectionSampling_ == true)
997 {
998 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
999 }
1000
1001 // If we just disabled tree pruning, but we are using prunedMeasure, we need to disable that as it required myself
1002 if (informedSampling == false && getPrunedMeasure() == true)
1003 {
1004 setPrunedMeasure(false);
1005 }
1006
1007 // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
1008 // we only want to do if one is already allocated.
1009 if (informedSampling != useInformedSampling_)
1010 {
1011 // If we're disabled informedSampling, and prunedMeasure is enabled, we need to disable that
1012 if (informedSampling == false && usePrunedMeasure_ == true)
1013 {
1014 setPrunedMeasure(false);
1015 }
1016
1017 // Store the value
1018 useInformedSampling_ = informedSampling;
1019
1020 // If we currently have a sampler, we need to make a new one
1021 if (sampler_ || infSampler_)
1022 {
1023 // Reset the samplers
1024 sampler_.reset();
1025 infSampler_.reset();
1026
1027 // Create the sampler
1028 allocSampler();
1029 }
1030 }
1031}
1032
1034{
1035 if (static_cast<bool>(opt_) == true)
1036 {
1037 if (opt_->hasCostToGoHeuristic() == false)
1038 {
1039 OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
1040 }
1041 }
1042
1043 // This option is mutually exclusive with setInformedSampling, assert that:
1044 if (reject == true && useInformedSampling_ == true)
1045 {
1046 OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
1047 }
1048
1049 // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
1050 // we only want to do if one is already allocated.
1051 if (reject != useRejectionSampling_)
1052 {
1053 // Store the setting
1054 useRejectionSampling_ = reject;
1055
1056 // If we currently have a sampler, we need to make a new one
1057 if (sampler_ || infSampler_)
1058 {
1059 // Reset the samplers
1060 sampler_.reset();
1061 infSampler_.reset();
1062
1063 // Create the sampler
1064 allocSampler();
1065 }
1066 }
1067}
1068
1070{
1071 // Make sure we're using some type of informed sampling
1072 if (useInformedSampling_ == false && useRejectionSampling_ == false)
1073 {
1074 OMPL_ERROR("%s: OrderedSampling requires either informed sampling or rejection sampling.", getName().c_str());
1075 }
1076
1077 // Check if we're changing the setting. If we are, we will need to create a new sampler, which we only want to do if
1078 // one is already allocated.
1079 if (orderSamples != useOrderedSampling_)
1080 {
1081 // Store the setting
1082 useOrderedSampling_ = orderSamples;
1083
1084 // If we currently have a sampler, we need to make a new one
1085 if (sampler_ || infSampler_)
1086 {
1087 // Reset the samplers
1088 sampler_.reset();
1089 infSampler_.reset();
1090
1091 // Create the sampler
1092 allocSampler();
1093 }
1094 }
1095}
1096
1098{
1099 // Allocate the appropriate type of sampler.
1100 if (useInformedSampling_)
1101 {
1102 // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
1103 OMPL_INFORM("%s: Using informed sampling.", getName().c_str());
1104 infSampler_ = opt_->allocInformedStateSampler(pdef_, numSampleAttempts_);
1105 }
1106 else if (useRejectionSampling_)
1107 {
1108 // We are explicitly using rejection sampling.
1109 OMPL_INFORM("%s: Using rejection sampling.", getName().c_str());
1110 infSampler_ = std::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
1111 }
1112 else
1113 {
1114 // We are using a regular sampler
1115 sampler_ = si_->allocStateSampler();
1116 }
1117
1118 // Wrap into a sorted sampler
1119 if (useOrderedSampling_ == true)
1120 {
1121 infSampler_ = std::make_shared<base::OrderedInfSampler>(infSampler_, batchSize_);
1122 }
1123 // No else
1124}
1125
1127{
1128 // Use the appropriate sampler
1129 if (useInformedSampling_ || useRejectionSampling_)
1130 {
1131 // Attempt the focused sampler and return the result.
1132 // If bestCost is changing a lot by small amounts, this could
1133 // be prunedCost_ to reduce the number of times the informed sampling
1134 // transforms are recalculated.
1135 return infSampler_->sampleUniform(statePtr, bestCost_);
1136 }
1137 else
1138 {
1139 // Simply return a state from the regular sampler
1140 sampler_->sampleUniform(statePtr);
1141
1142 // Always true
1143 return true;
1144 }
1145}
1146
1148{
1149 const auto dimDbl = static_cast<double>(si_->getStateDimension());
1150
1151 // k_rrt > 2^(d + 1) * e * (1 + 1 / d). K-nearest RRT*
1152 k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
1153
1154 // r_rrt > (2*(1+1/d))^(1/d)*(measure/ballvolume)^(1/d)
1155 // If we're not using the informed measure, prunedMeasure_ will be set to si_->getSpaceMeasure();
1156 r_rrt_ =
1157 rewireFactor_ *
1158 std::pow(2 * (1.0 + 1.0 / dimDbl) * (prunedMeasure_ / unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl);
1159}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition Goal.h:63
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition RRTstar.h:334
base::Cost cost
The cost up to this motion.
Definition RRTstar.h:354
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition RRTstar.h:358
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition RRTstar.h:361
Motion * parent
The parent motion in the exploration tree.
Definition RRTstar.h:348
base::State * state
The state contained by the motion.
Definition RRTstar.h:345
void freeMemory()
Free the memory allocated by this planner.
Definition RRTstar.cpp:641
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition RRTstar.cpp:1147
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition RRTstar.cpp:145
bool keepCondition(const Motion *motion, const base::Cost &threshold) const
Check whether the given motion passes the specified cost threshold, meaning it will be kept during pr...
Definition RRTstar.cpp:883
void updateChildCosts(Motion *m)
Updates the cost of the children of this node if the cost up to this node has changed.
Definition RRTstar.cpp:632
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition RRTstar.cpp:620
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition RRTstar.cpp:1033
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition RRTstar.cpp:985
void getNeighbors(Motion *motion, std::vector< Motion * > &nbh) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition RRTstar.cpp:603
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition RRTstar.cpp:165
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition RRTstar.cpp:1126
void setPrunedMeasure(bool informedMeasure)
Use the measure of the pruned subproblem instead of the measure of the entire problem domain (if such...
Definition RRTstar.cpp:942
void allocSampler()
Create the samplers.
Definition RRTstar.cpp:1097
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition RRTstar.cpp:656
void addChildrenToList(std::queue< Motion *, std::deque< Motion * > > *motionList, Motion *motion)
Add the children of a vertex to the given list.
Definition RRTstar.cpp:875
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition RRTstar.cpp:94
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition RRTstar.cpp:1069
void setTreePruning(bool prune)
Controls whether the tree is pruned during the search. This pruning removes a vertex if and only if i...
Definition RRTstar.cpp:922
base::Cost solutionHeuristic(const Motion *motion) const
Computes the solution cost heuristically as the cost to come from start to the motion plus the cost t...
Definition RRTstar.cpp:896
int pruneTree(const base::Cost &pruneTreeCost)
Prunes all those states which estimated total cost is higher than pruneTreeCost. Returns the number o...
Definition RRTstar.cpp:676
This class contains methods that automatically configure various parameters for motion planning....
Definition SelfConfig.h:60
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
std::function< void(const Planner *, const std::vector< const base::State * > &, const Cost)> ReportIntermediateSolutionFn
When a planner has an intermediate solution (e.g., optimizing planners), a function with this signatu...
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Representation of a solution to a planning problem.
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.