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 
250  bool getAdmissibleCostToCome() const
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  {
285  setInformedSampling(focus);
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 
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:525
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
unsigned int batchSize_
The size of the batches.
Definition: RRTstar.h:593
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition: RRTstar.h:340
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:606
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:406
RNG rng_
The random number generator.
Definition: RRTstar.h:538
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1097
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
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTstar.cpp:620
double pruneThreshold_
The tree is pruned when the change in solution cost is greater than this fraction.
Definition: RRTstar.h:569
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1033
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: RRTstar.h:548
Definition of an abstract state.
Definition: State.h:113
bool useInformedSampling_
Option to use informed sampling.
Definition: RRTstar.h:575
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1126
bool getOrderedSampling() const
Get the state of sample ordering.
Definition: RRTstar.h:356
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:454
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:545
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRTstar.h:528
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
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:289
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTstar.h:311
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
Definition: RRTstar.h:434
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:450
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: RRTstar.h:609
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool delayCC_
Option to delay and reduce collision checking within iterations.
Definition: RRTstar.h:554
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: RRTstar.h:238
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:301
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition: RRTstar.h:587
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
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:379
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTstar.h:522
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:444
std::vector< Motion * > startMotions_
Stores the start states as Motions.
Definition: RRTstar.h:596
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: RRTstar.h:551
Abstract definition of optimization objectives.
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition: RRTstar.h:578
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:362
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 clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: RRTstar.cpp:145
bool usePrunedMeasure_
Option to use the informed measure.
Definition: RRTstar.h:572
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:320
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1147
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:194
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:260
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:231
bool inGoal
Set to true if this vertex is in the goal region.
Definition: RRTstar.h:447
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition: RRTstar.h:541
bool getTreePruning() const
Get the state of the pruning option.
Definition: RRTstar.h:275
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:254
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:200
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRTstar.cpp:94
bool useNewStateRejection_
The status of the new-state rejection parameter.
Definition: RRTstar.h:581
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
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: RRTstar.h:563
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:532
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
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
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTstar.h:535
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:223
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTstar.cpp:985
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition: RRTstar.cpp:1069
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: RRTstar.h:599
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:400
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:216
Representation of a motion.
Definition: RRTstar.h:429
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:412
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:641
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:333
base::OptimizationObjectivePtr opt_
Objective we're optimizing.
Definition: RRTstar.h:557
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:394
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:485
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
bool useAdmissibleCostToCome_
The admissibility of the new-state rejection heuristic.
Definition: RRTstar.h:584
bool useOrderedSampling_
Option to create batches of samples and order them.
Definition: RRTstar.h:590
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:346
bool useTreePruning_
The status of the tree pruning option.
Definition: RRTstar.h:566
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
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:283
base::State * state
The state contained by the motion.
Definition: RRTstar.h:441
Motion * bestGoalMotion_
The best goal motion.
Definition: RRTstar.h:560
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:210
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition: RRTstar.h:327
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:457
Main namespace. Contains everything in this library.
base::Cost prunedCost_
The cost at which the graph was last pruned.
Definition: RRTstar.h:602
bool getFocusSearch() const
Get the state of search focusing.
Definition: RRTstar.h:388
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
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:368