Loading...
Searching...
No Matches
FMT.cpp
56 // An upper bound on the free space volume is the total space volume; the free fraction is estimated in sampleFree
63 ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples,
67 ompl::base::Planner::declareParam<bool>("use_k_nearest", this, &FMT::setNearestK, &FMT::getNearestK, "0,1");
68 ompl::base::Planner::declareParam<bool>("cache_cc", this, &FMT::setCacheCC, &FMT::getCacheCC, "0,1");
69 ompl::base::Planner::declareParam<bool>("heuristics", this, &FMT::setHeuristics, &FMT::getHeuristics, "0,1");
70 ompl::base::Planner::declareParam<bool>("extended_fmt", this, &FMT::setExtendedFMT, &FMT::getExtendedFMT, "0,1");
107 OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
115 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
200 return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
203double ompl::geometric::FMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
208 return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
237 freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
276ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
323 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
518 const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
617ompl::geometric::FMT::Motion *ompl::geometric::FMT::getBestParent(Motion *m, std::vector<Motion *> &neighbors,
646 const base::Cost worstCost = opt_->motionCost(neighborhoods_[i].back()->getState(), i->getState());
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double getThreshold() const
Get the distance to the goal that is allowed for a state to be considered in the goal region.
Definition GoalRegion.h:82
Abstract definition of a goal region that can be sampled.
Definition GoalSampleableRegion.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition PlannerData.h:175
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition PlannerData.cpp:413
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition PlannerData.cpp:422
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition PlannerData.cpp:432
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter, const GetterType &getter, const std::string &rangeSuggestion="")
This function declares a parameter for this planner instance, and specifies the setter and getter fun...
Definition Planner.h:371
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition FMT.h:278
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
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
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition FMT.h:474
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 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
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
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition FMT.h:483
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
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 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
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition PlannerStatus.h:60