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)
512 void saveNeighborhood(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn, BiDirMotion *m);
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: BFMT.h:181
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
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition: BFMT.h:619
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
base::Cost hcost_[2]
The minimum cost to go of this motion (heuristically computed)
Definition: BFMT.h:336
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:591
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:563
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
A shared pointer wrapper for ompl::base::StateSampler.
void setPrecomputeNN(bool p)
Sets Nearest Neighbors precomputation. Currently, it precomputes once solve() has been called...
Definition: BFMT.h:261
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:63
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
double distanceFunction(const BiDirMotion *a, const BiDirMotion *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: BFMT.h:490
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
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:760
BiDirMotionPtrs children_[2]
The set of motions descending from the current motion.
Definition: BFMT.h:324
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
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
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
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
Bidirectional Asymptotically Optimal Fast Marching Tree algorithm developed by J. Starek...
Definition: BFMT.h:82
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"...
Definition: BFMT.h:283
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified, the default assumes an obstacle-free unit hypercube, freeSpaceVolume = (maximumExtent/sqrt(dimension))^(dimension)
Definition: BFMT.h:172
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:594
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
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
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:607
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:570
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:625
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
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:70
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 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
A shared pointer wrapper for ompl::base::OptimizationObjective.
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
BiDirMotion(const base::SpaceInformationPtr &si, TreeType *tree)
Constructor that allocates memory for the state.
Definition: BFMT.h:303
std::set< BiDirMotion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: BFMT.h:339
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence...
Definition: BFMT.h:567
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
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
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
Comparator used to order motions in a binary heap.
Definition: BFMT.h:455