BITstar.cpp
76 BITstar::BITstar(const ompl::base::SpaceInformationPtr &si, const std::string &name /*= "BITstar"*/)
83 // Allocate my helper classes, they hold settings and must never be deallocated. Give them a pointer to my
112 // Approximate solutions are supported but must be enabled with the appropriate configuration parameter.
120 Planner::declareParam<double>("rewire_factor", this, &BITstar::setRewireFactor, &BITstar::getRewireFactor,
124 Planner::declareParam<bool>("use_k_nearest", this, &BITstar::setUseKNearest, &BITstar::getUseKNearest, "0,"
126 Planner::declareParam<bool>("use_graphPtr_pruning", this, &BITstar::setPruning, &BITstar::getPruning, "0,"
136 Planner::declareParam<bool>("drop_unconnected_samples_on_prune", this, &BITstar::setDropSamplesOnPrune,
138 Planner::declareParam<bool>("stop_on_each_solution_improvement", this, &BITstar::setStopOnSolnImprovement,
140 Planner::declareParam<bool>("use_strict_queue_ordering", this, &BITstar::setStrictQueueOrdering,
142 Planner::declareParam<bool>("find_approximate_solutions", this, &BITstar::setConsiderApproximateSolutions,
244 OMPL_ERROR("%s::setup() BIT* currently only supports goals that can be cast to a sampleable goal "
255 // Setup the CostHelper, it provides everything I need from optimization objective plus some frills
261 // Setup the graph, it does not hold a copy of this or Planner::pis_, but uses them to create a NN struct
326 throw ompl::Exception("%s::solve() failed to set up the planner. Has a problem definition been set?", Planner::getName().c_str());
330 OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
335 // If we don't have a goal yet, recall updateStartAndGoalStates, but wait for the first goal (or until the
346 - There is a theoretically better solution (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(),
376 // From OMPL's point-of-view, BIT* can always have an approximate solution, so mark solution true if either
378 // Our returned solution will only be approximate if it is not exact and we are finding approximate
381 return ompl::base::PlannerStatus(hasExactSolution_ || graphPtr_->getTrackApproximateSolutions(),
382 !hasExactSolution_ && graphPtr_->getTrackApproximateSolutions());
449 }
487 // Yes, we must have just finished a batch. Increase the resolution of the graph and restart the queue.
525 bestCost_))
535 {
563 // The edge cannot improve our solution, and therefore neither can any other edge in the queue. Give
582 graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition());
640 OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
705 throw ompl::Exception("bestPathFromGoalToStart called without an exact or approximate solution.");
711 // Then, use the vertex pointer like an iterator. Starting at the goal, we iterate up the chain pushing the
713 // This will allows us to add the start (as the parent of the first child) and then stop when we get to the
722 throw ompl::Exception("The path to the goal does not originate at a start state. Something went "
764 // If not, we just add the vertex, first mark the target vertex as no longer new and unexpanded:
805 // Add the parent to the child. This updates the cost of the child as well as all it's descendents (if
824 for (auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
903 // to include the start or goal; however, that makes no sense for multiple start/goal problems, so
907 Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
915 OMPL_INFORM("%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u "
925 OMPL_INFORM("%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing "
937 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
948 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
957 void BITstar::statusMessage(const ompl::msg::LogLevel &msgLevel, const std::string &status) const
971 outputStream << "l: " << std::setw(6) << std::setfill(' ') << std::setprecision(5) << bestCost_.value();
977 outputStream << ", g: " << std::setw(5) << std::setfill(' ') << graphPtr_->numConnectedVertices();
985 outputStream << ", s: " << std::setw(5) << std::setfill(' ') << graphPtr_->numStatesGenerated();
987 outputStream << ", v: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVerticesConnected();
995 outputStream << ", c(s): " << std::setw(5) << std::setfill(' ') << graphPtr_->numStateCollisionChecks();
1057 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
1062 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
1087 OMPL_WARN("%s: Turning pruning off has never really been tested.", Planner::getName().c_str());
1103 {
1174 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1178 OMPL_WARN("%s: The nearest neighbour datastructures cannot be changed once the planner is setup. "
1181 }
1186 }
1191 }
1196 }
1201 }
1206 }
1211 }
1216 }
1221 }
1236 }
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:530
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:1191
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:495
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:471
A shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:525
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1176
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:1201
void getVertexQueue(VertexConstPtrVector *verticesInQueue)
Get the whole set of vertices to be expanded. Expensive but helpful for some videos.
Definition: BITstar.cpp:520
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1230
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1142
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:1132
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1216
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:1108
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:1147
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1236
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:1196
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1211
@ 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:382
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:1221
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1186
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:1137
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1113
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1181
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:515
BITstar(const base::SpaceInformationPtr &si, const std::string &name="BITstar")
Construct!
Definition: BITstar.cpp:140
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1103
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
Definition: BITstar.cpp:535
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1166
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: BITstar.cpp:1093
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1206