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 {
72
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
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
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.
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:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
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
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
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()