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();
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  lastGoalMotion_ = 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: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
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 *solution = lastGoalMotion_;
215 
216  Motion *approximation = nullptr;
217  double approximatedist = std::numeric_limits<double>::infinity();
218  bool sufficientlyShort = false;
219 
220  auto *rmotion = new Motion(si_);
221  base::State *rstate = rmotion->state;
222  base::State *xstate = si_->allocState();
223 
224  std::vector<Motion *> nbh;
225 
226  std::vector<base::Cost> costs;
227  std::vector<base::Cost> incCosts;
228  std::vector<std::size_t> sortedCostIndices;
229 
230  std::vector<int> valid;
231  unsigned int rewireTest = 0;
232  unsigned int statesGenerated = 0;
233 
234  if (solution)
235  OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
236  solution->cost.value());
237 
238  if (useKNearest_)
239  OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
240  (unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
241  else
242  OMPL_INFORM(
243  "%s: Initial rewiring radius of %.2f", getName().c_str(),
244  std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
245  1 / (double)(si_->getStateDimension()))));
246 
247  // our functor for sorting nearest neighbors
248  CostIndexCompare compareFn(costs, *opt_);
249 
250  while (ptc == false)
251  {
252  iterations_++;
253 
254  // sample random state (with goal biasing)
255  // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
256  // states.
257  if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
258  goal_s->canSample())
259  goal_s->sampleGoal(rstate);
260  else
261  {
262  // Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
263  // loop and return to try again
264  if (!sampleUniform(rstate))
265  continue;
266  }
267 
268  // find closest state in the tree
269  Motion *nmotion = nn_->nearest(rmotion);
270 
271  if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
272  continue;
273 
274  base::State *dstate = rstate;
275 
276  // find state to add to the tree
277  double d = si_->distance(nmotion->state, rstate);
278  if (d > maxDistance_)
279  {
280  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
281  dstate = xstate;
282  }
283 
284  // Check if the motion between the nearest state and the state to add is valid
285  if (si_->checkMotion(nmotion->state, dstate))
286  {
287  // create a motion
288  auto *motion = new Motion(si_);
289  si_->copyState(motion->state, dstate);
290  motion->parent = nmotion;
291  motion->incCost = opt_->motionCost(nmotion->state, motion->state);
292  motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
293 
294  // Find nearby neighbors of the new motion
295  getNeighbors(motion, nbh);
296 
297  rewireTest += nbh.size();
298  ++statesGenerated;
299 
300  // cache for distance computations
301  //
302  // Our cost caches only increase in size, so they're only
303  // resized if they can't fit the current neighborhood
304  if (costs.size() < nbh.size())
305  {
306  costs.resize(nbh.size());
307  incCosts.resize(nbh.size());
308  sortedCostIndices.resize(nbh.size());
309  }
310 
311  // cache for motion validity (only useful in a symmetric space)
312  //
313  // Our validity caches only increase in size, so they're
314  // only resized if they can't fit the current neighborhood
315  if (valid.size() < nbh.size())
316  valid.resize(nbh.size());
317  std::fill(valid.begin(), valid.begin() + nbh.size(), 0);
318 
319  // Finding the nearest neighbor to connect to
320  // By default, neighborhood states are sorted by cost, and collision checking
321  // is performed in increasing order of cost
322  if (delayCC_)
323  {
324  // calculate all costs and distances
325  for (std::size_t i = 0; i < nbh.size(); ++i)
326  {
327  incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
328  costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
329  }
330 
331  // sort the nodes
332  //
333  // we're using index-value pairs so that we can get at
334  // original, unsorted indices
335  for (std::size_t i = 0; i < nbh.size(); ++i)
336  sortedCostIndices[i] = i;
337  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + nbh.size(), compareFn);
338 
339  // collision check until a valid motion is found
340  //
341  // ASYMMETRIC CASE: it's possible that none of these
342  // neighbors are valid. This is fine, because motion
343  // already has a connection to the tree through
344  // nmotion (with populated cost fields!).
345  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
346  i != sortedCostIndices.begin() + nbh.size(); ++i)
347  {
348  if (nbh[*i] == nmotion ||
349  ((!useKNearest_ || si_->distance(nbh[*i]->state, motion->state) < maxDistance_) &&
350  si_->checkMotion(nbh[*i]->state, motion->state)))
351  {
352  motion->incCost = incCosts[*i];
353  motion->cost = costs[*i];
354  motion->parent = nbh[*i];
355  valid[*i] = 1;
356  break;
357  }
358  else
359  valid[*i] = -1;
360  }
361  }
362  else // if not delayCC
363  {
364  motion->incCost = opt_->motionCost(nmotion->state, motion->state);
365  motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
366  // find which one we connect the new state to
367  for (std::size_t i = 0; i < nbh.size(); ++i)
368  {
369  if (nbh[i] != nmotion)
370  {
371  incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
372  costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
373  if (opt_->isCostBetterThan(costs[i], motion->cost))
374  {
375  if ((!useKNearest_ || si_->distance(nbh[i]->state, motion->state) < maxDistance_) &&
376  si_->checkMotion(nbh[i]->state, motion->state))
377  {
378  motion->incCost = incCosts[i];
379  motion->cost = costs[i];
380  motion->parent = nbh[i];
381  valid[i] = 1;
382  }
383  else
384  valid[i] = -1;
385  }
386  }
387  else
388  {
389  incCosts[i] = motion->incCost;
390  costs[i] = motion->cost;
391  valid[i] = 1;
392  }
393  }
394  }
395 
396  if (useNewStateRejection_)
397  {
398  if (opt_->isCostBetterThan(solutionHeuristic(motion), bestCost_))
399  {
400  nn_->add(motion);
401  motion->parent->children.push_back(motion);
402  }
403  else // If the new motion does not improve the best cost it is ignored.
404  {
405  si_->freeState(motion->state);
406  delete motion;
407  continue;
408  }
409  }
410  else
411  {
412  // add motion to the tree
413  nn_->add(motion);
414  motion->parent->children.push_back(motion);
415  }
416 
417  bool checkForSolution = false;
418  for (std::size_t i = 0; i < nbh.size(); ++i)
419  {
420  if (nbh[i] != motion->parent)
421  {
422  base::Cost nbhIncCost;
423  if (symCost)
424  nbhIncCost = incCosts[i];
425  else
426  nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state);
427  base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost);
428  if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost))
429  {
430  bool motionValid;
431  if (valid[i] == 0)
432  {
433  motionValid =
434  (!useKNearest_ || si_->distance(nbh[i]->state, motion->state) < maxDistance_) &&
435  si_->checkMotion(motion->state, nbh[i]->state);
436  }
437  else
438  {
439  motionValid = (valid[i] == 1);
440  }
441 
442  if (motionValid)
443  {
444  // Remove this node from its parent list
445  removeFromParent(nbh[i]);
446 
447  // Add this node to the new parent
448  nbh[i]->parent = motion;
449  nbh[i]->incCost = nbhIncCost;
450  nbh[i]->cost = nbhNewCost;
451  nbh[i]->parent->children.push_back(nbh[i]);
452 
453  // Update the costs of the node's children
454  updateChildCosts(nbh[i]);
455 
456  checkForSolution = true;
457  }
458  }
459  }
460  }
461 
462  // Add the new motion to the goalMotion_ list, if it satisfies the goal
463  double distanceFromGoal;
464  if (goal->isSatisfied(motion->state, &distanceFromGoal))
465  {
466  motion->inGoal = true;
467  goalMotions_.push_back(motion);
468  checkForSolution = true;
469  }
470 
471  // Checking for solution or iterative improvement
472  if (checkForSolution)
473  {
474  bool updatedSolution = false;
475  for (auto &goalMotion : goalMotions_)
476  {
477  if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
478  {
479  if (opt_->isFinite(bestCost_) == false)
480  {
481  OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
482  "vertices in the graph)",
483  getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
484  }
485  bestCost_ = goalMotion->cost;
486  updatedSolution = true;
487  }
488 
489  sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
490  if (sufficientlyShort)
491  {
492  solution = goalMotion;
493  break;
494  }
495  else if (!solution || opt_->isCostBetterThan(goalMotion->cost, solution->cost))
496  {
497  solution = goalMotion;
498  updatedSolution = true;
499  }
500  }
501 
502  if (updatedSolution)
503  {
504  if (useTreePruning_)
505  {
506  pruneTree(bestCost_);
507  }
508 
509  if (intermediateSolutionCallback)
510  {
511  std::vector<const base::State *> spath;
512  Motion *intermediate_solution =
513  solution->parent; // Do not include goal state to simplify code.
514 
515  // Push back until we find the start, but not the start itself
516  while (intermediate_solution->parent != nullptr)
517  {
518  spath.push_back(intermediate_solution->state);
519  intermediate_solution = intermediate_solution->parent;
520  }
521 
522  intermediateSolutionCallback(this, spath, bestCost_);
523  }
524  }
525  }
526 
527  // Checking for approximate solution (closest state found to the goal)
528  if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
529  {
530  approximation = motion;
531  approximatedist = distanceFromGoal;
532  }
533  }
534 
535  // terminate if a sufficient solution is found
536  if (solution && sufficientlyShort)
537  break;
538  }
539 
540  bool approximate = (solution == nullptr);
541  bool addedSolution = false;
542  if (approximate)
543  solution = approximation;
544  else
545  lastGoalMotion_ = solution;
546 
547  if (solution != nullptr)
548  {
549  ptc.terminate();
550  // construct the solution path
551  std::vector<Motion *> mpath;
552  while (solution != nullptr)
553  {
554  mpath.push_back(solution);
555  solution = solution->parent;
556  }
557 
558  // set the solution path
559  auto path(std::make_shared<PathGeometric>(si_));
560  for (int i = mpath.size() - 1; i >= 0; --i)
561  path->append(mpath[i]->state);
562 
563  // Add the solution path.
564  base::PlannerSolution psol(path);
565  psol.setPlannerName(getName());
566  if (approximate)
567  psol.setApproximate(approximatedist);
568  // Does the solution satisfy the optimization objective?
569  psol.setOptimized(opt_, bestCost_, sufficientlyShort);
570  pdef_->addSolutionPath(psol);
571 
572  addedSolution = true;
573  }
574 
575  si_->freeState(xstate);
576  if (rmotion->state)
577  si_->freeState(rmotion->state);
578  delete rmotion;
579 
580  OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
581  "%.3f",
582  getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
583 
584  return base::PlannerStatus(addedSolution, approximate);
585 }
586 
587 void ompl::geometric::RRTstar::getNeighbors(Motion *motion, std::vector<Motion *> &nbh) const
588 {
589  auto cardDbl = static_cast<double>(nn_->size() + 1u);
590  if (useKNearest_)
591  {
592  //- k-nearest RRT*
593  unsigned int k = std::ceil(k_rrt_ * log(cardDbl));
594  nn_->nearestK(motion, k, nbh);
595  }
596  else
597  {
598  double r = std::min(
599  maxDistance_, r_rrt_ * std::pow(log(cardDbl) / cardDbl, 1 / static_cast<double>(si_->getStateDimension())));
600  nn_->nearestR(motion, r, nbh);
601  }
602 }
603 
605 {
606  for (auto it = m->parent->children.begin(); it != m->parent->children.end(); ++it)
607  {
608  if (*it == m)
609  {
610  m->parent->children.erase(it);
611  break;
612  }
613  }
614 }
615 
617 {
618  for (std::size_t i = 0; i < m->children.size(); ++i)
619  {
620  m->children[i]->cost = opt_->combineCosts(m->cost, m->children[i]->incCost);
621  updateChildCosts(m->children[i]);
622  }
623 }
624 
626 {
627  if (nn_)
628  {
629  std::vector<Motion *> motions;
630  nn_->list(motions);
631  for (auto &motion : motions)
632  {
633  if (motion->state)
634  si_->freeState(motion->state);
635  delete motion;
636  }
637  }
638 }
639 
641 {
642  Planner::getPlannerData(data);
643 
644  std::vector<Motion *> motions;
645  if (nn_)
646  nn_->list(motions);
647 
648  if (lastGoalMotion_)
649  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
650 
651  for (auto &motion : motions)
652  {
653  if (motion->parent == nullptr)
654  data.addStartVertex(base::PlannerDataVertex(motion->state));
655  else
656  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
657  }
658 }
659 
661 {
662  // Variable
663  // The percent improvement (expressed as a [0,1] fraction) in cost
664  double fracBetter;
665  // The number pruned
666  int numPruned = 0;
667 
668  if (opt_->isFinite(prunedCost_))
669  {
670  fracBetter = std::abs((pruneTreeCost.value() - prunedCost_.value()) / prunedCost_.value());
671  }
672  else
673  {
674  fracBetter = 1.0;
675  }
676 
677  if (fracBetter > pruneThreshold_)
678  {
679  // We are only pruning motions if they, AND all descendents, have a estimated cost greater than pruneTreeCost
680  // The easiest way to do this is to find leaves that should be pruned and ascend up their ancestry until a
681  // motion is found that is kept.
682  // To avoid making an intermediate copy of the NN structure, we process the tree by descending down from the
683  // start(s).
684  // In the first pass, all Motions with a cost below pruneTreeCost, or Motion's with children with costs below
685  // pruneTreeCost are added to the replacement NN structure,
686  // while all other Motions are stored as either a 'leaf' or 'chain' Motion. After all the leaves are
687  // disconnected and deleted, we check
688  // if any of the the chain Motions are now leaves, and repeat that process until done.
689  // This avoids (1) copying the NN structure into an intermediate variable and (2) the use of the expensive
690  // NN::remove() method.
691 
692  // Variable
693  // The queue of Motions to process:
694  std::queue<Motion *, std::deque<Motion *>> motionQueue;
695  // The list of leaves to prune
696  std::queue<Motion *, std::deque<Motion *>> leavesToPrune;
697  // The list of chain vertices to recheck after pruning
698  std::list<Motion *> chainsToRecheck;
699 
700  // Clear the NN structure:
701  nn_->clear();
702 
703  // Put all the starts into the NN structure and their children into the queue:
704  // We do this so that start states are never pruned.
705  for (auto &startMotion : startMotions_)
706  {
707  // Add to the NN
708  nn_->add(startMotion);
709 
710  // Add their children to the queue:
711  addChildrenToList(&motionQueue, startMotion);
712  }
713 
714  while (motionQueue.empty() == false)
715  {
716  // Test, can the current motion ever provide a better solution?
717  if (keepCondition(motionQueue.front(), pruneTreeCost))
718  {
719  // Yes it can, so it definitely won't be pruned
720  // Add it back into the NN structure
721  nn_->add(motionQueue.front());
722 
723  // Add it's children to the queue
724  addChildrenToList(&motionQueue, motionQueue.front());
725  }
726  else
727  {
728  // No it can't, but does it have children?
729  if (motionQueue.front()->children.empty() == false)
730  {
731  // Yes it does.
732  // We can minimize the number of intermediate chain motions if we check their children
733  // If any of them won't be pruned, then this motion won't either. This intuitively seems
734  // like a nice balance between following the descendents forever.
735 
736  // Variable
737  // Whether the children are definitely to be kept.
738  bool keepAChild = false;
739 
740  // Find if any child is definitely not being pruned.
741  for (unsigned int i = 0u; keepAChild == false && i < motionQueue.front()->children.size(); ++i)
742  {
743  // Test if the child can ever provide a better solution
744  keepAChild = keepCondition(motionQueue.front()->children.at(i), pruneTreeCost);
745  }
746 
747  // Are we *definitely* keeping any of the children?
748  if (keepAChild)
749  {
750  // Yes, we are, so we are not pruning this motion
751  // Add it back into the NN structure.
752  nn_->add(motionQueue.front());
753  }
754  else
755  {
756  // No, we aren't. This doesn't mean we won't though
757  // Move this Motion to the temporary list
758  chainsToRecheck.push_back(motionQueue.front());
759  }
760 
761  // Either way. add it's children to the queue
762  addChildrenToList(&motionQueue, motionQueue.front());
763  }
764  else
765  {
766  // No, so we will be pruning this motion:
767  leavesToPrune.push(motionQueue.front());
768  }
769  }
770 
771  // Pop the iterator, std::list::erase returns the next iterator
772  motionQueue.pop();
773  }
774 
775  // We now have a list of Motions to definitely remove, and a list of Motions to recheck
776  // Iteratively check the two lists until there is nothing to to remove
777  while (leavesToPrune.empty() == false)
778  {
779  // First empty the leave-to-prune
780  while (leavesToPrune.empty() == false)
781  {
782  // If this leaf is a goal, remove it from the goal set
783  if (leavesToPrune.front()->inGoal == true)
784  {
785  // Remove it
786  goalMotions_.erase(std::remove(goalMotions_.begin(), goalMotions_.end(), leavesToPrune.front()),
787  goalMotions_.end());
788  }
789 
790  // Remove the leaf from its parent
791  removeFromParent(leavesToPrune.front());
792 
793  // Erase the actual motion
794  // First free the state
795  si_->freeState(leavesToPrune.front()->state);
796 
797  // then delete the pointer
798  delete leavesToPrune.front();
799 
800  // And finally remove it from the list, erase returns the next iterator
801  leavesToPrune.pop();
802 
803  // Update our counter
804  ++numPruned;
805  }
806 
807  // Now, we need to go through the list of chain vertices and see if any are now leaves
808  auto mIter = chainsToRecheck.begin();
809  while (mIter != chainsToRecheck.end())
810  {
811  // Is the Motion a leaf?
812  if ((*mIter)->children.empty() == true)
813  {
814  // It is, add to the removal queue
815  leavesToPrune.push(*mIter);
816 
817  // Remove from this queue, getting the next
818  mIter = chainsToRecheck.erase(mIter);
819  }
820  else
821  {
822  // Is isn't, skip to the next
823  ++mIter;
824  }
825  }
826  }
827 
828  // Now finally add back any vertices left in chainsToReheck.
829  // These are chain vertices that have descendents that we want to keep
830  for (std::list<Motion *>::const_iterator mIter = chainsToRecheck.begin(); mIter != chainsToRecheck.end();
831  ++mIter)
832  {
833  // Add the motion back to the NN struct:
834  nn_->add(*mIter);
835  }
836 
837  // All done pruning.
838  // Update the cost at which we've pruned:
839  prunedCost_ = pruneTreeCost;
840 
841  // And if we're using the pruned measure, the measure to which we've pruned
842  if (usePrunedMeasure_)
843  {
844  prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
845 
846  if (useKNearest_ == false)
847  {
848  calculateRewiringLowerBounds();
849  }
850  }
851  // No else, prunedMeasure_ is the si_ measure by default.
852  }
853 
854  return numPruned;
855 }
856 
857 void ompl::geometric::RRTstar::addChildrenToList(std::queue<Motion *, std::deque<Motion *>> *motionList, Motion *motion)
858 {
859  for (auto &child : motion->children)
860  {
861  motionList->push(child);
862  }
863 }
864 
865 bool ompl::geometric::RRTstar::keepCondition(const Motion *motion, const base::Cost &threshold) const
866 {
867  // We keep if the cost-to-come-heuristic of motion is <= threshold, by checking
868  // if (!threshold < heuristic), as if b is not better than a, then a is better than, or equal to, b
869  return !opt_->isCostBetterThan(threshold, solutionHeuristic(motion));
870 }
871 
873 {
874  base::Cost costToCome;
875  if (useAdmissibleCostToCome_)
876  {
877  // Start with infinite cost
878  costToCome = opt_->infiniteCost();
879 
880  // Find the min from each start
881  for (auto &startMotion : startMotions_)
882  {
883  costToCome = opt_->betterCost(
884  costToCome, opt_->motionCost(startMotion->state,
885  motion->state)); // lower-bounding cost from the start to the state
886  }
887  }
888  else
889  {
890  costToCome = motion->cost; // current cost from the state to the goal
891  }
892 
893  const base::Cost costToGo =
894  opt_->costToGo(motion->state, pdef_->getGoal().get()); // lower-bounding cost from the state to the goal
895  return opt_->combineCosts(costToCome, costToGo); // add the two costs
896 }
897 
899 {
900  if (static_cast<bool>(opt_) == true)
901  {
902  if (opt_->hasCostToGoHeuristic() == false)
903  {
904  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
905  }
906  }
907 
908  // If we just disabled tree pruning, but we wee using prunedMeasure, we need to disable that as it required myself
909  if (prune == false && getPrunedMeasure() == true)
910  {
911  setPrunedMeasure(false);
912  }
913 
914  // Store
915  useTreePruning_ = prune;
916 }
917 
919 {
920  if (static_cast<bool>(opt_) == true)
921  {
922  if (opt_->hasCostToGoHeuristic() == false)
923  {
924  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
925  }
926  }
927 
928  // This option only works with informed sampling
929  if (informedMeasure == true && (useInformedSampling_ == false || useTreePruning_ == false))
930  {
931  OMPL_ERROR("%s: InformedMeasure requires InformedSampling and TreePruning.", getName().c_str());
932  }
933 
934  // Check if we're changed and update parameters if we have:
935  if (informedMeasure != usePrunedMeasure_)
936  {
937  // Store the setting
938  usePrunedMeasure_ = informedMeasure;
939 
940  // Update the prunedMeasure_ appropriately, if it has been configured.
941  if (setup_ == true)
942  {
943  if (usePrunedMeasure_)
944  {
945  prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
946  }
947  else
948  {
949  prunedMeasure_ = si_->getSpaceMeasure();
950  }
951  }
952 
953  // And either way, update the rewiring radius if necessary
954  if (useKNearest_ == false)
955  {
956  calculateRewiringLowerBounds();
957  }
958  }
959 }
960 
962 {
963  if (static_cast<bool>(opt_) == true)
964  {
965  if (opt_->hasCostToGoHeuristic() == false)
966  {
967  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
968  }
969  }
970 
971  // This option is mutually exclusive with setSampleRejection, assert that:
972  if (informedSampling == true && useRejectionSampling_ == true)
973  {
974  OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
975  }
976 
977  // If we just disabled tree pruning, but we are using prunedMeasure, we need to disable that as it required myself
978  if (informedSampling == false && getPrunedMeasure() == true)
979  {
980  setPrunedMeasure(false);
981  }
982 
983  // Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which
984  // we only want to do if one is already allocated.
985  if (informedSampling != useInformedSampling_)
986  {
987  // If we're disabled informedSampling, and prunedMeasure is enabled, we need to disable that
988  if (informedSampling == false && usePrunedMeasure_ == true)
989  {
990  setPrunedMeasure(false);
991  }
992 
993  // Store the value
994  useInformedSampling_ = informedSampling;
995 
996  // If we currently have a sampler, we need to make a new one
997  if (sampler_ || infSampler_)
998  {
999  // Reset the samplers
1000  sampler_.reset();
1001  infSampler_.reset();
1002 
1003  // Create the sampler
1004  allocSampler();
1005  }
1006  }
1007 }
1008 
1010 {
1011  if (static_cast<bool>(opt_) == true)
1012  {
1013  if (opt_->hasCostToGoHeuristic() == false)
1014  {
1015  OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
1016  }
1017  }
1018 
1019  // This option is mutually exclusive with setInformedSampling, assert that:
1020  if (reject == true && useInformedSampling_ == true)
1021  {
1022  OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
1023  }
1024 
1025  // Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which
1026  // we only want to do if one is already allocated.
1027  if (reject != useRejectionSampling_)
1028  {
1029  // Store the setting
1030  useRejectionSampling_ = reject;
1031 
1032  // If we currently have a sampler, we need to make a new one
1033  if (sampler_ || infSampler_)
1034  {
1035  // Reset the samplers
1036  sampler_.reset();
1037  infSampler_.reset();
1038 
1039  // Create the sampler
1040  allocSampler();
1041  }
1042  }
1043 }
1044 
1046 {
1047  // Make sure we're using some type of informed sampling
1048  if (useInformedSampling_ == false && useRejectionSampling_ == false)
1049  {
1050  OMPL_ERROR("%s: OrderedSampling requires either informed sampling or rejection sampling.", getName().c_str());
1051  }
1052 
1053  // 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
1054  // one is already allocated.
1055  if (orderSamples != useOrderedSampling_)
1056  {
1057  // Store the setting
1058  useOrderedSampling_ = orderSamples;
1059 
1060  // If we currently have a sampler, we need to make a new one
1061  if (sampler_ || infSampler_)
1062  {
1063  // Reset the samplers
1064  sampler_.reset();
1065  infSampler_.reset();
1066 
1067  // Create the sampler
1068  allocSampler();
1069  }
1070  }
1071 }
1072 
1074 {
1075  // Allocate the appropriate type of sampler.
1076  if (useInformedSampling_)
1077  {
1078  // We are using informed sampling, this can end-up reverting to rejection sampling in some cases
1079  OMPL_INFORM("%s: Using informed sampling.", getName().c_str());
1080  infSampler_ = opt_->allocInformedStateSampler(pdef_, numSampleAttempts_);
1081  }
1082  else if (useRejectionSampling_)
1083  {
1084  // We are explicitly using rejection sampling.
1085  OMPL_INFORM("%s: Using rejection sampling.", getName().c_str());
1086  infSampler_ = std::make_shared<base::RejectionInfSampler>(pdef_, numSampleAttempts_);
1087  }
1088  else
1089  {
1090  // We are using a regular sampler
1091  sampler_ = si_->allocStateSampler();
1092  }
1093 
1094  // Wrap into a sorted sampler
1095  if (useOrderedSampling_ == true)
1096  {
1097  infSampler_ = std::make_shared<base::OrderedInfSampler>(infSampler_, batchSize_);
1098  }
1099  // No else
1100 }
1101 
1103 {
1104  // Use the appropriate sampler
1105  if (useInformedSampling_ || useRejectionSampling_)
1106  {
1107  // Attempt the focused sampler and return the result.
1108  // If bestCost is changing a lot by small amounts, this could
1109  // be prunedCost_ to reduce the number of times the informed sampling
1110  // transforms are recalculated.
1111  return infSampler_->sampleUniform(statePtr, bestCost_);
1112  }
1113  else
1114  {
1115  // Simply return a state from the regular sampler
1116  sampler_->sampleUniform(statePtr);
1117 
1118  // Always true
1119  return true;
1120  }
1121 }
1122 
1124 {
1125  const auto dimDbl = static_cast<double>(si_->getStateDimension());
1126 
1127  // k_rrt > 2^(d + 1) * e * (1 + 1 / d). K-nearest RRT*
1128  k_rrt_ = rewireFactor_ * (std::pow(2, dimDbl + 1) * boost::math::constants::e<double>() * (1.0 + 1.0 / dimDbl));
1129 
1130  // r_rrt > (2*(1+1/d))^(1/d)*(measure/ballvolume)^(1/d)
1131  // If we're not using the informed measure, prunedMeasure_ will be set to si_->getSpaceMeasure();
1132  r_rrt_ =
1133  rewireFactor_ *
1134  std::pow(2 * (1.0 + 1.0 / dimDbl) * (prunedMeasure_ / unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl);
1135 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
double getRange() const
Get the range the planner is using.
Definition: RRTstar.h:122
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:399
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:640
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:166
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:106
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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:285
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:226
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTConnect.h:158
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s r_rrg* (or k_rrg = s k_rrg*) ...
Definition: RRTstar.h:129
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
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1045
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:160
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:239
bool getTreePruning() const
Get the state of the pruning option.
Definition: RRTstar.h:181
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand). If a direct sampling method is not defined for the objective, rejection sampling will be used by default.
Definition: RRTstar.cpp:961
Representation of a solution to a planning problem.
double getRewireFactor() const
Set the rewiring scale factor, s, such that r_rrg = s r_rrg* > r_rrg* (or k_rrg = s k_rrg* > k_rrg*...
Definition: RRTstar.h:137
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:616
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:306
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:318
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:918
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 goals.
Definition: Goal.h:62
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:356
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:116
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTConnect.cpp:75
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTConnect.h:149
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths...
Definition: Planner.h:219
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:189
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g., x_new = steer(x_nearest, x_rand))
Definition: RRTstar.h:233
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:300
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
Representation of a motion.
Definition: RRTstar.h:335
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTConnect.h:167
base::State * state
The state contained by the motion.
Definition: RRTstar.h:347
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:335
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:625
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:312
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
Abstract definition of a goal region that can be sampled.
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:587
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1009
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not...
Definition: RRTstar.h:246
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:252
bool getFocusSearch() const
Get the state of search focusing.
Definition: RRTstar.h:294
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:865
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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:660
bool getOrderedSampling() const
Get the state of sample ordering.
Definition: RRTstar.h:262
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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 getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:274
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...
RNG rng_
The random number generator.
Definition: RRTConnect.h:170
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTstar.h:217
void addChildrenToList(std::queue< Motion *, std::deque< Motion *>> *motionList, Motion *motion)
Add the children of a vertex to the given list.
Definition: RRTstar.cpp:857
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:195
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:207
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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...
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:363
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:350
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1102
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1073
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:207
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1123
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:898
double value() const
The value of the cost.
Definition: Cost.h:56
void removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTstar.cpp:604
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:100
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:268
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
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 terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:145
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
base::Cost incCost
The incremental cost of this motion&#39;s parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:360
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
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:872
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68