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  {
74  class RRTstar : public base::Planner
75  {
76  public:
77  RRTstar(const base::SpaceInformationPtr &si);
78 
79  ~RRTstar() override;
80 
81  void getPlannerData(base::PlannerData &data) const override;
82 
83  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
84 
85  void clear() override;
86 
87  void setup() override;
88 
98  void setGoalBias(double goalBias)
99  {
100  goalBias_ = goalBias;
101  }
102 
104  double getGoalBias() const
105  {
106  return goalBias_;
107  }
108 
114  void setRange(double distance)
115  {
116  maxDistance_ = distance;
117  }
118 
120  double getRange() const
121  {
122  return maxDistance_;
123  }
124 
127  void setRewireFactor(double rewireFactor)
128  {
129  rewireFactor_ = rewireFactor;
131  }
132 
135  double getRewireFactor() const
136  {
137  return rewireFactor_;
138  }
139 
141  template <template <typename T> class NN>
142  void setNearestNeighbors()
143  {
144  if (nn_ && nn_->size() != 0)
145  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
146  clear();
147  nn_ = std::make_shared<NN<Motion *>>();
148  setup();
149  }
150 
158  void setDelayCC(bool delayCC)
159  {
160  delayCC_ = delayCC;
161  }
162 
164  bool getDelayCC() const
165  {
166  return delayCC_;
167  }
168 
176  void setTreePruning(bool prune);
177 
179  bool getTreePruning() const
180  {
181  return useTreePruning_;
182  }
183 
187  void setPruneThreshold(const double pp)
188  {
189  pruneThreshold_ = pp;
190  }
191 
193  double getPruneThreshold() const
194  {
195  return pruneThreshold_;
196  }
197 
202  void setPrunedMeasure(bool informedMeasure);
203 
205  bool getPrunedMeasure() const
206  {
207  return usePrunedMeasure_;
208  }
209 
212  void setInformedSampling(bool informedSampling);
213 
215  bool getInformedSampling() const
216  {
217  return useInformedSampling_;
218  }
219 
221  void setSampleRejection(bool reject);
222 
224  bool getSampleRejection() const
225  {
226  return useRejectionSampling_;
227  }
228 
231  void setNewStateRejection(const bool reject)
232  {
233  useNewStateRejection_ = reject;
234  }
235 
237  bool getNewStateRejection() const
238  {
239  return useNewStateRejection_;
240  }
241 
244  void setAdmissibleCostToCome(const bool admissible)
245  {
246  useAdmissibleCostToCome_ = admissible;
247  }
248 
251  {
253  }
254 
257  void setOrderedSampling(bool orderSamples);
258 
260  bool getOrderedSampling() const
261  {
262  return useOrderedSampling_;
263  }
264 
266  void setBatchSize(unsigned int batchSize)
267  {
268  batchSize_ = batchSize;
269  }
270 
272  unsigned int getBatchSize() const
273  {
274  return batchSize_;
275  }
276 
283  void setFocusSearch(const bool focus)
284  {
286  setTreePruning(focus);
287  setPrunedMeasure(focus);
288  setNewStateRejection(focus);
289  }
290 
292  bool getFocusSearch() const
293  {
295  }
296 
298  void setKNearest(bool useKNearest)
299  {
300  useKNearest_ = useKNearest;
301  }
302 
304  bool getKNearest() const
305  {
306  return useKNearest_;
307  }
308 
310  void setNumSamplingAttempts(unsigned int numAttempts)
311  {
312  numSampleAttempts_ = numAttempts;
313  }
314 
316  unsigned int getNumSamplingAttempts() const
317  {
318  return numSampleAttempts_;
319  }
320 
321  unsigned int numIterations() const
322  {
323  return iterations_;
324  }
325 
326  ompl::base::Cost bestCost() const
327  {
328  return bestCost_;
329  }
330 
331  protected:
333  class Motion
334  {
335  public:
338  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr), inGoal(false)
339  {
340  }
341 
342  ~Motion() = default;
343 
346 
348  Motion *parent;
349 
351  bool inGoal;
352 
355 
359 
361  std::vector<Motion *> children;
362  };
363 
365  void allocSampler();
366 
368  bool sampleUniform(base::State *statePtr);
369 
371  void freeMemory();
372 
373  // For sorting a list of costs and getting only their sorted indices
374  struct CostIndexCompare
375  {
376  CostIndexCompare(const std::vector<base::Cost> &costs, const base::OptimizationObjective &opt)
377  : costs_(costs), opt_(opt)
378  {
379  }
380  bool operator()(unsigned i, unsigned j)
381  {
382  return opt_.isCostBetterThan(costs_[i], costs_[j]);
383  }
384  const std::vector<base::Cost> &costs_;
385  const base::OptimizationObjective &opt_;
386  };
387 
389  double distanceFunction(const Motion *a, const Motion *b) const
390  {
391  return si_->distance(a->state, b->state);
392  }
393 
395  void getNeighbors(Motion *motion, std::vector<Motion *> &nbh) const;
396 
398  void removeFromParent(Motion *m);
399 
401  void updateChildCosts(Motion *m);
402 
406  int pruneTree(const base::Cost &pruneTreeCost);
407 
413  base::Cost solutionHeuristic(const Motion *motion) const;
414 
416  void addChildrenToList(std::queue<Motion *, std::deque<Motion *>> *motionList, Motion *motion);
417 
420  bool keepCondition(const Motion *motion, const base::Cost &threshold) const;
421 
424 
426  base::StateSamplerPtr sampler_;
427 
429  base::InformedSamplerPtr infSampler_;
430 
432  std::shared_ptr<NearestNeighbors<Motion *>> nn_;
433 
436  double goalBias_{.05};
437 
439  double maxDistance_{0.};
440 
442  RNG rng_;
443 
445  bool useKNearest_{true};
446 
449  double rewireFactor_{1.1};
450 
452  double k_rrt_{0u};
453 
455  double r_rrt_{0.};
456 
458  bool delayCC_{true};
459 
461  base::OptimizationObjectivePtr opt_;
462 
464  Motion *bestGoalMotion_{nullptr};
465 
467  std::vector<Motion *> goalMotions_;
468 
470  bool useTreePruning_{false};
471 
473  double pruneThreshold_{.05};
474 
476  bool usePrunedMeasure_{false};
477 
479  bool useInformedSampling_{false};
480 
482  bool useRejectionSampling_{false};
483 
485  bool useNewStateRejection_{false};
486 
488  bool useAdmissibleCostToCome_{true};
489 
491  unsigned int numSampleAttempts_{100u};
492 
494  bool useOrderedSampling_{false};
495 
497  unsigned int batchSize_{1u};
498 
500  std::vector<Motion *> startMotions_;
501 
503  base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
504 
506  base::Cost prunedCost_{std::numeric_limits<double>::quiet_NaN()};
507 
510  double prunedMeasure_{0.};
511 
513  unsigned int iterations_{0u};
514 
516  // Planner progress property functions
517  std::string numIterationsProperty() const
518  {
519  return std::to_string(numIterations());
520  }
521  std::string bestCostProperty() const
522  {
523  return std::to_string(bestCost().value());
524  }
525  };
526  }
527 }
528 
529 #endif
base::InformedSamplerPtr infSampler_
An informed sampler.
Definition: RRTstar.h:521
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:86
unsigned int batchSize_
The size of the batches.
Definition: RRTstar.h:589
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition: RRTstar.h:336
double prunedMeasure_
The measure of the problem when we pruned it (if this isn't in use, it will be set to si_->getSpaceMe...
Definition: RRTstar.h:602
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:402
RNG rng_
The random number generator.
Definition: RRTstar.h:534
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1096
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:675
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTstar.cpp:619
double pruneThreshold_
The tree is pruned when the change in solution cost is greater than this fraction.
Definition: RRTstar.h:565
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1032
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: RRTstar.h:544
Definition of an abstract state.
Definition: State.h:110
bool useInformedSampling_
Option to use informed sampling.
Definition: RRTstar.h:571
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1125
bool getOrderedSampling() const
Get the state of sample ordering.
Definition: RRTstar.h:352
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:450
double rewireFactor_
The rewiring factor, s, so that r_rrt = s \times r_rrt* > r_rrt* (or k_rrt = s \times k_rrt* > k_rrt*...
Definition: RRTstar.h:541
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRTstar.h:524
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:602
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:285
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTstar.h:307
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
Definition: RRTstar.h:430
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:446
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: RRTstar.h:605
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:108
bool delayCC_
Option to delay and reduce collision checking within iterations.
Definition: RRTstar.h:550
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: RRTstar.h:234
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:297
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition: RRTstar.h:583
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:164
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:375
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTstar.h:518
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:440
std::vector< Motion * > startMotions_
Stores the start states as Motions.
Definition: RRTstar.h:592
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: RRTstar.h:547
Abstract definition of optimization objectives.
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition: RRTstar.h:574
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:358
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:921
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:144
bool usePrunedMeasure_
Option to use the informed measure.
Definition: RRTstar.h:568
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:316
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1146
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:190
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:655
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:256
double getRewireFactor() const
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* > r_rrg* (or k_rrg = s \times k_r...
Definition: RRTstar.h:227
bool inGoal
Set to true if this vertex is in the goal region.
Definition: RRTstar.h:443
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition: RRTstar.h:537
bool getTreePruning() const
Get the state of the pruning option.
Definition: RRTstar.h:271
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:250
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:196
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTstar.cpp:93
bool useNewStateRejection_
The status of the new-state rejection parameter.
Definition: RRTstar.h:577
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:895
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: RRTstar.h:559
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:528
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:941
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:631
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTstar.h:531
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg* (or k_rrg = s \times k_rrg*)
Definition: RRTstar.h:219
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTstar.cpp:984
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:65
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1068
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: RRTstar.h:595
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:396
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...
double getRange() const
Get the range the planner is using.
Definition: RRTstar.h:212
Representation of a motion.
Definition: RRTstar.h:425
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:408
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:640
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:329
base::OptimizationObjectivePtr opt_
Objective we're optimizing.
Definition: RRTstar.h:553
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:390
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:481
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:479
bool useAdmissibleCostToCome_
The admissibility of the new-state rejection heuristic.
Definition: RRTstar.h:580
bool useOrderedSampling_
Option to create batches of samples and order them.
Definition: RRTstar.h:586
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:342
bool useTreePruning_
The status of the tree pruning option.
Definition: RRTstar.h:562
void addChildrenToList(std::queue< Motion *, std::deque< Motion * >> *motionList, Motion *motion)
Add the children of a vertex to the given list.
Definition: RRTstar.cpp:874
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:279
base::State * state
The state contained by the motion.
Definition: RRTstar.h:437
Motion * bestGoalMotion_
The best goal motion.
Definition: RRTstar.h:556
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:206
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition: RRTstar.h:323
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:453
Main namespace. Contains everything in this library.
Definition: AppBase.h:20
base::Cost prunedCost_
The cost at which the graph was last pruned.
Definition: RRTstar.h:598
bool getFocusSearch() const
Get the state of search focusing.
Definition: RRTstar.h:384
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:882
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:364