BITstar.cpp
84 BITstar::BITstar(const ompl::base::SpaceInformationPtr &si, const std::string &name /*= "BITstar"*/)
91 // Allocate my helper classes, they hold settings and must never be deallocated. Give them a pointer to my
120 // Approximate solutions are supported but must be enabled with the appropriate configuration parameter.
128 Planner::declareParam<double>("rewire_factor", this, &BITstar::setRewireFactor, &BITstar::getRewireFactor,
132 Planner::declareParam<bool>("use_k_nearest", this, &BITstar::setUseKNearest, &BITstar::getUseKNearest, "0,"
134 Planner::declareParam<bool>("use_graphPtr_pruning", this, &BITstar::setPruning, &BITstar::getPruning, "0,"
144 Planner::declareParam<bool>("drop_unconnected_samples_on_prune", this, &BITstar::setDropSamplesOnPrune,
146 Planner::declareParam<bool>("stop_on_each_solution_improvement", this, &BITstar::setStopOnSolnImprovement,
148 Planner::declareParam<bool>("use_strict_queue_ordering", this, &BITstar::setStrictQueueOrdering,
150 Planner::declareParam<bool>("find_approximate_solutions", this, &BITstar::setConsiderApproximateSolutions,
252 OMPL_ERROR("%s::setup() BIT* currently only supports goals that can be cast to a sampleable goal "
263 // Setup the CostHelper, it provides everything I need from optimization objective plus some frills
269 // Setup the graph, it does not hold a copy of this or Planner::pis_, but uses them to create a NN struct
271 graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_.get(), queuePtr_.get(), this, Planner::pis_);
334 throw ompl::Exception("%s::solve() failed to set up the planner. Has a problem definition been set?", Planner::getName().c_str());
338 OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
343 // If we don't have a goal yet, recall updateStartAndGoalStates, but wait for the first goal (or until the
353 // We don't have a start, since there's no way to wait for one to appear, so we will not be solving this "problem" today
354 OMPL_WARN("%s: A solution cannot be found as no valid start states are available.", Planner::getName().c_str());
361 // We don't have a goal (and we waited as long as ptc allowed us for one to appear), so we will not be solving this "problem" today
362 OMPL_WARN("%s: A solution cannot be found as no valid goal states are available.", Planner::getName().c_str());
370 - There is a theoretically better solution (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(),
390 }
400 // From OMPL's point-of-view, BIT* can always have an approximate solution, so mark solution true if either
402 // Our returned solution will only be approximate if it is not exact and we are finding approximate
405 return ompl::base::PlannerStatus(hasExactSolution_ || graphPtr_->getTrackApproximateSolutions(),
473 }
511 // Yes, we must have just finished a batch. Increase the resolution of the graph and restart the queue.
549 bestCost_))
559 {
587 // The edge cannot improve our solution, and therefore neither can any other edge in the queue. Give
606 graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition());
664 OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
729 throw ompl::Exception("bestPathFromGoalToStart called without an exact or approximate solution.");
735 // Then, use the vertex pointer like an iterator. Starting at the goal, we iterate up the chain pushing the
737 // This will allows us to add the start (as the parent of the first child) and then stop when we get to the
746 throw ompl::Exception("The path to the goal does not originate at a start state. Something went "
848 for (auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
927 // to include the start or goal; however, that makes no sense for multiple start/goal problems, so
931 Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
939 OMPL_INFORM("%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u "
949 OMPL_INFORM("%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing "
961 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
972 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
981 void BITstar::statusMessage(const ompl::msg::LogLevel &msgLevel, const std::string &status) const
995 outputStream << "l: " << std::setw(6) << std::setfill(' ') << std::setprecision(5) << bestCost_.value();
1001 outputStream << ", g: " << std::setw(5) << std::setfill(' ') << graphPtr_->numConnectedVertices();
1009 outputStream << ", s: " << std::setw(5) << std::setfill(' ') << graphPtr_->numStatesGenerated();
1011 outputStream << ", v: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVerticesConnected();
1017 outputStream << ", n: " << std::setw(5) << std::setfill(' ') << graphPtr_->numNearestLookups();
1019 outputStream << ", c(s): " << std::setw(5) << std::setfill(' ') << graphPtr_->numStateCollisionChecks();
1081 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
1086 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
1111 OMPL_WARN("%s: Turning pruning off has never really been tested.", Planner::getName().c_str());
1127 {
1198 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1202 OMPL_WARN("%s: The nearest neighbour datastructures cannot be changed once the planner is setup. "
1205 }
1210 }
1215 }
1220 }
1225 }
1230 }
1235 }
1240 }
1245 }
1260 }
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:554
void setJustInTimeSampling(bool useJit)
Delay the generation of samples until they are necessary. This only works when using an r-disc connec...
Definition: BITstar.cpp:1215
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:234
ompl::base::Cost getNextEdgeValueInQueue()
Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if neces...
Definition: BITstar.cpp:519
std::pair< const ompl::base::State *, const ompl::base::State * > getNextEdgeInQueue()
Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and the...
Definition: BITstar.cpp:495
A shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:549
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1200
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:463
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1225
void getVertexQueue(VertexConstPtrVector *verticesInQueue)
Get the whole set of vertices to be expanded. Expensive but helpful for some videos.
Definition: BITstar.cpp:544
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1254
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1166
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:133
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:232
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:228
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1156
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1240
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1132
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:127
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:238
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1171
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1260
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
Definition: PlannerData.cpp:597
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition: BITstar.cpp:1220
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1235
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Solve.
Definition: BITstar.cpp:390
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded.
Definition: Console.cpp:142
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
Definition: BITstar.cpp:1245
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1210
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:222
void setStrictQueueOrdering(bool beStrict)
Enable "strict sorting" of the edge queue. Rewirings can change the position in the queue of an edge....
Definition: BITstar.cpp:1161
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1137
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1205
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:539
BITstar(const base::SpaceInformationPtr &si, const std::string &name="BITstar")
Construct!
Definition: BITstar.cpp:148
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1127
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
Definition: BITstar.cpp:559
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1190
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: BITstar.cpp:1117
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:52
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1230