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  const CostHelperPtr &costHelper, const SearchQueuePtr &searchQueue,
79  const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &pis);
80 
82  void clear();
84 
86  // Graph access:
88  bool hasAGoal() const;
89 
91  VertexPtrVector::const_iterator startVerticesBeginConst() const;
92 
94  VertexPtrVector::const_iterator startVerticesEndConst() const;
95 
97  VertexPtrVector::const_iterator goalVerticesBeginConst() const;
98 
100  VertexPtrVector::const_iterator goalVerticesEndConst() const;
101 
103  ompl::base::Cost minCost() const;
104 
106  bool hasInformedMeasure() const;
107 
109  double getInformedMeasure(const ompl::base::Cost &cost) const;
110 
113  double distanceFunction(const VertexConstPtr &a, const VertexConstPtr &b) const;
114 
116  void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples);
117 
120  void nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices);
121 
124 
127 
129  double smallestDistanceToGoal() const;
130 
132  unsigned int getConnectivityK() const;
133 
135  double getConnectivityR() const;
137 
139  // Graph modification:
142  void hasSolution(const ompl::base::Cost &solnCost);
143 
148 
151  void addNewSamples(const unsigned int &numSamples);
152 
156  std::pair<unsigned int, unsigned int> prune(double prunedMeasure);
158 
160  // Adding/remove individual states:
162  void addSample(const VertexPtr &newSample);
163 
165  void removeSample(const VertexPtr &oldSample);
166 
168  void addVertex(const VertexPtr &newVertex, bool removeFromFree);
169 
172  unsigned int removeVertex(const VertexPtr &oldVertex, bool moveToFree);
174 
176  // Graph settings:
178  void setRewireFactor(double rewireFactor);
179 
181  double getRewireFactor() const;
182 
184  void setUseKNearest(bool useKNearest);
185 
187  bool getUseKNearest() const;
188 
190  void setJustInTimeSampling(bool useJit);
191 
193  bool getJustInTimeSampling() const;
194 
196  void setDropSamplesOnPrune(bool dropSamples);
197 
199  bool getDropSamplesOnPrune() const;
200 
202  void setTrackApproximateSolutions(bool findApproximate);
203 
205  bool getTrackApproximateSolutions() const;
206 
208  template <template <typename T> class NN>
209  void setNearestNeighbors();
211 
213  // Get the progress property counters
215  unsigned int numFreeSamples() const;
216 
218  unsigned int numConnectedVertices() const;
219 
221  unsigned int numStatesGenerated() const;
222 
224  unsigned int numVerticesConnected() const;
225 
227  unsigned int numFreeStatesPruned() const;
228 
230  unsigned int numVerticesDisconnected() const;
231 
233  unsigned int numNearestLookups() const;
234 
236  unsigned int numStateCollisionChecks() const;
239 
240  private:
242  // High-level primitives updating the graph:
245  void updateSamples(const VertexConstPtr &vertex);
246 
249  void findVertexClosestToGoal();
251 
253  // High-level primitives pruning the graph:
257  std::pair<unsigned int, unsigned int> pruneStartsGoals();
258 
261  unsigned int pruneSamples();
263 
265  // Low-level random geometric graph helper and calculations
268  void testClosestToGoal(const VertexConstPtr &newVertex);
269 
272  ompl::base::Cost neighbourhoodCost(const VertexConstPtr &vertex) const;
273 
275  virtual void updateNearestTerms();
276 
278  double calculateR(unsigned int N) const;
279 
281  unsigned int calculateK(unsigned int N) const;
282 
286  double minimumRggR() const;
287 
291  double minimumRggK() const;
293 
295  // Debug
297  void confirmSetup() const;
299 
301  // Variables -- Make sure every one is configured in setup() and reset in clear():
303  NameFunc nameFunc_;
304 
306  bool isSetup_;
307 
310 
313 
316  CostHelperPtr costHelpPtr_;
317 
320  SearchQueuePtr queuePtr_;
321 
323  ompl::RNG rng_;
324 
326  ompl::base::InformedSamplerPtr sampler_;
327 
330  VertexPtrVector startVertices_;
331 
334  VertexPtrVector goalVertices_;
335 
337  VertexPtrVector prunedStartVertices_;
338 
340  VertexPtrVector prunedGoalVertices_;
341 
343  VertexPtrVector newSamples_;
344 
346  VertexPtrVector recycledSamples_;
347 
350  VertexPtrNNPtr freeStateNN_;
351 
354  VertexPtrNNPtr vertexNN_;
355 
357  unsigned int samplesInThisBatch_;
358 
362  unsigned int numUniformStates_;
363 
365  double r_;
366 
369  double k_rgg_;
370 
372  unsigned int k_;
373 
376  double approximationMeasure_;
377 
379  ompl::base::Cost minCost_;
380 
382  ompl::base::Cost maxCost_;
383 
385  ompl::base::Cost costSampled_;
386 
388  bool hasExactSolution_;
389 
391  VertexConstPtr closestVertexToGoal_;
392 
395  double closestDistToGoal_;
397 
399  // Informational variables - Make sure initialized in setup and reset in clear
401  unsigned int numSamples_;
402 
404  unsigned int numVertices_;
405 
407  unsigned int numFreeStatesPruned_;
408 
410  unsigned int numVerticesDisconnected_;
411 
413  unsigned int numNearestNeighbours_;
414 
416  unsigned int numStateCollisionChecks_;
418 
420  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
422  double rewireFactor_;
423 
425  bool useKNearest_;
426 
428  bool useJustInTimeSampling_;
429 
431  bool dropSamplesOnPrune_;
432 
434  bool findApprox_;
436  }; // class: ImplicitGraph
437  } // geometric
438 } // ompl
439 #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 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...
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.
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.
std::shared_ptr< SearchQueue > SearchQueuePtr
An search queue shared pointer.
Definition: BITstar.h:150
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:58
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.
void setup(const ompl::base::SpaceInformationPtr &si, const ompl::base::ProblemDefinitionPtr &pdef, const CostHelperPtr &costHelper, const SearchQueuePtr &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.
Base class for a planner.
Definition: Planner.h:232
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e., k or r).
unsigned int removeVertex(const VertexPtr &oldVertex, bool moveToFree)
Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples...
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_).
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:153
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.
std::shared_ptr< CostHelper > CostHelperPtr
A cost helper shared pointer.
Definition: BITstar.h:146
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_).