Loading...
Searching...
No Matches
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
51namespace 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
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>
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
216 {
218 }
219
221 void setSampleRejection(bool reject);
222
225 {
227 }
228
231 void setNewStateRejection(const bool reject)
232 {
233 useNewStateRejection_ = reject;
234 }
235
238 {
240 }
241
244 void setAdmissibleCostToCome(const bool admissible)
245 {
246 useAdmissibleCostToCome_ = admissible;
247 }
248
251 {
253 }
254
257 void setOrderedSampling(bool orderSamples);
258
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);
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
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
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
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
465
467 std::vector<Motion *> goalMotions_;
468
470 bool useTreePruning_{false};
471
473 double pruneThreshold_{.05};
474
476 bool usePrunedMeasure_{false};
477
480
483
486
489
491 unsigned int numSampleAttempts_{100u};
492
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of optimization objectives.
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...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:417
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition RRTstar.h:334
bool inGoal
Set to true if this vertex is in the goal region.
Definition RRTstar.h:351
base::Cost cost
The cost up to this motion.
Definition RRTstar.h:354
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
Definition RRTstar.h:358
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
Definition RRTstar.h:338
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition RRTstar.h:361
Motion * parent
The parent motion in the exploration tree.
Definition RRTstar.h:348
base::State * state
The state contained by the motion.
Definition RRTstar.h:345
Optimal Rapidly-exploring Random Trees.
Definition RRTstar.h:75
bool getAdmissibleCostToCome() const
Get the admissibility of the pruning and new-state rejection heuristic.
Definition RRTstar.h:250
bool getKNearest() const
Get the state of using a k-nearest search for rewiring.
Definition RRTstar.h:304
bool useRejectionSampling_
The status of the sample rejection parameter.
Definition RRTstar.h:482
bool useAdmissibleCostToCome_
The admissibility of the new-state rejection heuristic.
Definition RRTstar.h:488
bool getInformedSampling() const
Get the state direct heuristic sampling.
Definition RRTstar.h:215
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:187
double getGoalBias() const
Get the goal bias the planner is using.
Definition RRTstar.h:104
base::InformedSamplerPtr infSampler_
An informed sampler.
Definition RRTstar.h:429
void freeMemory()
Free the memory allocated by this planner.
Definition RRTstar.cpp:641
base::StateSamplerPtr sampler_
State sampler.
Definition RRTstar.h:426
void calculateRewiringLowerBounds()
Calculate the k_RRG* and r_RRG* terms.
Definition RRTstar.cpp:1147
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition RRTstar.cpp:145
unsigned int numSampleAttempts_
The number of attempts to make at informed sampling.
Definition RRTstar.h:491
bool getDelayCC() const
Get the state of the delayed collision checking option.
Definition RRTstar.h:164
void setGoalBias(double goalBias)
Set the goal bias.
Definition RRTstar.h:98
base::Cost bestCost_
Best cost found so far by algorithm.
Definition RRTstar.h:503
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
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:127
double k_rrt_
A constant for k-nearest rewiring calculations.
Definition RRTstar.h:452
double pruneThreshold_
The tree is pruned when the change in solution cost is greater than this fraction.
Definition RRTstar.h:473
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:449
bool getSampleRejection() const
Get the state of the sample rejection option.
Definition RRTstar.h:224
std::vector< Motion * > startMotions_
Stores the start states as Motions.
Definition RRTstar.h:500
void setBatchSize(unsigned int batchSize)
Set the batch size used for sample ordering.
Definition RRTstar.h:266
double r_rrt_
A constant for r-disc rewiring calculations.
Definition RRTstar.h:455
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
unsigned int batchSize_
The size of the batches.
Definition RRTstar.h:497
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
Definition RRTstar.h:158
bool useTreePruning_
The status of the tree pruning option.
Definition RRTstar.h:470
bool useNewStateRejection_
The status of the new-state rejection parameter.
Definition RRTstar.h:485
base::Cost prunedCost_
The cost at which the graph was last pruned.
Definition RRTstar.h:506
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition RRTstar.h:142
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
Definition RRTstar.cpp:620
bool delayCC_
Option to delay and reduce collision checking within iterations.
Definition RRTstar.h:458
void setAdmissibleCostToCome(const bool admissible)
Controls whether pruning and new-state rejection uses an admissible cost-to-come estimate or not.
Definition RRTstar.h:244
void setSampleRejection(bool reject)
Controls whether heuristic rejection is used on samples (e.g., x_rand)
Definition RRTstar.cpp:1033
void setInformedSampling(bool informedSampling)
Use direct sampling of the heuristic for the generation of random samples (e.g., x_rand)....
Definition RRTstar.cpp:985
bool getNewStateRejection() const
Get the state of the new-state rejection option.
Definition RRTstar.h:237
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
bool useInformedSampling_
Option to use informed sampling.
Definition RRTstar.h:479
unsigned int getNumSamplingAttempts() const
Get the number of attempts to make while performing rejection or informed sampling.
Definition RRTstar.h:316
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:510
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
bool sampleUniform(base::State *statePtr)
Generate a sample.
Definition RRTstar.cpp:1126
double getPruneThreshold() const
Get the current prune states percentage threshold parameter.
Definition RRTstar.h:193
bool useOrderedSampling_
Option to create batches of samples and order them.
Definition RRTstar.h:494
bool getOrderedSampling() const
Get the state of sample ordering.
Definition RRTstar.h:260
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
Definition RRTstar.h:467
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition RRTstar.h:432
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 setKNearest(bool useKNearest)
Use a k-nearest search for rewiring instead of a r-disc search.
Definition RRTstar.h:298
void allocSampler()
Create the samplers.
Definition RRTstar.cpp:1097
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
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
bool getPrunedMeasure() const
Get the state of using the pruned measure.
Definition RRTstar.h:205
void setRange(double distance)
Set the range the planner is supposed to use.
Definition RRTstar.h:114
RNG rng_
The random number generator.
Definition RRTstar.h:442
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 usePrunedMeasure_
Option to use the informed measure.
Definition RRTstar.h:476
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition RRTstar.h:439
unsigned int getBatchSize() const
Get the batch size used for sample ordering.
Definition RRTstar.h:272
Motion * bestGoalMotion_
The best goal motion.
Definition RRTstar.h:464
void setOrderedSampling(bool orderSamples)
Controls whether samples are returned in ordered by the heuristic. This is accomplished by generating...
Definition RRTstar.cpp:1069
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:135
void setNumSamplingAttempts(unsigned int numAttempts)
Set the number of attempts to make while performing rejection or informed sampling.
Definition RRTstar.h:310
base::OptimizationObjectivePtr opt_
Objective we're optimizing.
Definition RRTstar.h:461
void setNewStateRejection(const bool reject)
Controls whether heuristic rejection is used on new states before connection (e.g....
Definition RRTstar.h:231
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:436
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition RRTstar.h:389
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
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:283
bool useKNearest_
Option to use k-nearest search for rewiring.
Definition RRTstar.h:445
bool getFocusSearch() const
Get the state of search focusing.
Definition RRTstar.h:292
bool getTreePruning() const
Get the state of the pruning option.
Definition RRTstar.h:179
unsigned int iterations_
Number of iterations the algorithm performed.
Definition RRTstar.h:513
double getRange() const
Get the range the planner is using.
Definition RRTstar.h:120
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()