RRTstar.h
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 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/OptimizationObjective.h"
42 #include "ompl/datastructures/NearestNeighbors.h"
43 
44 #include <limits>
45 #include <vector>
46 #include <queue>
47 #include <deque>
48 #include <utility>
49 #include <list>
50 
51 namespace ompl
52 {
53  namespace geometric
54  {
76  class RRTstar : public base::Planner
77  {
78  public:
80 
81  ~RRTstar() override;
82 
83  void getPlannerData(base::PlannerData &data) const override;
84 
86 
87  void clear() override;
88 
89  void setup() override;
90 
100  void setGoalBias(double goalBias)
101  {
102  goalBias_ = goalBias;
103  }
104 
106  double getGoalBias() const
107  {
108  return goalBias_;
109  }
110 
116  void setRange(double distance)
117  {
118  maxDistance_ = distance;
119  }
120 
122  double getRange() const
123  {
124  return maxDistance_;
125  }
126 
129  void setRewireFactor(double rewireFactor)
130  {
131  rewireFactor_ = rewireFactor;
133  }
134 
137  double getRewireFactor() const
138  {
139  return rewireFactor_;
140  }
141 
143  template <template <typename T> class NN>
145  {
146  if (nn_ && nn_->size() != 0)
147  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
148  clear();
149  nn_ = std::make_shared<NN<Motion *>>();
150  setup();
151  }
152 
160  void setDelayCC(bool delayCC)
161  {
162  delayCC_ = delayCC;
163  }
164 
166  bool getDelayCC() const
167  {
168  return delayCC_;
169  }
170 
178  void setTreePruning(bool prune);
179 
181  bool getTreePruning() const
182  {
183  return useTreePruning_;
184  }
185 
189  void setPruneThreshold(const double pp)
190  {
191  pruneThreshold_ = pp;
192  }
193 
195  double getPruneThreshold() const
196  {
197  return pruneThreshold_;
198  }
199 
204  void setPrunedMeasure(bool informedMeasure);
205 
207  bool getPrunedMeasure() const
208  {
209  return usePrunedMeasure_;
210  }
211 
214  void setInformedSampling(bool informedSampling);
215 
217  bool getInformedSampling() const
218  {
219  return useInformedSampling_;
220  }
221 
223  void setSampleRejection(bool reject);
224 
226  bool getSampleRejection() const
227  {
228  return useRejectionSampling_;
229  }
230 
233  void setNewStateRejection(const bool reject)
234  {
235  useNewStateRejection_ = reject;
236  }
237 
239  bool getNewStateRejection() const
240  {
241  return useNewStateRejection_;
242  }
243 
246  void setAdmissibleCostToCome(const bool admissible)
247  {
248  useAdmissibleCostToCome_ = admissible;
249  }
250 
253  {
255  }
256 
259  void setOrderedSampling(bool orderSamples);
260 
262  bool getOrderedSampling() const
263  {
264  return useOrderedSampling_;
265  }
266 
268  void setBatchSize(unsigned int batchSize)
269  {
270  batchSize_ = batchSize;
271  }
272 
274  unsigned int getBatchSize() const
275  {
276  return batchSize_;
277  }
278 
285  void setFocusSearch(const bool focus)
286  {
287  setInformedSampling(focus);
288  setTreePruning(focus);
289  setPrunedMeasure(focus);
290  setNewStateRejection(focus);
291  }
292 
294  bool getFocusSearch() const
295  {
297  }
298 
300  void setKNearest(bool useKNearest)
301  {
302  useKNearest_ = useKNearest;
303  }
304 
306  bool getKNearest() const
307  {
308  return useKNearest_;
309  }
310 
312  void setNumSamplingAttempts(unsigned int numAttempts)
313  {
314  numSampleAttempts_ = numAttempts;
315  }
316 
318  unsigned int getNumSamplingAttempts() const
319  {
320  return numSampleAttempts_;
321  }
322 
323  unsigned int numIterations() const
324  {
325  return iterations_;
326  }
327 
328  ompl::base::Cost bestCost() const
329  {
330  return bestCost_;
331  }
332 
333  protected:
335  class Motion
336  {
337  public:
340  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), inGoal(false)
341  {
342  }
343 
344  ~Motion() = default;
345 
348 
351 
353  bool inGoal;
354 
357 
361 
363  std::vector<Motion *> children;
364  };
365 
367  void allocSampler();
368 
370  bool sampleUniform(base::State *statePtr);
371 
373  void freeMemory();
374 
375  // For sorting a list of costs and getting only their sorted indices
377  {
378  CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
379  : costs_(costs), opt_(opt)
380  {
381  }
382  bool operator()(unsigned i, unsigned j)
383  {
384  return opt_.isCostBetterThan(costs_[i], costs_[j]);
385  }
386  const std::vector<base::Cost> &costs_;
387  const base::OptimizationObjective &opt_;
388  };
389 
391  double distanceFunction(const Motion *a, const Motion *b) const
392  {
393  return si_->distance(a->state, b->state);
394  }
395 
397  void getNeighbors(Motion *motion, std::vector<Motion *> &nbh) const;
398 
400  void removeFromParent(Motion *m);
401 
403  void updateChildCosts(Motion *m);
404 
408  int pruneTree(const base::Cost &pruneTreeCost);
409 
415  base::Cost solutionHeuristic(const Motion *motion) const;
416 
418  void addChildrenToList(std::queue<Motion *, std::deque<Motion *>> *motionList, Motion *motion);
419 
422  bool keepCondition(const Motion *motion, const base::Cost &threshold) const;
423 
426 
429 
431  base::InformedSamplerPtr infSampler_;
432 
434  std::shared_ptr<NearestNeighbors<Motion *>> nn_;
435 
438  double goalBias_{.05};
439 
441  double maxDistance_{0.};
442 
445 
447  bool useKNearest_{true};
448 
451  double rewireFactor_{1.1};
452 
454  double k_rrt_{0u};
455 
457  double r_rrt_{0.};
458 
460  bool delayCC_{true};
461 
464 
467 
469  std::vector<Motion *> goalMotions_;
470 
472  bool useTreePruning_{false};
473 
475  double pruneThreshold_{.05};
476 
478  bool usePrunedMeasure_{false};
479 
481  bool useInformedSampling_{false};
482 
485 
488 
491 
493  unsigned int numSampleAttempts_{100u};
494 
496  bool useOrderedSampling_{false};
497 
499  unsigned int batchSize_{1u};
500 
502  std::vector<Motion *> startMotions_;
503 
505  base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
506 
508  base::Cost prunedCost_{std::numeric_limits<double>::quiet_NaN()};
509 
512  double prunedMeasure_{0.};
513 
515  unsigned int iterations_{0u};
516 
518  // Planner progress property functions
519  std::string numIterationsProperty() const
520  {
521  return std::to_string(numIterations());
522  }
523  std::string bestCostProperty() const
524  {
525  return std::to_string(bestCost().value());
526  }
527  };
528  }
529 }
530 
531 #endif
double getRange() const
Get the range the planner is using.
Definition: RRTstar.h:122
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: RRTstar.cpp:656
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h: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
Optimal Rapidly-exploring Random Trees.
Definition: RRTstar.h:76
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:226
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition: RRTstar.h:484
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 setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1069
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:985
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:632
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:306
base::OptimizationObjectivePtr opt_
Objective we&#39;re optimizing.
Definition: RRTstar.h:463
A shared pointer wrapper for ompl::base::StateSampler.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:391
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:942
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: RRTstar.h:438
bool useAdmissibleCostToCome_
The admissibility of the new-state rejection heuristic.
Definition: RRTstar.h:490
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...
bool useNewStateRejection_
The status of the new-state rejection parameter.
Definition: RRTstar.h:487
bool useTreePruning_
The status of the tree pruning option.
Definition: RRTstar.h:472
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:116
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: RRTstar.h:469
bool inGoal
Set to true if this vertex is in the goal region.
Definition: RRTstar.h:353
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
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: RRTstar.h:505
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
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTstar.h:441
Representation of a motion.
Definition: RRTstar.h:335
base::State * state
The state contained by the motion.
Definition: RRTstar.h:347
base::Cost prunedCost_
The cost at which the graph was last pruned.
Definition: RRTstar.h:508
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:641
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:312
unsigned int batchSize_
The size of the batches.
Definition: RRTstar.h:499
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void getNeighbors(Motion *motion, std::vector< Motion *> &nbh) const
Gets the neighbours of a given motion, using either k-nearest of radius as appropriate.
Definition: RRTstar.cpp:603
Motion * bestGoalMotion_
The best goal motion.
Definition: RRTstar.h:466
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1033
double pruneThreshold_
The tree is pruned when the change in solution cost is greater than this fraction.
Definition: RRTstar.h:475
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
bool useOrderedSampling_
Option to create batches of samples and order them.
Definition: RRTstar.h:496
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:883
Base class for a planner.
Definition: Planner.h:223
int pruneTree(const base::Cost &pruneTreeCost)
Prunes all those states which estimated total cost is higher than pruneTreeCost. Returns the number o...
Definition: RRTstar.cpp:676
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
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:274
A shared pointer wrapper for ompl::base::SpaceInformation.
double prunedMeasure_
The measure of the problem when we pruned it (if this isn&#39;t in use, it will be set to si_->getSpaceMe...
Definition: RRTstar.h:512
Definition of an abstract state.
Definition: State.h:49
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:875
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:195
base::InformedSamplerPtr infSampler_
An informed sampler.
Definition: RRTstar.h:431
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:207
Abstract definition of optimization objectives.
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTstar.h:428
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: RRTstar.h:144
A shared pointer wrapper for ompl::base::OptimizationObjective.
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:363
RNG rng_
The random number generator.
Definition: RRTstar.h:444
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: RRTstar.h:457
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:350
double rewireFactor_
The rewiring factor, s, so that r_rrt = s r_rrt* > r_rrt* (or k_rrt = s k_rrt* > k_rrt*) ...
Definition: RRTstar.h:451
bool delayCC_
Option to delay and reduce collision checking within iterations.
Definition: RRTstar.h:460
bool useInformedSampling_
Option to use informed sampling.
Definition: RRTstar.h:481
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1126
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: RRTstar.h:454
std::vector< Motion * > startMotions_
Stores the start states as Motions.
Definition: RRTstar.h:502
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1097
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1147
void setTreePruning(bool prune)
Controls whether the tree is pruned during the search. This pruning removes a vertex if and only if i...
Definition: RRTstar.cpp:922
void removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTstar.cpp:620
bool usePrunedMeasure_
Option to use the informed measure.
Definition: RRTstar.h:478
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRTstar.h:434
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
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
Definition: RRTstar.h:340
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 clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:145
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:896
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition: RRTstar.h:447
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: RRTstar.h:515
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition: RRTstar.h:493