BFMT.cpp
20 , freeSpaceVolume_(si_->getStateSpace()->getMeasure()) // An upper bound on the free space volume is the
31 ompl::base::Planner::declareParam<bool>("nearest_k", this, &BFMT::setNearestK, &BFMT::getNearestK, "0,1");
32 ompl::base::Planner::declareParam<bool>("balanced", this, &BFMT::setExploration, &BFMT::getExploration,
34 ompl::base::Planner::declareParam<bool>("optimality", this, &BFMT::setTermination, &BFMT::getTermination,
36 ompl::base::Planner::declareParam<bool>("heuristics", this, &BFMT::setHeuristics, &BFMT::getHeuristics,
38 ompl::base::Planner::declareParam<bool>("cache_cc", this, &BFMT::setCacheCC, &BFMT::getCacheCC, "0,1");
39 ompl::base::Planner::declareParam<bool>("extended_fmt", this, &BFMT::setExtendedFMT, &BFMT::getExtendedFMT,
79 OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
87 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
190 void BFMT::saveNeighborhood(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn, BiDirMotion *m)
249 return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
257 return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
double distanceFunction(const Motion *a, const Motion *b) const
Compute the distance between two motions as the cost between their contained states. Note that for computationally intensive cost functions, the cost between motions should be stored to avoid duplicate calculations.
Definition: FMT.h:366
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:652
void sampleFree(const ompl::base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition: FMT.cpp:212
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
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...
Definition: Console.cpp:120
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found...
Definition: BFMT.h:244
void sampleFree(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, const base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition: BFMT.cpp:215
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
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.
Definition: Planner.cpp:264
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
Definition: FMT.cpp:203
MotionBinHeap Open_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: FMT.h:430
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: BFMT.cpp:271
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition: FMT.h:459
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:63
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:474
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: BFMT.h:188
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:135
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: FMT.h:440
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:760
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
bool expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
Definition: FMT.cpp:498
void updateNeighborhood(BiDirMotion *m, std::vector< BiDirMotion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: BFMT.cpp:843
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:213
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:827
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision...
Definition: BFMT.h:427
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:47
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:119
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:164
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:129
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: FMT.cpp:169
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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
void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
Complete one iteration of the main loop of the BFMT* algorithm: Find K nearest nodes in set Unvisited...
Definition: BFMT.cpp:447
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for BFMT...
Definition: BFMT.h:155
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
Definition: BFMT.cpp:252
void updateNeighborhood(Motion *m, std::vector< Motion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: FMT.cpp:637
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
Abstract representation of a container that can perform nearest neighbors queries.
Definition: NearestNeighbors.h:48
bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Executes the actual planning algorithm, swapping and expanding the trees.
Definition: BFMT.cpp:568
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
void saveNeighborhood(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:190
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: BFMT.cpp:121
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:784
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
std::map< Motion *, std::vector< Motion * > > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition: FMT.h:434
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:234
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:471
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:207
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:226
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:200
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
void setNumSamples(const unsigned int numSamples)
Set the number of states that the planner should sample. The planner will sample this number of state...
Definition: BFMT.h:123