FMT.h
MotionBinHeap H_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in H have been e...
Definition: FMT.h:323
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:181
SetType
The FMT* planner begins with all nodes included in set W "Waiting for optimal connection". As nodes are connected to the tree, they are transferred into set H "Horizon of explored tree." Once a node in H is no longer close enough to the frontier to connect to any more nodes in W, it is removed from H. These three SetTypes are flags indicating which set the node belongs to; H, W, or neither.
Definition: FMT.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 FMT*...
Definition: FMT.h:119
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:72
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: FMT.h:136
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:265
A boost shared pointer wrapper for ompl::base::StateSampler.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
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:331
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:98
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:108
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:330
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: FMT.cpp:238
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: FMT.h:172
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:104
ompl::BinaryHeap< Motion *, MotionCompare > MotionBinHeap
A binary heap for storing explored motions in cost-to-come sorted order.
Definition: FMT.h:317
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone...
Definition: FMT.h:77
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:354
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:128
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:218
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:206
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:66
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: FMT.h:363
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region. If not, add a goal state from that region directly into the set of vertices. In this way, FMT is able to find a solution, if one exists. If no sampled nodes are within a goal region, there would be no way for the algorithm to successfully find a path to that region.
Definition: FMT.cpp:204
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Definition: FMT.h:145
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: FMT.cpp:121
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:173
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:351
bool expandTreeFromNode(Motion *&z, const double r)
Complete one iteration of the main loop of the FMT* algorithm: Find all nodes in set W within a radiu...
Definition: FMT.cpp:350
void saveNeighborhood(Motion *m, const double r)
Save the neighbors within a given radius of a state.
Definition: FMT.cpp:141
Comparator used to order motions in a binary heap.
Definition: FMT.h:245
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:163
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
std::map< Motion *, MotionBinHeap::Element * > hElements_
A map of all of the elements stored within the MotionBinHeap H, used to convert between Motion and El...
Definition: FMT.h:327