Loading...
Searching...
No Matches
ImplicitGraph.cpp
97 // Store that I am setup so that any debug-level tests will pass. This requires assuring that this function
126 // Add any start and goals vertices that exist to the queue, but do NOT wait for any more goals:
136 // A not horrible place to start would be hypercube proportional to the distance between the start and
137 // goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
151 throw ompl::Exception("For unbounded planning problems, at least one start and one goal must exist "
303 void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
384 ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
393 Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
394 samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
395 whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
396 but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
426 And then do the same for starts. We do this last as the starts are added to the queue, which uses a
427 cost-to-go heuristic in it's ordering, and for that we want all the goals updated. As there is no way to
428 wait for new *start* states, this loop can be cleaner There is no need to rebuild the queue when we add
429 start vertices, as the queue is ordered on current cost-to-come, and adding a start doesn't change that.
463 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
517 // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
523 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
599 // Iterate through the existing vertices and find the current best approximate solution (if enabled)
607 // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
610 OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
709 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
773 unsigned int BITstar::ImplicitGraph::removeFromVertices(const VertexPtr &vertex, bool moveToFree)
779 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
783 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
806 // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
824 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
830 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
900 void BITstar::ImplicitGraph::removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
905 throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
929 // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
938 // Calculate the sample density given the number of samples per batch and the measure of this batch
940 double sampleDensity = static_cast<double>(numNewSamplesInCurrentBatch_) / approximationMeasure_;
949 // And the fractional part represents the probability of one more sample. I like being pedantic.
1038 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1048 // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1049 // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1050 // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1107 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1185 // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1255 // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
1295 ompl::base::Cost BITstar::ImplicitGraph::calculateNeighbourhoodCost(const VertexConstPtr &vertex) const
1306 // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1308 // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1341 // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1342 // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1343 // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1457 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1467 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1566 OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1594 OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1606 OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1679 void BITstar::ImplicitGraph::setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
1692 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1696 OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Definition NearestNeighbors.h:51
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition PlannerData.h:175
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
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
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
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition Planner.h:78
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition Planner.cpp:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:237
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:355
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:274
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
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
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
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
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex's cost or those of its descendants....
Definition Vertex.cpp:370
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition Vertex.cpp:299
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition Vertex.cpp:176
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex's cost, and can update the descenden...
Definition Vertex.cpp:267
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition Vertex.cpp:783
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition Vertex.cpp:198
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
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition PlannerTerminationCondition.cpp:189
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Definition GeometricEquations.cpp:55
STL namespace.