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 
52 ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si)
53  : base::Planner(si, "RRTstar")
54 {
56  specs_.optimizingPaths = 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,
75  Planner::declareParam<bool>("use_admissible_heuristic", this, &RRTstar::setAdmissibleCostToCome,
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 
89 ompl::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_)
105  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
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
239  OMPL_INFORM(
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 
603 void 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 
875 void 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 
883 bool 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 }
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition: RRTstar.h:340
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:406
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1097
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
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
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
Definition of an abstract state.
Definition: State.h:113
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:467
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1126
bool getOrderedSampling() const
Get the state of sample ordering.
Definition: RRTstar.h:356
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:454
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
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:289
double value() const
The value of the cost.
Definition: Cost.h:152
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTstar.h:311
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:450
Representation of a solution to a planning problem.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:301
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
void setFocusSearch(const bool focus)
A meta parameter to focusing the search to improving the current solution. This is the parameter set ...
Definition: RRTstar.h:379
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:444
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:362
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:145
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:320
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1147
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:194
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
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
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:260
double getRewireFactor() const
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* > r_rrg* (or k_rrg = s \times k_r...
Definition: RRTstar.h:231
bool getTreePruning() const
Get the state of the pruning option.
Definition: RRTstar.h:275
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:254
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:200
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTstar.cpp:94
A class to store the exit status of Planner::solve()
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:275
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
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 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 setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* (or k_rrg = s \times k_rrg*)
Definition: RRTstar.h:223
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTstar.cpp:985
Abstract definition of goals.
Definition: Goal.h:126
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1069
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:400
double getRange() const
Get the range the planner is using.
Definition: RRTstar.h:216
Representation of a motion.
Definition: RRTstar.h:429
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:412
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:641
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:333
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:394
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...
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...
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...
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:346
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 setPruneThreshold(const double pp)
Set the fractional change in solution cost necessary for pruning to occur, i.e., prune if the new sol...
Definition: RRTstar.h:283
base::State * state
The state contained by the motion.
Definition: RRTstar.h:441
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
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...
Abstract definition of a goal region that can be sampled.
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:210
@ INVALID_START
Invalid start state or no start state specified.
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition: RRTstar.h:327
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:457
bool getFocusSearch() const
Get the state of search focusing.
Definition: RRTstar.h:388
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
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:368