ImplicitGraph.h
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
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,...
Definition: PlannerData.h:175
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...
Definition: PlannerTerminationCondition.h:64
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.
Definition: ImplicitGraph.h:57
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
Definition: ImplicitGraph.cpp:1725
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...
Definition: ImplicitGraph.cpp:617
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...
Definition: ImplicitGraph.cpp:383
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
Definition: ImplicitGraph.cpp:277
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
Definition: ImplicitGraph.cpp:303
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
Definition: ImplicitGraph.cpp:703
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
Definition: ImplicitGraph.cpp:1487
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
Definition: ImplicitGraph.cpp:695
unsigned int numStatesGenerated() const
The total number of states generated.
Definition: ImplicitGraph.cpp:1720
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
Definition: ImplicitGraph.cpp:1635
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
Definition: ImplicitGraph.cpp:1514
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
Definition: ImplicitGraph.cpp:1482
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
Definition: ImplicitGraph.cpp:1492
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
Definition: ImplicitGraph.cpp:1457
bool hasAGoal() const
Gets whether the graph contains a goal or not.
Definition: ImplicitGraph.cpp:1452
unsigned int numVertices() const
The number of vertices in the search tree.
Definition: ImplicitGraph.cpp:1712
unsigned int numStateCollisionChecks() const
The number of state collision checks.
Definition: ImplicitGraph.cpp:1745
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition: ImplicitGraph.cpp:1614
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
Definition: ImplicitGraph.cpp:824
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
Definition: ImplicitGraph.cpp:1503
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: ImplicitGraph.cpp:1690
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: ImplicitGraph.cpp:1584
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
Definition: ImplicitGraph.cpp:323
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
Definition: ImplicitGraph.cpp:1536
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
Definition: ImplicitGraph.cpp:1477
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
Definition: ImplicitGraph.cpp:1740
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: ImplicitGraph.cpp:1543
bool hasAStart() const
Gets whether the graph contains a start or not.
Definition: ImplicitGraph.cpp:1447
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
Definition: ImplicitGraph.cpp:1735
void setJustInTimeSampling(bool useJit)
Definition: ImplicitGraph.cpp:1589
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
Definition: ImplicitGraph.cpp:1645
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: ImplicitGraph.cpp:1640
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
Definition: ImplicitGraph.cpp:1467
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
Definition: ImplicitGraph.cpp:1674
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
Definition: ImplicitGraph.cpp:744
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
Definition: ImplicitGraph.cpp:1462
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
Definition: ImplicitGraph.cpp:669
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...
Definition: ImplicitGraph.cpp:368
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
Definition: ImplicitGraph.cpp:1684
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...
Definition: ImplicitGraph.cpp:646
unsigned int numFreeStatesPruned() const
The number of states pruned.
Definition: ImplicitGraph.cpp:1730
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Definition: ImplicitGraph.cpp:1679
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: ImplicitGraph.cpp:1561
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
Definition: ImplicitGraph.cpp:1249
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,...
Definition: ImplicitGraph.cpp:91
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
Definition: ImplicitGraph.cpp:1261
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
Definition: ImplicitGraph.cpp:900
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
Definition: ImplicitGraph.cpp:1472
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
Definition: ImplicitGraph.cpp:1619
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...
Definition: ImplicitGraph.cpp:773
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
Definition: ImplicitGraph.cpp:751
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::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:149