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:
79  RRTstar(const base::SpaceInformationPtr &si);
80 
81  ~RRTstar() override;
82 
83  void getPlannerData(base::PlannerData &data) const override;
84 
85  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
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>
144  void setNearestNeighbors()
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 
252  bool getAdmissibleCostToCome() const
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 
350  Motion *parent;
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
376  struct CostIndexCompare
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 
428  base::StateSamplerPtr sampler_;
429 
431  base::InformedSamplerPtr infSampler_;
432 
434  std::shared_ptr<NearestNeighbors<Motion *>> nn_;
435 
438  double goalBias_{.05};
439 
441  double maxDistance_{0.};
442 
444  RNG rng_;
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 
463  base::OptimizationObjectivePtr opt_;
464 
466  Motion *lastGoalMotion_{nullptr};
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 
484  bool useRejectionSampling_{false};
485 
488 
490  bool useAdmissibleCostToCome_{true};
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
base::InformedSamplerPtr infSampler_
An informed sampler.
Definition: RRTstar.h:527
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
unsigned int batchSize_
The size of the batches.
Definition: RRTstar.h:595
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition: RRTstar.h:342
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:608
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:408
RNG rng_
The random number generator.
Definition: RRTstar.h:540
void allocSampler()
Create the samplers.
Definition: RRTstar.cpp:1073
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:660
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition: RRTstar.cpp:604
Definition: RRTstar.h:472
double pruneThreshold_
The tree is pruned when the change in solution cost is greater than this fraction.
Definition: RRTstar.h:571
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1009
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition: RRTstar.h:550
Definition of an abstract state.
Definition: State.h:113
bool useInformedSampling_
Option to use informed sampling.
Definition: RRTstar.h:577
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition: RRTstar.cpp:1102
bool getOrderedSampling() const
Get the state of sample ordering.
Definition: RRTstar.h:358
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition: RRTstar.h:456
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:547
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition: RRTstar.h:530
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:587
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition: RRTstar.h:291
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition: RRTstar.h:313
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
Definition: RRTstar.h:436
base::Cost cost
The cost up to this motion.
Definition: RRTstar.h:452
unsigned int iterations_
Number of iterations the algorithm performed.
Definition: RRTstar.h:611
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:556
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: RRTstar.h:240
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition: RRTstar.h:303
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition: RRTstar.h:589
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:381
base::StateSamplerPtr sampler_
State sampler.
Definition: RRTstar.h:524
Motion * parent
The parent motion in the exploration tree.
Definition: RRTstar.h:446
std::vector< Motion * > startMotions_
Stores the start states as Motions.
Definition: RRTstar.h:598
double r_rrt_
A constant for r-disc rewiring calculations.
Definition: RRTstar.h:553
Abstract definition of optimization objectives.
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition: RRTstar.h:580
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition: RRTstar.h:364
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:898
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:574
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition: RRTstar.h:322
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1123
void setGoalBias(double goalBias)
Set the goal bias.
Definition: RRTstar.h:196
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:640
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition: RRTstar.h:262
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:233
bool inGoal
Set to true if this vertex is in the goal region.
Definition: RRTstar.h:449
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition: RRTstar.h:543
bool getTreePruning() const
Get the state of the pruning option.
Definition: RRTstar.h:277
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition: RRTstar.h:256
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRTstar.h:202
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:583
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:872
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition: RRTstar.h:565
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:534
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:918
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:616
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: RRTstar.h:537
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:225
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition: RRTstar.cpp:961
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRTstar.h:562
#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:1045
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: RRTstar.h:601
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition: RRTstar.h:402
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:218
Representation of a motion.
Definition: RRTstar.h:431
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition: RRTstar.h:414
void freeMemory()
Free the memory allocated by this planner.
Definition: RRTstar.cpp:625
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition: RRTstar.h:335
base::OptimizationObjectivePtr opt_
Objective we're optimizing.
Definition: RRTstar.h:559
void setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition: RRTstar.h:396
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRTstar.h:487
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:470
bool useAdmissibleCostToCome_
The admissibility of the new-state rejection heuristic.
Definition: RRTstar.h:586
bool useOrderedSampling_
Option to create batches of samples and order them.
Definition: RRTstar.h:592
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition: RRTstar.h:348
bool useTreePruning_
The status of the tree pruning option.
Definition: RRTstar.h:568
void addChildrenToList(std::queue< Motion *, std::deque< Motion * >> *motionList, Motion *motion)
Add the children of a vertex to the given list.
Definition: RRTstar.cpp:857
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:285
base::State * state
The state contained by the motion.
Definition: RRTstar.h:443
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: RRTstar.h:212
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition: RRTstar.h:329
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: RRTstar.h:459
Main namespace. Contains everything in this library.
Definition: Cost.h:42
base::Cost prunedCost_
The cost at which the graph was last pruned.
Definition: RRTstar.h:604
bool getFocusSearch() const
Get the state of search focusing.
Definition: RRTstar.h:390
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:865
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition: RRTstar.h:370