ImplicitGraph.cpp
86 // Store that I am setup so that any debug-level tests will pass. This requires assuring that this function
125 // Add any start and goals vertices that exist to the queue, but do NOT wait for any more goals:
135 // A not horrible place to start would be hypercube proportional to the distance between the start and
136 // goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
150 throw ompl::Exception("For unbounded planning problems, at least one start and one goal must exist "
158 // The scale on the maximum distance, i.e. the width of the hypercube is equal to this value times the
168 maxDist = std::max(maxDist, si_->distance(startVertex->stateConst(), goalVertex->stateConst()));
190 // Reset everything to the state of construction OTHER than planner name and settings/parameters
261 double BITstar::ImplicitGraph::distanceFunction(const VertexConstPtr &a, const VertexConstPtr &b) const
276 // Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other
282 void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
302 void BITstar::ImplicitGraph::nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices)
325 {
383 maxCost_ = solnCost;
399 // Whether we have to rebuid the queue, i.e.. whether we've called updateStartAndGoalStates before
403 Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
404 samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
405 whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
406 but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
438 // And then do the for starts. We do this last as the starts are added to the queue, which uses a cost-to-go
441 // There is no need to rebuild the queue when we add start vertices, as the queue is ordered on current
468 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
526 // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
532 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
623 // Iterate through the existing vertices and find the current best approximate solution (if enabled)
631 // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
634 OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. Since PlannerInputStates "
678 // We don't add *new* samples until the next time "nearSamples" is called. This is to support JIT sampling.
771 }
779 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
787 vertexNN_->remove(vertexToDelete);
792 // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
801 // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
819 // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
835 // Calculate the sample density given the number of samples per batch and the measure of this batch
845 // And the fractional part represents the probability of one more sample. I like being pedantic.
925 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
936 // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
986 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1050 // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1130 // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1132 // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1161 // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1162 // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1163 // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1190 return rewireFactor_ * this->minimumRggR() * std::pow(std::log(cardDbl) / cardDbl, 1 / dimDbl);
1207 std::pow((1.0 + 1.0 / dimDbl) * (approximationMeasure_ / unitNBallMeasure(si_->getStateDimension())),
1211 unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl); //RRT* radius (smaller for unit-volume problem)
1212 return 2.0 * std::pow((1.0 / dimDbl) * (approximationMeasure_ / unitNBallMeasure(si_->getStateDimension())),
1213 1.0 / dimDbl); //FMT* radius (smallest for R2, equiv to RRT* for R3 and then middle for higher d. All
1247 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1257 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1321 }
1336 }
1349 OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1377 OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1379 nameFunc_().c_str());
1389 OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1403 {
1436 closestVertexToGoal_.reset();
1460 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1464 OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1466 nameFunc_().c_str());
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
Definition: ImplicitGraph.cpp:346
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
std::shared_ptr< SearchQueue > SearchQueuePtr
An search queue shared pointer.
Definition: BITstar.h:246
void addVertex(const VertexPtr &newVertex, bool removeFromFree)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
Definition: ImplicitGraph.cpp:805
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:335
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
Definition: ImplicitGraph.cpp:1466
unsigned int numStatesGenerated() const
The total number of states generated (numSamples_).
Definition: ImplicitGraph.cpp:1550
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: ImplicitGraph.cpp:1390
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...
Definition: ImplicitGraph.cpp:454
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
Definition: ImplicitGraph.cpp:1487
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.
Definition: ImplicitGraph.cpp:1321
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
Definition: ImplicitGraph.cpp:1326
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...
Definition: ImplicitGraph.cpp:838
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,...
Definition: ImplicitGraph.cpp:145
unsigned int numConnectedVertices() const
The number of vertices in the tree (Size of vertexNN_).
Definition: ImplicitGraph.cpp:1545
bool hasAGoal() const
Gets whether the graph contains a goal or not.
Definition: ImplicitGraph.cpp:1306
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: ImplicitGraph.cpp:1408
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
Definition: ImplicitGraph.cpp:383
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: ImplicitGraph.cpp:1431
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
std::function< double(const VertexPtr &, const VertexPtr &)> DistanceFunction
The definition of a distance function.
Definition: NearestNeighbors.h:116
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
Definition: ImplicitGraph.cpp:1316
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected (numVerticesDisconnected_).
Definition: ImplicitGraph.cpp:1565
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition: GeometricEquations.cpp:55
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
Definition: ImplicitGraph.cpp:1341
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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...
Definition: ImplicitGraph.cpp:439
void nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices)
Get the nearest samples from the vertexNN_ using the appropriate "near" definition (i....
Definition: ImplicitGraph.cpp:366
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...
Definition: ImplicitGraph.cpp:325
unsigned int numVerticesConnected() const
The total number of vertices added to the graph (numVertices_).
Definition: ImplicitGraph.cpp:1555
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:264
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
Definition: ImplicitGraph.cpp:1346
A shared pointer wrapper for ompl::base::ProblemDefinition.
unsigned int numStateCollisionChecks() const
The number of state collision checks (numStateCollisionChecks_).
Definition: ImplicitGraph.cpp:1575
unsigned int numFreeSamples() const
The number of free samples (size of freeStateNN_).
Definition: ImplicitGraph.cpp:1540
unsigned int numFreeStatesPruned() const
The number of states pruned (numFreeStatesPruned_).
Definition: ImplicitGraph.cpp:1560
unsigned int numNearestLookups() const
The number of nearest neighbour calls (numNearestNeighbours_).
Definition: ImplicitGraph.cpp:1570
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
Definition: ImplicitGraph.cpp:1336
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Definition: PlannerData.cpp:391
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition: ImplicitGraph.cpp:1461
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
Definition: ImplicitGraph.cpp:1357
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
Definition: ImplicitGraph.cpp:1516
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
Definition: ImplicitGraph.cpp:1331
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: ImplicitGraph.cpp:1482
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:413
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:342
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:222
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: ImplicitGraph.cpp:1522
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
Definition: ImplicitGraph.cpp:1368
void addSample(const VertexPtr &newSample)
Add an unconnected sample.
Definition: ImplicitGraph.cpp:771
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...
Definition: ImplicitGraph.cpp:745
void removeSample(const VertexPtr &oldSample)
Remove an unconnected sample.
Definition: ImplicitGraph.cpp:787
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
Definition: ImplicitGraph.cpp:1311
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:432
void setJustInTimeSampling(bool useJit)
Definition: ImplicitGraph.cpp:1436
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
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:706