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 */
36 
37 #include "ompl/geometric/planners/rrt/RRTstar.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41 #include <algorithm>
42 #include <limits>
43 #include <map>
44 #include <queue>
45 #include <boost/math/constants/constants.hpp>
46 
47 ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
48  base::Planner(si, "RRTstar"),
49  goalBias_(0.05),
50  maxDistance_(0.0),
51  delayCC_(true),
52  lastGoalMotion_(NULL),
53  prune_(false),
54  pruneStatesThreshold_(0.95),
55  iterations_(0),
56  bestCost_(std::numeric_limits<double>::quiet_NaN())
57 {
59  specs_.optimizingPaths = true;
61 
62  Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
63  Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
64  Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
65  Planner::declareParam<bool>("prune", this, &RRTstar::setPrune, &RRTstar::getPrune, "0,1");
66  Planner::declareParam<double>("prune_states_threshold", this, &RRTstar::setPruneStatesImprovementThreshold, &RRTstar::getPruneStatesImprovementThreshold, "0.:.01:1.");
67 
68  addPlannerProgressProperty("iterations INTEGER",
69  boost::bind(&RRTstar::getIterationCount, this));
70  addPlannerProgressProperty("best cost REAL",
71  boost::bind(&RRTstar::getBestCost, this));
72 }
73 
74 ompl::geometric::RRTstar::~RRTstar()
75 {
76  freeMemory();
77 }
78 
80 {
81  Planner::setup();
84  if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
85  {
86  OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
87  }
88 
89  if (!nn_)
90  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
91  nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
92 
93  // Setup optimization objective
94  //
95  // If no optimization objective was specified, then default to
96  // optimizing path length as computed by the distance() function
97  // in the state space.
98  if (pdef_)
99  {
100  if (pdef_->hasOptimizationObjective())
101  opt_ = pdef_->getOptimizationObjective();
102  else
103  {
104  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
106  }
107  }
108  else
109  {
110  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
111  setup_ = false;
112  }
113 }
114 
116 {
117  Planner::clear();
118  sampler_.reset();
119  freeMemory();
120  if (nn_)
121  nn_->clear();
122 
123  lastGoalMotion_ = NULL;
124  goalMotions_.clear();
125 
126  iterations_ = 0;
127  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
128 }
129 
131 {
132  checkValidity();
133  base::Goal *goal = pdef_->getGoal().get();
134  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
135 
136  bool symCost = opt_->isSymmetric();
137 
138  while (const base::State *st = pis_.nextStart())
139  {
140  Motion *motion = new Motion(si_);
141  si_->copyState(motion->state, st);
142  motion->cost = opt_->identityCost();
143  nn_->add(motion);
144  startMotion_ = motion;
145  }
146 
147  if (nn_->size() == 0)
148  {
149  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
151  }
152 
153  if (!sampler_)
154  sampler_ = si_->allocStateSampler();
155 
156  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
157 
158  if (prune_ && !si_->getStateSpace()->isMetricSpace())
159  OMPL_WARN("%s: tree pruning was activated but since the state space %s is not a metric space, "
160  "the optimization objective might not satisfy the triangle inequality. You may need to disable pruning."
161  , getName().c_str(), si_->getStateSpace()->getName().c_str());
162 
163  const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
164 
165  Motion *solution = lastGoalMotion_;
166 
167  // \TODO Make this variable unnecessary, or at least have it
168  // persist across solve runs
169  base::Cost bestCost = opt_->infiniteCost();
170 
171  bestCost_ = opt_->infiniteCost();
172 
173  Motion *approximation = NULL;
174  double approximatedist = std::numeric_limits<double>::infinity();
175  bool sufficientlyShort = false;
176 
177  Motion *rmotion = new Motion(si_);
178  base::State *rstate = rmotion->state;
179  base::State *xstate = si_->allocState();
180 
181  // e+e/d. K-nearest RRT*
182  double k_rrg = boost::math::constants::e<double>() +
183  (boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension());
184 
185  std::vector<Motion*> nbh;
186 
187  std::vector<base::Cost> costs;
188  std::vector<base::Cost> incCosts;
189  std::vector<std::size_t> sortedCostIndices;
190 
191  std::vector<int> valid;
192  unsigned int rewireTest = 0;
193  unsigned int statesGenerated = 0;
194 
195  if (solution)
196  OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(), solution->cost.value());
197  OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(), (unsigned int)std::ceil(k_rrg * log((double)(nn_->size() + 1))));
198 
199  // our functor for sorting nearest neighbors
200  CostIndexCompare compareFn(costs, *opt_);
201 
202  while (ptc == false)
203  {
204  iterations_++;
205 
206  // sample random state (with goal biasing)
207  // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
208  if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
209  goal_s->sampleGoal(rstate);
210  else
211  {
212  sampler_->sampleUniform(rstate);
213 
214  if (prune_ && opt_->isCostBetterThan(bestCost_, costToGo(rmotion)))
215  continue;
216  }
217 
218  // find closest state in the tree
219  Motion *nmotion = nn_->nearest(rmotion);
220 
221  if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
222  continue;
223 
224  base::State *dstate = rstate;
225 
226  // find state to add to the tree
227  double d = si_->distance(nmotion->state, rstate);
228  if (d > maxDistance_)
229  {
230  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
231  dstate = xstate;
232  }
233 
234  // Check if the motion between the nearest state and the state to add is valid
235  if (si_->checkMotion(nmotion->state, dstate))
236  {
237  // create a motion
238  Motion *motion = new Motion(si_);
239  si_->copyState(motion->state, dstate);
240  motion->parent = nmotion;
241  motion->incCost = opt_->motionCost(nmotion->state, motion->state);
242  motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
243 
244  // Find nearby neighbors of the new motion - k-nearest RRT*
245  unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
246  nn_->nearestK(motion, k, nbh);
247  rewireTest += nbh.size();
248  statesGenerated++;
249 
250  // cache for distance computations
251  //
252  // Our cost caches only increase in size, so they're only
253  // resized if they can't fit the current neighborhood
254  if (costs.size() < nbh.size())
255  {
256  costs.resize(nbh.size());
257  incCosts.resize(nbh.size());
258  sortedCostIndices.resize(nbh.size());
259  }
260 
261  // cache for motion validity (only useful in a symmetric space)
262  //
263  // Our validity caches only increase in size, so they're
264  // only resized if they can't fit the current neighborhood
265  if (valid.size() < nbh.size())
266  valid.resize(nbh.size());
267  std::fill(valid.begin(), valid.begin() + nbh.size(), 0);
268 
269  // Finding the nearest neighbor to connect to
270  // By default, neighborhood states are sorted by cost, and collision checking
271  // is performed in increasing order of cost
272  if (delayCC_)
273  {
274  // calculate all costs and distances
275  for (std::size_t i = 0 ; i < nbh.size(); ++i)
276  {
277  incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
278  costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
279  }
280 
281  // sort the nodes
282  //
283  // we're using index-value pairs so that we can get at
284  // original, unsorted indices
285  for (std::size_t i = 0; i < nbh.size(); ++i)
286  sortedCostIndices[i] = i;
287  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + nbh.size(),
288  compareFn);
289 
290  // collision check until a valid motion is found
291  //
292  // ASYMMETRIC CASE: it's possible that none of these
293  // neighbors are valid. This is fine, because motion
294  // already has a connection to the tree through
295  // nmotion (with populated cost fields!).
296  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
297  i != sortedCostIndices.begin() + nbh.size();
298  ++i)
299  {
300  if (nbh[*i] == nmotion || si_->checkMotion(nbh[*i]->state, motion->state))
301  {
302  motion->incCost = incCosts[*i];
303  motion->cost = costs[*i];
304  motion->parent = nbh[*i];
305  valid[*i] = 1;
306  break;
307  }
308  else valid[*i] = -1;
309  }
310  }
311  else // if not delayCC
312  {
313  motion->incCost = opt_->motionCost(nmotion->state, motion->state);
314  motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
315  // find which one we connect the new state to
316  for (std::size_t i = 0 ; i < nbh.size(); ++i)
317  {
318  if (nbh[i] != nmotion)
319  {
320  incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
321  costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
322  if (opt_->isCostBetterThan(costs[i], motion->cost))
323  {
324  if (si_->checkMotion(nbh[i]->state, motion->state))
325  {
326  motion->incCost = incCosts[i];
327  motion->cost = costs[i];
328  motion->parent = nbh[i];
329  valid[i] = 1;
330  }
331  else valid[i] = -1;
332  }
333  }
334  else
335  {
336  incCosts[i] = motion->incCost;
337  costs[i] = motion->cost;
338  valid[i] = 1;
339  }
340  }
341  }
342 
343  if (prune_)
344  {
345  if (opt_->isCostBetterThan(costToGo(motion, false), bestCost_))
346  {
347  nn_->add(motion);
348  motion->parent->children.push_back(motion);
349  }
350  else // If the new motion does not improve the best cost it is ignored.
351  {
352  --statesGenerated;
353  si_->freeState(motion->state);
354  delete motion;
355  continue;
356  }
357  }
358  else
359  {
360  // add motion to the tree
361  nn_->add(motion);
362  motion->parent->children.push_back(motion);
363  }
364 
365  bool checkForSolution = false;
366  for (std::size_t i = 0; i < nbh.size(); ++i)
367  {
368  if (nbh[i] != motion->parent)
369  {
370  base::Cost nbhIncCost;
371  if (symCost)
372  nbhIncCost = incCosts[i];
373  else
374  nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state);
375  base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost);
376  if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost))
377  {
378  bool motionValid;
379  if (valid[i] == 0)
380  motionValid = si_->checkMotion(motion->state, nbh[i]->state);
381  else
382  motionValid = (valid[i] == 1);
383 
384  if (motionValid)
385  {
386  // Remove this node from its parent list
387  removeFromParent (nbh[i]);
388 
389  // Add this node to the new parent
390  nbh[i]->parent = motion;
391  nbh[i]->incCost = nbhIncCost;
392  nbh[i]->cost = nbhNewCost;
393  nbh[i]->parent->children.push_back(nbh[i]);
394 
395  // Update the costs of the node's children
396  updateChildCosts(nbh[i]);
397 
398  checkForSolution = true;
399  }
400  }
401  }
402  }
403 
404  // Add the new motion to the goalMotion_ list, if it satisfies the goal
405  double distanceFromGoal;
406  if (goal->isSatisfied(motion->state, &distanceFromGoal))
407  {
408  goalMotions_.push_back(motion);
409  checkForSolution = true;
410  }
411 
412  // Checking for solution or iterative improvement
413  if (checkForSolution)
414  {
415  bool updatedSolution = false;
416  for (size_t i = 0; i < goalMotions_.size(); ++i)
417  {
418  if (opt_->isCostBetterThan(goalMotions_[i]->cost, bestCost))
419  {
420  bestCost = goalMotions_[i]->cost;
421  bestCost_ = bestCost;
422  updatedSolution = true;
423  }
424 
425  sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->cost);
426  if (sufficientlyShort)
427  {
428  solution = goalMotions_[i];
429  break;
430  }
431  else if (!solution ||
432  opt_->isCostBetterThan(goalMotions_[i]->cost,solution->cost))
433  {
434  solution = goalMotions_[i];
435  updatedSolution = true;
436  }
437  }
438 
439  if (updatedSolution)
440  {
441  if (prune_)
442  {
443  int n = pruneTree(bestCost_);
444  statesGenerated -= n;
445  }
446 
447  if (intermediateSolutionCallback)
448  {
449  std::vector<const base::State *> spath;
450  Motion *intermediate_solution = solution->parent; // Do not include goal state to simplify code.
451 
452  do
453  {
454  spath.push_back(intermediate_solution->state);
455  intermediate_solution = intermediate_solution->parent;
456  } while (intermediate_solution->parent != 0); // Do not include the start state.
457 
458  intermediateSolutionCallback(this, spath, bestCost_);
459  }
460  }
461  }
462 
463  // Checking for approximate solution (closest state found to the goal)
464  if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
465  {
466  approximation = motion;
467  approximatedist = distanceFromGoal;
468  }
469  }
470 
471  // terminate if a sufficient solution is found
472  if (solution && sufficientlyShort)
473  break;
474  }
475 
476  bool approximate = (solution == NULL);
477  bool addedSolution = false;
478  if (approximate)
479  solution = approximation;
480  else
481  lastGoalMotion_ = solution;
482 
483  if (solution != NULL)
484  {
485  ptc.terminate();
486  // construct the solution path
487  std::vector<Motion*> mpath;
488  while (solution != NULL)
489  {
490  mpath.push_back(solution);
491  solution = solution->parent;
492  }
493 
494  // set the solution path
495  PathGeometric *geoPath = new PathGeometric(si_);
496  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
497  geoPath->append(mpath[i]->state);
498 
499  base::PathPtr path(geoPath);
500  // Add the solution path.
501  base::PlannerSolution psol(path);
502  psol.setPlannerName(getName());
503  if (approximate)
504  psol.setApproximate(approximatedist);
505  // Does the solution satisfy the optimization objective?
506  psol.setOptimized(opt_, bestCost, sufficientlyShort);
507  pdef_->addSolutionPath(psol);
508 
509  addedSolution = true;
510  }
511 
512  si_->freeState(xstate);
513  if (rmotion->state)
514  si_->freeState(rmotion->state);
515  delete rmotion;
516 
517  OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree.", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size());
518 
519  return base::PlannerStatus(addedSolution, approximate);
520 }
521 
523 {
524  for (std::vector<Motion*>::iterator it = m->parent->children.begin ();
525  it != m->parent->children.end (); ++it)
526  if (*it == m)
527  {
528  m->parent->children.erase(it);
529  break;
530  }
531 }
532 
534 {
535  for (std::size_t i = 0; i < m->children.size(); ++i)
536  {
537  m->children[i]->cost = opt_->combineCosts(m->cost, m->children[i]->incCost);
538  updateChildCosts(m->children[i]);
539  }
540 }
541 
543 {
544  if (nn_)
545  {
546  std::vector<Motion*> motions;
547  nn_->list(motions);
548  for (std::size_t i = 0 ; i < motions.size() ; ++i)
549  {
550  if (motions[i]->state)
551  si_->freeState(motions[i]->state);
552  delete motions[i];
553  }
554  }
555 }
556 
558 {
559  Planner::getPlannerData(data);
560 
561  std::vector<Motion*> motions;
562  if (nn_)
563  nn_->list(motions);
564 
565  if (lastGoalMotion_)
566  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
567 
568  for (std::size_t i = 0 ; i < motions.size() ; ++i)
569  {
570  if (motions[i]->parent == NULL)
571  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
572  else
573  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
574  base::PlannerDataVertex(motions[i]->state));
575  }
576 }
577 
579 {
580  const int tree_size = nn_->size();
581  pruneScratchSpace_.newTree.reserve(tree_size);
582  pruneScratchSpace_.newTree.clear();
583  pruneScratchSpace_.toBePruned.reserve(tree_size);
584  pruneScratchSpace_.toBePruned.clear();
585  pruneScratchSpace_.candidates.clear();
586  pruneScratchSpace_.candidates.push_back(startMotion_);
587  std::size_t j = 0;
588  while (j != pruneScratchSpace_.candidates.size())
589  {
590  Motion *candidate = pruneScratchSpace_.candidates[j++];
591  if (opt_->isCostBetterThan(pruneTreeCost, costToGo(candidate)))
592  pruneScratchSpace_.toBePruned.push_back(candidate);
593  else
594  {
595  pruneScratchSpace_.newTree.push_back(candidate);
596  pruneScratchSpace_.candidates.insert(pruneScratchSpace_.candidates.end(),
597  candidate->children.begin(), candidate->children.end());
598  }
599  }
600 
601  // To create the new nn takes one order of magnitude in time more than just checking how many
602  // states would be pruned. Therefore, only prune if it removes a significant amount of states.
603  if ((double)pruneScratchSpace_.newTree.size() / tree_size < pruneStatesThreshold_)
604  {
605  for (std::size_t i = 0; i < pruneScratchSpace_.toBePruned.size(); ++i)
606  deleteBranch(pruneScratchSpace_.toBePruned[i]);
607 
608  nn_->clear();
609  nn_->add(pruneScratchSpace_.newTree);
610 
611  return (tree_size - pruneScratchSpace_.newTree.size());
612  }
613  return 0;
614 }
615 
617 {
618  removeFromParent(motion);
619 
620  std::vector<Motion *>& toDelete = pruneScratchSpace_.candidates;
621  toDelete.clear();
622  toDelete.push_back(motion);
623 
624  while (!toDelete.empty())
625  {
626  Motion *mto_delete = toDelete.back();
627  toDelete.pop_back();
628 
629  for(std::size_t i = 0; i < mto_delete->children.size(); ++i)
630  toDelete.push_back(mto_delete->children[i]);
631 
632  si_->freeState(mto_delete->state);
633  delete mto_delete;
634  }
635 }
636 
637 ompl::base::Cost ompl::geometric::RRTstar::costToGo(const Motion *motion, const bool shortest) const
638 {
639  base::Cost costToCome;
640  if (shortest)
641  costToCome = opt_->motionCost(startMotion_->state, motion->state); // h_s
642  else
643  costToCome = motion->cost; //d_s
644 
645  const base::Cost costToGo = base::goalRegionCostToGo(motion->state, pdef_->getGoal().get()); // h_g
646  return opt_->combineCosts(costToCome, costToGo); // h_s + h_g
647 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
double getRange() const
Get the range the planner is using.
Definition: RRTstar.h:121
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:391
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:146
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:105
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTConnect.h:164
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:115
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:104
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:140
Representation of a solution to a planning problem.
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:533
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:241
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:63
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:213
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:115
Cost goalRegionCostToGo(const State *state, const Goal *goal)
For use when goal region&#39;s distanceGoal() is equivalent to the cost-to-go of a state under the optimi...
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTConnect.cpp:71
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths...
Definition: Planner.h:228
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTstar.cpp:79
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
Representation of a motion.
Definition: RRTstar.h:192
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:173
base::State * state
The state contained by the motion.
Definition: RRTstar.h:207
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:542
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:418
void setPruneStatesImprovementThreshold(const double pp)
Set the percentage threshold (between 0 and 1) for pruning the tree. If the new tree has removed at l...
Definition: RRTstar.h:165
Abstract definition of a goal region that can be sampled.
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
#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.
void deleteBranch(Motion *motion)
Deletes (frees memory) the motion and its children motions.
Definition: RRTstar.cpp:616
boost::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...
void setPrune(const bool prune)
Controls whether the tree is pruned during the search.
Definition: RRTstar.h:152
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:578
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...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: RRTstar.cpp:557
An optimization objective which corresponds to optimizing path length.
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:176
Definition of an abstract state.
Definition: State.h:50
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:100
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:403
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:219
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL 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...
double getPruneStatesImprovementThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:171
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
base::Cost costToGo(const Motion *motion, const bool shortest=true) const
Computes the Cost To Go heuristically as the cost to come from start to motion plus the cost to go fr...
Definition: RRTstar.cpp:637
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:210
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
bool getPrune() const
Get the state of the pruning option.
Definition: RRTstar.h:158
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: RRTstar.cpp:130
double value() const
The value of the cost.
Definition: Cost.h:54
void removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTstar.cpp:522
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:99
Definition of a geometric path.
Definition: PathGeometric.h:60
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns...
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:216
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68