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 */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_IMPLICITGRAPH_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_IMPLICITGRAPH_
39 
40 // OMPL:
41 // The cost class:
42 #include "ompl/base/Cost.h"
43 // The optimization objective class:
44 #include "ompl/base/OptimizationObjective.h"
45 // The nearest neighbours structure
46 #include "ompl/datastructures/NearestNeighbors.h"
47 
48 // BIT*:
49 // I am member class of the BITstar class (i.e., I am in it's namespace), so I need to include it's definition to be
50 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
51 // include any I use in my .cpp (to avoid dependency loops).
52 #include "ompl/geometric/planners/bitstar/BITstar.h"
53 
54 namespace ompl
55 {
56  namespace geometric
57  {
65  {
66  public:
68  // Public functions:
69  // Construction and initialization
71  ImplicitGraph(NameFunc nameFunc);
72 
73  virtual ~ImplicitGraph() = default;
74 
78  CostHelper *costHelper, SearchQueue *searchQueue,
79  const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &pis);
80 
82  void clear();
84 
86  // Graph access:
88  bool hasAStart() const;
89 
91  bool hasAGoal() const;
92 
94  VertexPtrVector::const_iterator startVerticesBeginConst() const;
95 
97  VertexPtrVector::const_iterator startVerticesEndConst() const;
98 
100  VertexPtrVector::const_iterator goalVerticesBeginConst() const;
101 
103  VertexPtrVector::const_iterator goalVerticesEndConst() const;
104 
106  ompl::base::Cost minCost() const;
107 
109  bool hasInformedMeasure() const;
110 
112  double getInformedMeasure(const ompl::base::Cost &cost) const;
113 
116  double distanceFunction(const VertexConstPtr &a, const VertexConstPtr &b) const;
117 
119  void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples);
120 
123  void nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices);
124 
127 
130 
132  double smallestDistanceToGoal() const;
133 
135  unsigned int getConnectivityK() const;
136 
138  double getConnectivityR() const;
140 
142  // Graph modification:
145  void hasSolution(const ompl::base::Cost &solnCost);
146 
151 
154  void addNewSamples(const unsigned int &numSamples);
155 
159  std::pair<unsigned int, unsigned int> prune(double prunedMeasure);
161 
163  // Adding/remove individual states:
165  void addSample(const VertexPtr &newSample);
166 
168  void removeSample(const VertexPtr &oldSample);
169 
171  void addVertex(const VertexPtr &newVertex, bool removeFromFree);
172 
175  unsigned int removeVertex(const VertexPtr &oldSample, bool moveToFree);
177 
179  // General helper functions
180  void assertValidSample(const VertexConstPtr &sample, bool mustBeNew);
182 
184  // Graph settings:
186  void setRewireFactor(double rewireFactor);
187 
189  double getRewireFactor() const;
190 
192  void setUseKNearest(bool useKNearest);
193 
195  bool getUseKNearest() const;
196 
198  void setJustInTimeSampling(bool useJit);
199 
201  bool getJustInTimeSampling() const;
202 
204  void setDropSamplesOnPrune(bool dropSamples);
205 
207  bool getDropSamplesOnPrune() const;
208 
210  void setTrackApproximateSolutions(bool findApproximate);
211 
213  bool getTrackApproximateSolutions() const;
214 
216  template <template <typename T> class NN>
217  void setNearestNeighbors();
219 
221  // Get the progress property counters
223  unsigned int numFreeSamples() const;
224 
226  unsigned int numConnectedVertices() const;
227 
229  unsigned int numStatesGenerated() const;
230 
232  unsigned int numVerticesConnected() const;
233 
235  unsigned int numFreeStatesPruned() const;
236 
238  unsigned int numVerticesDisconnected() const;
239 
241  unsigned int numNearestLookups() const;
242 
244  unsigned int numStateCollisionChecks() const;
247 
248  private:
250  // High-level primitives updating the graph:
253  void updateSamples(const VertexConstPtr &vertex);
254 
257  void findVertexClosestToGoal();
259 
261  // High-level primitives pruning the graph:
265  std::pair<unsigned int, unsigned int> pruneStartsGoals();
266 
269  unsigned int pruneSamples();
271 
273  // Low-level random geometric graph helper and calculations
276  void testClosestToGoal(const VertexConstPtr &newVertex);
277 
280  ompl::base::Cost neighbourhoodCost(const VertexConstPtr &vertex) const;
281 
283  virtual void updateNearestTerms();
284 
286  double calculateR(unsigned int N) const;
287 
289  unsigned int calculateK(unsigned int N) const;
290 
294  double minimumRggR() const;
295 
299  double minimumRggK() const;
301 
303  // Debug
305  void assertSetup() const;
307 
309  // Variables -- Make sure every one is configured in setup() and reset in clear():
311  NameFunc nameFunc_;
312 
314  bool isSetup_{false};
315 
317  ompl::base::SpaceInformationPtr si_{nullptr};
318 
320  ompl::base::ProblemDefinitionPtr pdef_{nullptr};
321 
324  CostHelper *costHelpPtr_{nullptr};
325 
328  SearchQueue *queuePtr_{nullptr};
329 
331  ompl::RNG rng_;
332 
334  ompl::base::InformedSamplerPtr sampler_{nullptr};
335 
338  VertexPtrVector startVertices_;
339 
342  VertexPtrVector goalVertices_;
343 
345  VertexPtrVector prunedStartVertices_;
346 
348  VertexPtrVector prunedGoalVertices_;
349 
351  VertexPtrVector newSamples_;
352 
354  VertexPtrVector recycledSamples_;
355 
358  VertexPtrNNPtr freeStateNN_{nullptr};
359 
362  VertexPtrNNPtr vertexNN_{nullptr};
363 
365  unsigned int samplesInThisBatch_{0u};
366 
370  unsigned int numUniformStates_{0u};
371 
373  double r_{0.};
374 
377  double k_rgg_{0.};
378 
380  unsigned int k_{0u};
381 
384  double approximationMeasure_{0.};
385 
387  ompl::base::Cost minCost_{std::numeric_limits<double>::infinity()};
388 
390  ompl::base::Cost maxCost_{std::numeric_limits<double>::infinity()};
391 
393  ompl::base::Cost costSampled_{std::numeric_limits<double>::infinity()};
394 
396  bool hasExactSolution_{false};
397 
399  VertexConstPtr closestVertexToGoal_{nullptr};
400 
403  double closestDistToGoal_{std::numeric_limits<double>::infinity()};
405 
407  // Informational variables - Make sure initialized in setup and reset in clear
409  unsigned int numSamples_{0u};
410 
412  unsigned int numVertices_{0u};
413 
415  unsigned int numFreeStatesPruned_{0u};
416 
418  unsigned int numVerticesDisconnected_{0u};
419 
421  unsigned int numNearestNeighbours_{0u};
422 
424  unsigned int numStateCollisionChecks_{0u};
426 
428  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
430  double rewireFactor_{1.1};
431 
433  bool useKNearest_{true};
434 
436  bool useJustInTimeSampling_{false};
437 
439  bool dropSamplesOnPrune_{false};
440 
442  bool findApprox_{false};
444  }; // class: ImplicitGraph
445  } // geometric
446 } // ompl
447 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_IMPLICITGRAPH_
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices)
Get the nearest samples from the vertexNN_ using the appropriate "near" definition (i...
A shared pointer wrapper for ompl::base::ProblemDefinition.
unsigned int numConnectedVertices() const
The number of vertices in the tree (Size of vertexNN_).
bool hasAGoal() const
Gets whether the graph contains a goal or not.
void setup(const ompl::base::SpaceInformationPtr &si, const ompl::base::ProblemDefinitionPtr &pdef, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &pis)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates, but checks it for starts/goals.
void addVertex(const VertexPtr &newVertex, bool removeFromFree)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
void addSample(const VertexPtr &newSample)
Add an unconnected sample.
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Pruning is performed by using the prune con...
unsigned int numVerticesConnected() const
The total number of vertices added to the graph (numVertices_).
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition: BITstar.h:144
unsigned int numFreeSamples() const
The number of free samples (size of freeStateNN_).
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
bool hasAStart() const
Gets whether the graph contains a start or not.
unsigned int numStatesGenerated() const
The total number of states generated (numSamples_).
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
void clear()
Clear the graph to the state of construction.
ImplicitGraph(NameFunc nameFunc)
Construct an implicit graph.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected (numVerticesDisconnected_).
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s r_rrg*.
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
double getRewireFactor() const
Get the rewiring scale factor.
void updateStartAndGoalStates(ompl::base::PlannerInputStates &pis, const base::PlannerTerminationCondition &ptc)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
bool getJustInTimeSampling() const
Get whether we&#39;re using just-in-time sampling.
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
unsigned int removeVertex(const VertexPtr &oldSample, bool moveToFree)
Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples...
Base class for a planner.
Definition: Planner.h:223
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e., k or r).
A shared pointer wrapper for ompl::base::SpaceInformation.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
void removeSample(const VertexPtr &oldSample)
Remove an unconnected sample.
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found and that the graph should be limited to the given heuristic value...
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...
unsigned int numFreeStatesPruned() const
The number of states pruned (numFreeStatesPruned_).
A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of t...
Definition: SearchQueue.h:92
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:147
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:64
unsigned int numNearestLookups() const
The number of nearest neighbour calls (numNearestNeighbours_).
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
double distanceFunction(const VertexConstPtr &a, const VertexConstPtr &b) const
The distance function. Calculates the distance directionally from the given state to all the other st...
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
unsigned int numStateCollisionChecks() const
The number of state collision checks (numStateCollisionChecks_).