BFMT.h
37 /* Algorithm design: Joseph Starek (Stanford), Ed Schmerling (Stanford), Lucas Janson (Stanford) and Marco Pavone
303 BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree) : state_(si->allocState()), tree_(tree)
Abstract representation of a container that can perform nearest neighbors queries.
Definition: NearestNeighbors.h:48
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:48
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:71
virtual Cost combineCosts(Cost c1, Cost c2) const
Get the cost that corresponds to combining the costs c1 and c2. Default implementation defines this c...
Definition: OptimizationObjective.cpp:91
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
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
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition: BFMT.h:284
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion
Definition: BFMT.h:324
BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree)
Constructor that allocates memory for the state.
Definition: BFMT.h:303
base::Cost hcost_[2]
The minimum cost to go of this motion (heuristically computed)
Definition: BFMT.h:336
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: BFMT.h:339
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek,...
Definition: BFMT.h:83
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:563
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:607
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:655
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:571
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:226
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition: BFMT.h:619
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
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:830
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition: BFMT.h:181
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:763
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:570
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
BiDirMotionBinHeap Open_[2]
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: BFMT.h:604
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found....
Definition: BFMT.h:244
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
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:594
double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition: BFMT.h:490
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
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:207
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
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 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
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
std::map< BiDirMotion *, BiDirMotionPtrs > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition: BFMT.h:598
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:787
void saveNeighborhood(BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:190
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:846
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
void setPrecomputeNN(bool p)
Sets Nearest Neighbors precomputation. Currently, it precomputes once solve() has been called.
Definition: BFMT.h:261
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition: BFMT.h:567
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:213
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:129
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:591
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition: BFMT.h:172
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:200
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:164
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
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:135
bool setPrecomputeNN() const
Returns true if Nearest Neighbor precomputation is done.
Definition: BFMT.h:267
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:53
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Comparator used to order motions in a binary heap.
Definition: BFMT.h:456