Loading...
Searching...
No Matches
ImplicitGraph.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 University of Toronto 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: Jonathan Gammell, Marlin Strub */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_
38#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_
39
40#include "ompl/base/Cost.h"
41#include "ompl/base/OptimizationObjective.h"
42#include "ompl/datastructures/NearestNeighbors.h"
43#include "ompl/geometric/planners/informedtrees/BITstar.h"
44
45namespace ompl
46{
47 namespace geometric
48 {
57 {
58 public:
59 // ---
60 // Construction, initialization and destruction.
61 // ---
62
64 ImplicitGraph(NameFunc nameFunc);
65
67 virtual ~ImplicitGraph() = default;
68
71 void setup(const ompl::base::SpaceInformationPtr &spaceInformation,
72 const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper,
73 SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr,
75
77 void reset();
78
79 // ---
80 // Information access.
81 // ---
82
84 bool hasAStart() const;
85
87 bool hasAGoal() const;
88
90 VertexPtrVector::const_iterator startVerticesBeginConst() const;
91
93 VertexPtrVector::const_iterator startVerticesEndConst() const;
94
96 VertexPtrVector::const_iterator goalVerticesBeginConst() const;
97
99 VertexPtrVector::const_iterator goalVerticesEndConst() const;
100
103
105 bool hasInformedMeasure() const;
106
108 double getInformedMeasure(const ompl::base::Cost &cost) const;
109
111 double distance(const VertexConstPtr &a, const VertexConstPtr &b) const;
112
114 double distance(const VertexConstPtrPair &vertices) const;
115
117 void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples);
118
121
124
126 double smallestDistanceToGoal() const;
127
129 unsigned int getConnectivityK() const;
130
132 double getConnectivityR() const;
133
136
137 // ---
138 // Modification.
139 // ---
140
143 void registerSolutionCost(const ompl::base::Cost &solutionCost);
144
148 const base::PlannerTerminationCondition &terminationCondition);
149
152 void addNewSamples(const unsigned int &numSamples);
153
156 std::pair<unsigned int, unsigned int> prune(double prunedMeasure);
157
159 void addToSamples(const VertexPtr &sample);
160
162 void addToSamples(const VertexPtrVector &samples);
163
165 void removeFromSamples(const VertexPtr &sample);
166
168 void pruneSample(const VertexPtr &sample);
169
171 void recycleSample(const VertexPtr &sample);
172
174 void registerAsVertex(const VertexPtr &vertex);
175
178 unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree);
179
181 std::pair<unsigned int, unsigned int> pruneVertex(const VertexPtr &vertex);
182
185 void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates);
186
187 // ---
188 // Settings.
189 // ---
190
192 void setRewireFactor(double rewireFactor);
193
195 double getRewireFactor() const;
196
198 void setUseKNearest(bool useKNearest);
199
201 bool getUseKNearest() const;
202
204 void setJustInTimeSampling(bool useJit);
205
207 bool getJustInTimeSampling() const;
208
210 void setDropSamplesOnPrune(bool dropSamples);
211
213 void setPruning(bool usePruning);
214
216 bool getDropSamplesOnPrune() const;
217
219 void setTrackApproximateSolutions(bool findApproximate);
220
222 bool getTrackApproximateSolutions() const;
223
226
229
231 template <template <typename T> class NN>
232 void setNearestNeighbors();
233
234 // ---
235 // Progress counters.
236 // ---
237
239 unsigned int numSamples() const;
240
242 unsigned int numVertices() const;
243
245 unsigned int numStatesGenerated() const;
246
248 unsigned int numVerticesConnected() const;
249
251 unsigned int numFreeStatesPruned() const;
252
254 unsigned int numVerticesDisconnected() const;
255
257 unsigned int numNearestLookups() const;
258
260 unsigned int numStateCollisionChecks() const;
261
262 // ---
263 // General helper functions.
264 // ---
265
268 bool canVertexBeDisconnected(const VertexPtr &vertex) const;
269
272 bool canSampleBePruned(const VertexPtr &sample) const;
273
274 private:
275 // ---
276 // High-level primitives updating the graph.
277 // ---
278
281 void updateSamples(const VertexConstPtr &vertex);
282
285 void updateVertexClosestToGoal();
286
287 // ---
288 // High-level primitives pruning the graph.
289 // ---
290
294 std::pair<unsigned int, unsigned int> pruneStartAndGoalVertices();
295
298 std::pair<unsigned int, unsigned int> pruneSamples();
299
300 // ---
301 // Low-level random geometric graph helper and calculations
302 // ---
303
306 void testClosestToGoal(const VertexConstPtr &vertex);
307
310 ompl::base::Cost calculateNeighbourhoodCost(const VertexConstPtr &vertex) const;
311
313 virtual void updateNearestTerms();
314
316 std::size_t computeNumberOfSamplesInInformedSet() const;
317
319 double calculateR(unsigned int numUniformSamples) const;
320
322 unsigned int calculateK(unsigned int numUniformSamples) const;
323
327 double calculateMinimumRggR() const;
328
332 double calculateMinimumRggK() const;
333
334 // ---
335 // Debug helpers.
336 // ---
337
339 void assertSetup() const;
340
341 // ---
342 // Member variables (Make all are configured in setup() and reset in reset()).
343 // ---
344
346 NameFunc nameFunc_;
347
349 bool isSetup_{false};
350
352 ompl::base::SpaceInformationPtr spaceInformation_{nullptr};
353
355 ompl::base::ProblemDefinitionPtr problemDefinition_{nullptr};
356
359 CostHelper *costHelpPtr_{nullptr};
360
363 SearchQueue *queuePtr_{nullptr};
364
366 ompl::RNG rng_;
367
369 ompl::base::InformedSamplerPtr sampler_{nullptr};
370
373 VertexPtrVector startVertices_;
374
377 VertexPtrVector goalVertices_;
378
380 VertexPtrVector prunedStartVertices_;
381
383 VertexPtrVector prunedGoalVertices_;
384
386 VertexPtrVector newSamples_;
387
389 VertexPtrNNPtr samples_{nullptr};
390
392 VertexPtrVector recycledSamples_;
393
395 unsigned int numNewSamplesInCurrentBatch_{0u};
396
400 unsigned int numUniformStates_{0u};
401
403 double r_{0.};
404
407 double k_rgg_{0.};
408
410 unsigned int k_{0u};
411
414 double approximationMeasure_{0.};
415
417 ompl::base::Cost minCost_{std::numeric_limits<double>::infinity()};
418
420 ompl::base::Cost solutionCost_{std::numeric_limits<double>::infinity()};
421
423 ompl::base::Cost sampledCost_{std::numeric_limits<double>::infinity()};
424
426 bool hasExactSolution_{false};
427
429 VertexConstPtr closestVertexToGoal_{nullptr};
430
433 double closestDistanceToGoal_{std::numeric_limits<double>::infinity()};
434
436 unsigned int numSamples_{0u};
437
439 unsigned int numVertices_{0u};
440
442 unsigned int numFreeStatesPruned_{0u};
443
445 unsigned int numVerticesDisconnected_{0u};
446
448 unsigned int numNearestNeighbours_{0u};
449
451 unsigned int numStateCollisionChecks_{0u};
452
454 const std::shared_ptr<unsigned int> approximationId_;
455
456 // ---
457 // Parameters - Set defaults in construction/setup and do not reset in clear.
458 // ---
459
461 double rewireFactor_{1.1};
462
464 bool useKNearest_{true};
465
467 bool useJustInTimeSampling_{false};
468
470 bool dropSamplesOnPrune_{false};
471
473 bool isPruningEnabled_{true};
474
476 bool findApprox_{false};
477
480 std::size_t averageNumOfAllowedFailedAttemptsWhenSampling_{2u};
481 }; // class ImplicitGraph
482 } // namespace geometric
483} // namespace ompl
484
485#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_IMPLICITGRAPH_
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
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition Planner.h:78
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:223
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
A conceptual representation of samples as an edge-implicit random geometric graph.
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...
void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
unsigned int numStatesGenerated() const
The total number of states generated.
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
void reset()
Reset the graph to the state of construction.
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
bool hasAGoal() const
Gets whether the graph contains a goal or not.
unsigned int numVertices() const
The number of vertices in the search tree.
unsigned int numStateCollisionChecks() const
The number of state collision checks.
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
bool hasAStart() const
Gets whether the graph contains a start or not.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
unsigned int numSamples() const
The number of samples.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found and that the graph should be limited to the given heuristic value...
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected...
unsigned int numFreeStatesPruned() const
The number of states pruned.
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
double getRewireFactor() const
Get the rewiring scale factor.
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
virtual ~ImplicitGraph()=default
Destruct the graph using default destruction.
unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree)
Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples...
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
A queue of edges, sorted according to a sort key.
Definition SearchQueue.h:65
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition BITstar.h:146
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition BITstar.h:137
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition BITstar.h:125
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition BITstar.h:119
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition BITstar.h:116
void setup() override
Setup the algorithm.
Definition BITstar.cpp:175
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition BITstar.h:149
Main namespace. Contains everything in this library.