FMT.h
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
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
Definition: OptimizationObjective.cpp:70
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
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:278
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: FMT.h:234
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:266
SetType
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"....
Definition: FMT.h:225
base::Cost hcost_
The minimum cost to go of this motion (heuristically computed)
Definition: FMT.h:331
std::set< Motion * > collChecksDone_
Contains the connections attempted FROM this node.
Definition: FMT.h:337
std::vector< Motion * > children_
The set of motions descending from the current motion.
Definition: FMT.h:340
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Definition: FMT.h:291
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone.
Definition: FMT.h:91
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner.
Definition: FMT.h:168
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: FMT.h:200
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
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
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: FMT.h:194
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:474
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: FMT.h:122
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: FMT.cpp:276
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:471
void traceSolutionPathThroughTree(Motion *goalMotion)
Trace the path from a goal state back to the start state and save the result as a solution in the Pro...
Definition: FMT.cpp:478
void setFreeSpaceVolume(const double freeSpaceVolume)
Store the volume of the obstacle-free configuration space. If no value is specified,...
Definition: FMT.h:159
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
Motion * getBestParent(Motion *m, std::vector< Motion * > &neighbors, base::Cost &cMin)
Returns the best parent and the connection cost in the neighborhood of a motion m.
Definition: FMT.cpp:617
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
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:116
base::State * goalState_
Goal state caching to accelerate cost to go heuristic computation.
Definition: FMT.h:486
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:135
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition: FMT.h:459
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
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: FMT.h:187
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for FMT*...
Definition: FMT.h:142
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: FMT.h:483
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:151
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: FMT.h:440
double distanceFunction(const Motion *a, const Motion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition: FMT.h:366
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
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
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: FMT.h:175
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
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: FMT.cpp:149
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:78
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: FMT.h:110
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region....
Definition: FMT.cpp:241
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Definition: FMT.h:493
Comparator used to order motions in a binary heap.
Definition: FMT.h:345