52 namespace ompl
95 ~FMT() override;
97 void setup() override;
101 void clear() override;
112 numSamples_ = numSamples;
118 return numSamples_;
124 nearestK_ = nearestK;
130 return nearestK_;
145 throw Exception("Radius multiplier must be greater than zero");
146 radiusMultiplier_ = radiusMultiplier;
153 return radiusMultiplier_;
162 throw Exception("Free space volume should be greater than zero");
163 freeSpaceVolume_ = freeSpaceVolume;
170 return freeSpaceVolume_;
177 cacheCC_ = ccc;
183 return cacheCC_;
189 heuristics_ = h;
196 return heuristics_;
202 extendedFMT_ = e;
208 return extendedFMT_;
241 ~Motion() = default;
246 state_ = state;
252 return state_;
258 parent_ = parent;
264 return parent_;
270 cost_ = cost;
276 return cost_;
282 currentSet_ = currentSet;
288 return currentSet_;
309 hcost_ = h;
315 return hcost_;
321 return children_;
358 if (heuristics_)
366 bool heuristics_;
379 void freeMemory();
394 double calculateUnitBallVolume(const unsigned int dimension) const;
403 double calculateRadius(unsigned int dimension, unsigned int n) const;
502 : costs_(costs), opt_(opt)
Flag to activate the K nearest neighbors strategy.
Flag to activate the collision check caching.
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
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.
bool getNearestK() const
Get the state of the nearestK strategy.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
void sampleFree(const ompl::base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
unsigned int numSamples_
The number of samples to use when planning.
The FMT* planner begins with all nodes included in set Unvisited "Waiting for optimal connection"...
void setParent(Motion *parent)
Set the parent motion of the current motion.
SetType getSetType() const
Get the set that this motion belongs to.
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*...
std::vector< Motion * > & getChildren()
Get the children of the motion.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void addCC(Motion *m)
Caches a failed collision check to m.
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)
The flag indicating which set a motion belongs to.
A shared pointer wrapper for ompl::base::StateSampler.
Radius employed in the nearestR strategy.
Add new samples if the tree was not able to find a solution.
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
The cost objective function.
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
ompl::BinaryHeap< Motion *, MotionCompare > MotionBinHeap
A binary heap for storing explored motions in cost-to-come sorted order.
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
base::Cost getHeuristicCost() const
Get the cost to go heuristic cost.
The minimum cost to go of this motion (heuristically computed)
Free the memory allocated by this planner.
base::Cost getCost() const
Get the cost-to-come for the current motion.
void setNumSamples(const unsigned int numSamples)
Set the number of states that the planner should sample. The planner will sample this number of state...
bool expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
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...
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Asymptotically Optimal Fast Marching Tree algorithm developed by L. Janson and M. Pavone...
Abstract definition of a goal region that can be sampled.
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.
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
std::set< Motion * > collChecksDone_
Contains the connections attempted FROM this node.
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
base::State * getState() const
Get the state associated with the motion.
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
A class to store the exit status of Planner::solve()
A shared pointer wrapper for ompl::base::SpaceInformation.
base::State * goalState_
Goal state caching to accelerate cost to go heuristic computation.
void setState(base::State *state)
Set the state associated with the motion.
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
bool getCacheCC() const
Get the state of the collision check caching.
double getFreeSpaceVolume() const
Get the volume of the free configuration space that is being used by the planner. ...
Abstract definition of optimization objectives.
std::vector< Motion * > children_
The set of motions descending from the current motion.
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
A 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.
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
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...
std::map< Motion *, std::vector< Motion * > > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Motion * parent_
The parent motion in the exploration tree.
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
void updateNeighborhood(Motion *m, const std::vector< Motion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Comparator used to order motions in a binary heap.
base::State * state_
The state contained by the motion.
Flag to activate the cost to go heuristics.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision...