BITstar.cpp
65 BITstar::BITstar(const ompl::base::SpaceInformationPtr &si, const std::string &name /*= "BITstar"*/)
72 // Allocate my helper classes, they hold settings and must never be deallocated. Give them a pointer to my
81 // Approximate solutions are supported but must be enabled with the appropriate configuration parameter.
89 Planner::declareParam<double>("rewire_factor", this, &BITstar::setRewireFactor, &BITstar::getRewireFactor,
93 Planner::declareParam<bool>("use_k_nearest", this, &BITstar::setUseKNearest, &BITstar::getUseKNearest,
96 Planner::declareParam<bool>("use_graph_pruning", this, &BITstar::setPruning, &BITstar::getPruning,
107 Planner::declareParam<bool>("drop_unconnected_samples_on_prune", this, &BITstar::setDropSamplesOnPrune,
109 Planner::declareParam<bool>("stop_on_each_solution_improvement", this, &BITstar::setStopOnSolnImprovement,
111 Planner::declareParam<bool>("use_strict_queue_ordering", this, &BITstar::setStrictQueueOrdering,
113 Planner::declareParam<bool>("find_approximate_solutions", this, &BITstar::setConsiderApproximateSolutions,
120 addPlannerProgressProperty("current free states INTEGER", [this] { return currentFreeProgressProperty(); });
213 // Setup the CostHelper, it provides everything I need from optimization objective plus some frills
219 // Setup the graph, it does not hold a copy of this or Planner::pis_, but uses them to create a NN
235 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
236 OMPL_WARN("BIT*: An r-disc version of BIT* can not be named 'kBITstar', as this name is reserved "
242 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
243 OMPL_WARN("BIT*: A k-nearest version of BIT* can not be named 'BITstar', as this name is reserved "
249 // It's current the default k-nearest ABIT* name, and we're toggling, so set to the default r-disc
256 // It's current the default r-disc ABIT* name, and we're toggling, so set to the default k-nearest
315 throw ompl::Exception("%s::solve() failed to set up the planner. Has a problem definition been set?",
320 OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
325 // If we don't have a goal yet, recall updateStartAndGoalStates, but wait for the first goal (or until the
335 // We don't have a start, since there's no way to wait for one to appear, so we will not be solving this
345 // We don't have a goal (and we waited as long as ptc allowed us for one to appear), so we will not be
362 - There is a theoretically better solution (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(),
392 // From OMPL's point-of-view, BIT* can always have an approximate solution, so mark solution true if either
394 // Our returned solution will only be approximate if it is not exact and we are finding approximate
634 graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition());
654 /* Profiling reveals that pruning is very expensive, mainly because the nearest neighbour structure of
655 * the samples has to be updated. On the other hand, nearest neighbour lookup gets more expensive the
656 * bigger the structure, so it's a tradeoff. Pruning on every cost update seems insensible, but so does
657 * never pruning at all. The criteria to prune should depend on how many vertices/samples there are and
658 * how many of them could be pruned, as the decrease in cost associated with nearest neighbour lookup
659 * for fewer samples must justify the cost of pruning. It turns out that counting is affordable, so we
673 // Only prune if the decrease in number of samples and the associated decrease in nearest neighbour
674 // lookup cost justifies the cost of pruning. There has to be a way to make this more formal, and less
695 OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
772 throw ompl::Exception("bestPathFromGoalToStart called without an exact or approximate solution.");
778 // Then, use the vertex pointer like an iterator. Starting at the goal, we iterate up the chain pushing the
780 // This will allows us to add the start (as the parent of the first child) and then stop when we get to the
782 for (/*Already allocated & initialized*/; !curVertex->isRoot(); curVertex = curVertex->getParent())
788 throw ompl::Exception("The path to the goal does not originate at a start state. Something went "
902 for (auto it = graphPtr_->goalVerticesBeginConst(); it != graphPtr_->goalVerticesEndConst(); ++it)
980 // to include the start or goal; however, that makes no sense for multiple start/goal problems, so
984 Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
992 OMPL_INFORM("%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u "
1002 OMPL_INFORM("%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing "
1014 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
1025 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
1034 void BITstar::statusMessage(const ompl::msg::LogLevel &logLevel, const std::string &status) const
1048 outputStream << "l: " << std::setw(6) << std::setfill(' ') << std::setprecision(5) << bestCost_.value();
1062 outputStream << ", s: " << std::setw(5) << std::setfill(' ') << graphPtr_->numStatesGenerated();
1064 outputStream << ", v: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVerticesConnected();
1070 outputStream << ", n: " << std::setw(5) << std::setfill(' ') << graphPtr_->numNearestLookups();
1072 outputStream << ", c(s): " << std::setw(5) << std::setfill(' ') << graphPtr_->numStateCollisionChecks();
1180 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
1181 OMPL_WARN("BIT*: An r-disc version of BIT* can not be named 'kBITstar', as this name is reserved for "
1187 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
1188 OMPL_WARN("BIT*: A k-nearest version of BIT* can not be named 'BITstar', as this name is reserved for "
1194 // It's current the default k-nearest ABIT* name, and we're toggling, so set to the default r-disc
1195 OMPL_WARN("ABIT*: An r-disc version of ABIT* can not be named 'kABITstar', as this name is reserved "
1201 // It's current the default r-disc ABIT* name, and we're toggling, so set to the default k-nearest
1202 OMPL_WARN("ABIT*: A k-nearest version of ABIT* can not be named 'ABITstar', as this name is reserved "
1323 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1327 OMPL_WARN("%s: The nearest neighbour datastructures cannot be changed once the planner is setup. "
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1315
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1310
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition: PlannerTerminationCondition.cpp:189
ompl::base::OptimizationObjectivePtr getOptimizationObjective(const base::SpaceInformationPtr &si, const std::string &objective, double threshold)
Create an optimization objective. The objective name can be: "length", "max min clearance",...
Definition: appUtil.cpp:256
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:477
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:1266
BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name="BITstar")
Construct with a pointer to the space information and an optional name.
Definition: BITstar.cpp:65
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:447
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:423
A shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:472
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:119
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1248
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:410
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:137
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1276
void setInflationScalingParameter(double parameter)
The parameter that scales the inflation factor on the second search of each RGG approximation....
Definition: BITstar.cpp:1112
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1305
double getInitialInflationFactor() const
Get the inflation factor for the initial search.
Definition: BITstar.cpp:1127
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1220
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:70
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:134
double getCurrentTruncationFactor() const
Get the truncation factor for the current search.
Definition: BITstar.cpp:1147
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1209
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1291
base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override
Solve the problem given a termination condition.
Definition: BITstar.cpp:307
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1167
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1227
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1321
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
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:1271
void enableCascadingRewirings(bool enable)
Enable the cascading of rewirings.
Definition: BITstar.cpp:1122
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1286
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
void setTruncationScalingParameter(double parameter)
Sets the parameter that scales the truncation factor for the searches of each RGG approximation....
Definition: BITstar.cpp:1117
double getCurrentInflationFactor() const
Get the inflation factor for the current search.
Definition: BITstar.cpp:1142
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:143
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:1296
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1259
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:1214
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1172
double getInflationScalingParameter() const
Get the inflation scaling parameter.
Definition: BITstar.cpp:1132
double getTruncationScalingParameter() const
Get the truncation factor parameter.
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:1253
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:467
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1162
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data.
Definition: BITstar.cpp:482
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1238
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:1152
void setInitialInflationFactor(double factor)
Set the inflation for the initial search of RGG approximation. See ABIT*'s class description for more...
Definition: BITstar.cpp:1106
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1281