2#include <boost/math/constants/constants.hpp>
3#include <boost/math/distributions/binomial.hpp>
4#include <ompl/datastructures/BinaryHeap.h>
5#include <ompl/tools/config/SelfConfig.h>
7#include <ompl/datastructures/NearestNeighborsGNAT.h>
8#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
9#include <ompl/geometric/planners/fmt/BFMT.h>
12#include <ompl/base/spaces/RealVectorStateSpace.h>
18 BFMT::BFMT(
const base::SpaceInformationPtr &si)
19 : base::Planner(si,
"BFMT")
20 , freeSpaceVolume_(si_->getStateSpace()->getMeasure())
24 specs_.approximateSolutions =
false;
25 specs_.directed =
false;
27 ompl::base::Planner::declareParam<unsigned int>(
"num_samples",
this, &BFMT::setNumSamples,
28 &BFMT::getNumSamples,
"10:10:1000000");
29 ompl::base::Planner::declareParam<double>(
"radius_multiplier",
this, &BFMT::setRadiusMultiplier,
30 &BFMT::getRadiusMultiplier,
"0.1:0.05:50.");
31 ompl::base::Planner::declareParam<bool>(
"nearest_k",
this, &BFMT::setNearestK, &BFMT::getNearestK,
"0,1");
32 ompl::base::Planner::declareParam<bool>(
"balanced",
this, &BFMT::setExploration, &BFMT::getExploration,
34 ompl::base::Planner::declareParam<bool>(
"optimality",
this, &BFMT::setTermination, &BFMT::getTermination,
36 ompl::base::Planner::declareParam<bool>(
"heuristics",
this, &BFMT::setHeuristics, &BFMT::getHeuristics,
38 ompl::base::Planner::declareParam<bool>(
"cache_cc",
this, &BFMT::setCacheCC, &BFMT::getCacheCC,
"0,1");
39 ompl::base::Planner::declareParam<bool>(
"extended_fmt",
this, &BFMT::setExtendedFMT, &BFMT::getExtendedFMT,
43 ompl::geometric::BFMT::~BFMT()
55 if (
pdef_->hasOptimizationObjective())
59 OMPL_INFORM(
"%s: No optimization objective specified. Defaulting to optimizing path length.",
61 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(
si_);
71 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<BiDirMotion *>(
this));
79 OMPL_WARN(
"%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
87 OMPL_INFORM(
"%s: problem definition is not set, deferring setup completion...",
getName().c_str());
96 BiDirMotionPtrs motions;
98 for (
auto &motion : motions)
100 si_->freeState(motion->getState());
124 BiDirMotionPtrs motions;
127 int numStartNodes = 0;
128 int numGoalNodes = 0;
133 int fwd_tree_tag = 1;
134 int rev_tree_tag = 2;
136 for (
auto motion : motions)
138 bool inFwdTree = (motion->currentSet_[FWD] != BiDirMotion::SET_UNVISITED);
143 if (motion->parent_[FWD] ==
nullptr)
163 for (
auto motion : motions)
165 bool inRevTree = (motion->currentSet_[REV] != BiDirMotion::SET_UNVISITED);
170 if (motion->parent_[REV] ==
nullptr)
195 BiDirMotionPtrs neighborhood;
197 nn_->nearestK(m,
NNk_, neighborhood);
199 nn_->nearestR(m,
NNr_, neighborhood);
201 if (!neighborhood.empty())
204 neighborhoods_[m] = std::vector<BiDirMotion *>(neighborhood.size() - 1,
nullptr);
205 std::copy(neighborhood.begin() + 1, neighborhood.end(),
neighborhoods_[m].begin());
218 unsigned int nodeCount = 0;
219 unsigned int sampleAttempts = 0;
225 sampler_->sampleUniform(motion->getState());
227 if (
si_->isValid(motion->getState()))
234 si_->freeState(motion->getState());
239 boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
240 si_->getStateSpace()->getMeasure();
254 double a = 1.0 / (double)dimension;
258 std::pow(log((
double)n) / (
double)n, a);
275 if (goal_s ==
nullptr)
284 bool valid_initMotion =
false;
291 initMotion->
currentSet_[REV] = BiDirMotion::SET_UNVISITED;
292 nn_->add(initMotion);
297 initMotion->
currentSet_[FWD] = BiDirMotion::SET_OPEN;
299 valid_initMotion =
true;
304 if ((initMotion ==
nullptr) || !valid_initMotion)
306 OMPL_ERROR(
"Start state undefined or invalid.");
312 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure",
getName().c_str(),
319 (boost::math::constants::e<double>() / (
double)
si_->getStateDimension()) *
320 log((
double)
nn_->size()));
330 bool valid_goalMotion =
false;
337 goalMotion->
currentSet_[FWD] = BiDirMotion::SET_UNVISITED;
338 nn_->add(goalMotion);
343 goalMotion->
currentSet_[REV] = BiDirMotion::SET_OPEN;
345 valid_goalMotion =
true;
350 if ((goalMotion ==
nullptr) || !valid_goalMotion)
352 OMPL_ERROR(
"Goal state undefined or invalid.");
360 bool earlyFailure =
true;
362 if (initMotion !=
nullptr && goalMotion !=
nullptr)
364 earlyFailure =
plan(initMotion, goalMotion, connection_point, ptc);
368 OMPL_ERROR(
"Initial/goal state(s) are undefined!");
379 base::Cost fwd_cost, rev_cost, connection_cost;
383 BiDirMotionPtrs path_fwd;
385 fwd_cost = connection_point->
getCost();
388 BiDirMotionPtrs path_rev;
390 rev_cost = connection_point->
getCost();
395 if (path_rev.size() > 1)
397 connection_cost =
base::Cost(rev_cost.
value() - path_rev[1]->getCost().value());
398 path_rev.erase(path_rev.begin());
400 else if (path_fwd.size() > 1)
402 connection_cost =
base::Cost(fwd_cost.
value() - path_fwd[1]->getCost().value());
403 path_fwd.erase(path_fwd.begin());
407 OMPL_ERROR(
"Solution path traced incorrectly or otherwise constructed improperly \
408 through forward/reverse trees (both paths are one node in length, each).");
413 path_rev[0]->setCost(
base::Cost(path_fwd[0]->getCost().value() + connection_cost.
value()));
414 path_rev[0]->setParent(path_fwd[0]);
415 for (
unsigned int i = 1; i < path_rev.size(); ++i)
417 path_rev[i]->setCost(
419 path_rev[i]->setParent(path_rev[i - 1]);
422 BiDirMotionPtrs mpath;
423 std::reverse(path_rev.begin(), path_rev.end());
424 mpath.reserve(path_fwd.size() + path_rev.size());
425 mpath.insert(mpath.end(), path_rev.begin(), path_rev.end());
426 mpath.insert(mpath.end(), path_fwd.begin(), path_fwd.end());
429 auto path(std::make_shared<PathGeometric>(
si_));
430 for (
int i = mpath.size() - 1; i >= 0; --i)
432 path->append(mpath[i]->getState());
435 static const bool approximate =
false;
436 static const double cost_difference_from_goal = 0.0;
437 pdef_->addSolutionPath(path, approximate, cost_difference_from_goal,
getName());
444 return {
false,
false};
450 BiDirMotionPtrs Open_new;
453 BiDirMotionPtrs zNear;
456 for (
auto i : zNeighborhood)
458 if (i->getCurrentSet() == BiDirMotion::SET_UNVISITED)
471 BiDirMotionPtrs xNear;
473 for (
auto j : xNeighborhood)
475 if (j->getCurrentSet() == BiDirMotion::SET_OPEN)
482 double cMin = std::numeric_limits<double>::infinity();
483 for (
auto &j : xNear)
498 bool collision_free =
false;
503 collision_free =
si_->checkMotion(xMin->
getState(), x->getState());
514 collision_free =
si_->checkMotion(xMin->
getState(), x->getState());
528 if (x->getOtherSet() != BiDirMotion::SET_UNVISITED)
530 if (connection_point ==
nullptr)
532 connection_point = x;
541 (x->cost_[FWD].value() + x->cost_[REV].value()))
543 connection_point = x;
548 Open_new.push_back(x);
549 x->setCurrentSet(BiDirMotion::SET_CLOSED);
561 for (
auto &i : Open_new)
566 i->setCurrentSet(BiDirMotion::SET_OPEN);
577 BiDirMotionPtrs sampleNodes;
578 nn_->list(sampleNodes);
583 for (
auto &sampleNode : sampleNodes)
597 for (
auto &sampleNode : sampleNodes)
599 sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
606 for (
auto &sampleNode : sampleNodes)
608 sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
613 bool earlyFailure =
false;
614 bool success =
false;
634 OMPL_INFORM(
"Both Open are empty before path was found --> no feasible path exists");
651 earlyFailure =
false;
659 std::vector<BiDirMotion *> nbh;
660 std::vector<base::Cost> costs;
661 std::vector<base::Cost> incCosts;
662 std::vector<std::size_t> sortedCostIndices;
671 sampler_->sampleUniform(m->getState());
672 if (!
si_->isValid(m->getState()))
676 std::vector<BiDirMotion *> yNear;
682 yNear.reserve(nbh.size());
685 if (j->getCurrentSet() == BiDirMotion::SET_CLOSED)
691 const base::Cost connCost =
opt_->motionCost(j->getState(), m->getState());
695 if (
opt_->isCostBetterThan(worstCost, connCost))
712 if (costs.size() < yNear.size())
714 costs.resize(yNear.size());
715 incCosts.resize(yNear.size());
716 sortedCostIndices.resize(yNear.size());
724 for (std::size_t i = 0; i < yNear.size(); ++i)
726 incCosts[i] =
opt_->motionCost(yNear[i]->getState(), m->getState());
727 costs[i] =
opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
734 for (std::size_t i = 0; i < yNear.size(); ++i)
735 sortedCostIndices[i] = i;
736 std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
739 for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
740 i != sortedCostIndices.begin() + yNear.size(); ++i)
743 if (
si_->checkMotion(yNear[*i]->getState(), m->getState()))
745 const base::Cost incCost =
opt_->motionCost(yNear[*i]->getState(), m->getState());
746 m->setParent(yNear[*i]);
747 yNear[*i]->getChildren().push_back(m);
748 m->setCost(
opt_->combineCosts(yNear[*i]->getCost(), incCost));
750 m->setCurrentSet(BiDirMotion::SET_OPEN);
766 bool terminate =
false;
771 return (connection_point !=
nullptr || ptc);
778 else if (z->
getOtherSet() == BiDirMotion::SET_CLOSED)
791 case SWAP_EVERY_TIME:
802 case CHOOSE_SMALLEST_Z:
834 while (solution !=
nullptr)
836 path.push_back(solution);
854 if (i->getCurrentSet() == BiDirMotion::SET_CLOSED)
860 if (it->second.empty())
864 const base::Cost worstCost =
opt_->motionCost(it->second.back()->getState(), i->getState());
866 if (
opt_->isCostBetterThan(worstCost, connCost))
870 std::vector<BiDirMotion *> &nbhToUpdate = it->second;
871 for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
874 const base::Cost cost =
opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
875 if (
opt_->isCostBetterThan(connCost, cost))
877 nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
Element * insert(const _T &data)
Add a new element.
void remove(Element *element)
Remove a specific element.
Element * top() const
Return the top element. nullptr for an empty heap.
void clear()
Clear the heap.
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Abstract representation of a container that can perform nearest neighbors queries.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
double value() const
The value of the cost.
Abstract definition of a goal region that can be sampled.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
ProblemDefinitionPtr pdef_
The user set problem definition.
const std::string & getName() const
Get the name of the planner.
SpaceInformationPtr si_
The space information for which planning is done.
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
bool setup_
Flag indicating whether setup() has been called.
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition of an abstract state.
Representation of a bidirectional motion.
BiDirMotion * getParent() const
Get the parent of the motion.
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
base::Cost cost_[2]
The cost of this motion
SetType getOtherSet() const
Get set of this motion in the inactive tree.
base::State * getState() const
Get the state associated with the motion.
BiDirMotionPtrs getChildren() const
Get the children of the motion.
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
SetType currentSet_[2]
Current set in which the motion is included.
void setCurrentSet(SetType set)
Set the current set of the motion.
base::Cost getCost() const
Set the state associated with the motion.
TerminateType termination_
Termination strategy used.
double NNr_
Radius employed in the nearestR strategy.
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
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.
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
TreeType tree_
Active tree.
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
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 freeMemory()
Free the memory allocated by this planner.
BiDirMotionBinHeap Open_[2]
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L....
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
Compute the distance between two motions as the cost between their contained states....
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...
ExploreType exploration_
Exploration strategy used.
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
bool heuristics_
Flag to activate the cost to go heuristics.
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
base::StateSamplerPtr sampler_
State sampler.
std::map< BiDirMotion *, BiDirMotionPtrs > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
base::OptimizationObjectivePtr opt_
The cost objective function.
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
void saveNeighborhood(BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
unsigned int NNk_
K used in the nearestK strategy.
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 ...
void useFwdTree()
Sets forward tree active.
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...
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...
unsigned int numSamples_
The number of samples to use when planning.
void useRevTree()
Sets reverse tree active.
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
void swapTrees()
Change the active tree.
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
bool cacheCC_
Flag to activate the collision check caching.
bool nearestK_
Flag to activate the K nearest neighbors strategy.
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.