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