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 
45 namespace ompl
46 {
47  namespace geometric
48  {
56  class BITstar::ImplicitGraph
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,
74  ompl::base::PlannerInputStates &inputStates);
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 
102  ompl::base::Cost minCost() const;
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 
225  void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number);
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_
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Base class for a planner.
Definition: Planner.h:287
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
unsigned int numStatesGenerated() const
The total number of states generated.
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.
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:142
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
unsigned int numVertices() const
The number of vertices in the search tree.
bool hasAGoal() const
Gets whether the graph contains a goal or not.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:245
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:233
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &inputStates)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates,...
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
ImplicitGraph(NameFunc nameFunc)
Construct an implicit graph.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition: BITstar.h:242
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
virtual ~ImplicitGraph()=default
Destruct the graph using default destruction.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
A shared pointer wrapper for ompl::base::ProblemDefinition.
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...
unsigned int numStateCollisionChecks() const
The number of state collision checks.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
unsigned int numFreeStatesPruned() const
The number of states pruned.
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
void reset()
Reset the graph to the state of construction.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
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 setNearestNeighbors()
Set a different nearest neighbours datastructure.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
unsigned int numSamples() const
The number of samples.
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
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...
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...
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
double getRewireFactor() const
Get the rewiring scale factor.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...