52 #include "ompl/util/Console.h"
80 namespace ompl
87 : nameFunc_(std::move(nameFunc))
92 const ompl::base::ProblemDefinitionPtr &pdef,
96 // Store that I am setup so that any debug-level tests will pass. This requires assuring that this function
101 si_ = si;
102 pdef_ = pdef;
122 NearestNeighbors<VertexPtr>::DistanceFunction distfun(
135 // Add any start and goals vertices that exist to the queue, but do NOT wait for any more goals:
136 this->updateStartAndGoalStates(pis, ompl::base::plannerAlwaysTerminatingCondition());
139 approximationMeasure_ = si_->getSpaceMeasure();
145 // A not horrible place to start would be hypercube proportional to the distance between the start and
146 // goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
152 throw ompl::Exception("For unbounded planning problems, just-in-time sampling must be enabled "
160 throw ompl::Exception("For unbounded planning problems, at least one start and one goal must exist "
168 // The scale on the maximum distance, i.e. the width of the hypercube is equal to this value times the
178 maxDist = std::max(maxDist, si_->distance(startVertex->stateConst(), goalVertex->stateConst()));
183 approximationMeasure_ = std::pow(distScale * maxDist, si_->getStateDimension());
200 // Reset everything to the state of construction OTHER than planner name and settings/parameters
213 rng_ = ompl::RNG();
247 minCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
248 maxCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
249 costSampled_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
271 double BITstar::ImplicitGraph::distanceFunction(const VertexConstPtr &a, const VertexConstPtr &b) const
278 throw ompl::Exception("a->state is unallocated");
282 throw ompl::Exception("b->state is unallocated");
286 // Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other
289 return si_->distance(b->stateConst(), a->stateConst());
292 void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
312 void BITstar::ImplicitGraph::nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices)
338 VertexPtrVector samples;
357 VertexPtrVector vertices;
401 const base::PlannerTerminationCondition &ptc)
409 // Whether we have to rebuid the queue, i.e.. whether we've called updateStartAndGoalStates before
413 Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
414 samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
415 whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
416 but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
434 goalVertices_.push_back(std::make_shared<Vertex>(si_, costHelpPtr_));
437 si_->copyState(goalVertices_.back()->state(), newGoal);
446 } while (pis.haveMoreGoalStates());
449 And then do the same for starts. We do this last as the starts are added to the queue, which uses a cost-to-go
452 There is no need to rebuild the queue when we add start vertices, as the queue is ordered on current
455 while (pis.haveMoreStartStates())
465 startVertices_.push_back(std::make_shared<Vertex>(si_, costHelpPtr_, true));
468 si_->copyState(startVertices_.back()->state(), newStart);
485 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
543 // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
549 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
618 pdef_, std::numeric_limits<unsigned int>::max());
626 OMPL_INFORM("%s: Added new starts and/or goals to the problem. Rebuilding the queue.",
640 // Iterate through the existing vertices and find the current best approximate solution (if enabled)
648 // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
651 OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
700 // We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT sampling.
710 throw ompl::Exception("A graph cannot be pruned until a solution is found");
753 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our own copy.
756 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
758 VertexPtr sampleToDelete(oldSample);
764 throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
789 throw ompl::Exception("Vertices must be connected to the graph before adding");
819 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our own copy.
822 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
824 VertexPtr vertexToDelete(oldVertex);
830 throw ompl::Exception("A code change has prevented ImplicitGraph::removeVertex() "
845 // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
854 // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
863 void BITstar::ImplicitGraph::assertValidSample(const VertexConstPtr &sample, bool mustBeNew)
868 throw ompl::Exception("Sample is a graph root.");
873 throw ompl::Exception("Sample already has a parent.");
878 throw ompl::Exception("Sample already has children.");
883 throw ompl::Exception("Sample is not marked new (as specified).");
888 throw ompl::Exception("Sample is already marked as expanded to samples.");
893 throw ompl::Exception("Sample is already marked as expanded to vertices.");
898 throw ompl::Exception("Sample already has an entry in the vertex queue.");
905 void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
909 ompl::base::Cost costReqd = this->neighbourhoodCost(vertex);
911 // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
927 // Calculate the sample density given the number of samples per batch and the measure of this batch
937 // And the fractional part represents the probability of one more sample. I like being pedantic.
956 auto newState = std::make_shared<Vertex>(si_, costHelpPtr_);
963 if (si_->isValid(newState->stateConst()))
989 VertexPtrVector vertices;
1017 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1027 // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1028 // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1029 // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1078 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1142 // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1174 VertexPtrVector samples;
1198 void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &newVertex)
1205 pdef_->getGoal()->isSatisfied(newVertex->stateConst(), &distFromGoal);
1222 // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1224 // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1228 ompl::base::Cost(2.0 * r_)));
1253 // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1254 // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1255 // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1277 auto dimDbl = static_cast<double>(si_->getStateDimension());
1282 return rewireFactor_ * this->minimumRggR() * std::pow(std::log(cardDbl) / cardDbl, 1 / dimDbl);
1288 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(N)));
1295 auto dimDbl = static_cast<double>(si_->getStateDimension());
1333 auto dimDbl = static_cast<double>(si_->getStateDimension());
1344 throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1361 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1371 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1401 throw ompl::Exception("Approximate solutions are not being tracked.");
1412 throw ompl::Exception("Approximate solutions are not being tracked.");
1423 throw ompl::Exception("Using an r-disc graph.");
1434 throw ompl::Exception("Using a k-nearest graph.");
1463 OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1491 OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1503 OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1521 OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1537 void BITstar::ImplicitGraph::setTrackApproximateSolutions(bool findApproximate)
1566 bool BITstar::ImplicitGraph::getTrackApproximateSolutions() const
1574 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1578 OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void nearestVertices(const VertexPtr &vertex, VertexPtrVector *neighbourVertices)
Get the nearest samples from the vertexNN_ using the appropriate "near" definition (i...
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...
void setJustInTimeSampling(bool useJit)
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.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
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.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
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_).
The user set problem definition.
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
Clear the graph to the state of construction.
bool haveMoreStartStates() const
Check if there are more potential start states.
Construct an implicit graph.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Simple termination condition that always returns true. The termination condition will always be met...
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.
double getRewireFactor() const
Get the rewiring scale factor.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
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.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
bool getJustInTimeSampling() const
Get whether we'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...
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e., k or r).
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...
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.
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...
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...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
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...
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
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.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
bool haveMoreGoalStates() const
Check if there are more potential goal states.
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.
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...
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.
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
The space information for which planning is done.
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.
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
unsigned int numStateCollisionChecks() const
The number of state collision checks (numStateCollisionChecks_).