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_;
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:640
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:1045
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:961
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:616
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:918
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
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRTstar.h:466
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:625
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:587
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition: RRTstar.cpp:1009
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:58
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:865
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:660
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:857
#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:1102
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:1073
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition: RRTstar.cpp:1123
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 removeFromParent(Motion *m)
Removes the given motion from the parent&#39;s child list.
Definition: RRTstar.cpp:604
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:872
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