45 #include <boost/math/constants/constants.hpp>
46 #include <boost/math/distributions/binomial.hpp>
48 #include <ompl/datastructures/BinaryHeap.h>
49 #include <ompl/tools/config/SelfConfig.h>
50 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
51 #include <ompl/geometric/planners/fmt/FMT.h>
53 ompl::geometric::FMT::FMT(
const base::SpaceInformationPtr &si)
54 : base::Planner(si,
"FMT")
57 freeSpaceVolume_ = si_->getStateSpace()->getMeasure();
58 lastGoalMotion_ =
nullptr;
60 specs_.approximateSolutions =
false;
61 specs_.directed =
false;
63 ompl::base::Planner::declareParam<unsigned int>(
"num_samples",
this, &FMT::setNumSamples, &FMT::getNumSamples,
65 ompl::base::Planner::declareParam<double>(
"radius_multiplier",
this, &FMT::setRadiusMultiplier,
66 &FMT::getRadiusMultiplier,
"0.1:0.05:50.");
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");
73 ompl::geometric::FMT::~FMT()
85 if (pdef_->hasOptimizationObjective())
86 opt_ = pdef_->getOptimizationObjective();
89 OMPL_INFORM(
"%s: No optimization objective specified. Defaulting to optimizing path length.",
91 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
93 pdef_->setOptimizationObjective(opt_);
95 Open_.getComparisonOperator().opt_ = opt_.get();
96 Open_.getComparisonOperator().heuristics_ = heuristics_;
99 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(
this));
100 nn_->setDistanceFunction([
this](
const Motion *a,
const Motion *b)
102 return distanceFunction(a, b);
105 if (nearestK_ && !nn_->reportsSortedResults())
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());
124 std::vector<Motion *> motions;
125 motions.reserve(nn_->size());
127 for (
auto &motion : motions)
129 si_->freeState(motion->getState());
138 lastGoalMotion_ =
nullptr;
144 neighborhoods_.clear();
146 collisionChecks_ = 0;
151 Planner::getPlannerData(data);
152 std::vector<Motion *> motions;
155 if (lastGoalMotion_ !=
nullptr)
158 unsigned int size = motions.size();
159 for (
unsigned int i = 0; i < size; ++i)
161 if (motions[i]->getParent() ==
nullptr)
172 if (neighborhoods_.find(m) == neighborhoods_.end())
174 std::vector<Motion *> nbh;
176 nn_->nearestK(m, NNk_, nbh);
178 nn_->nearestR(m, NNr_, nbh);
182 neighborhoods_[m] = std::vector<Motion *>(nbh.size() - 1,
nullptr);
183 std::copy(nbh.begin() + 1, nbh.end(), neighborhoods_[m].begin());
188 neighborhoods_[m] = std::vector<Motion *>(0);
200 return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
205 double a = 1.0 / (double)dimension;
206 double unitBallVolume = calculateUnitBallVolume(dimension);
208 return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
209 std::pow(
log((
double)n) / (
double)n, a);
214 unsigned int nodeCount = 0;
215 unsigned int sampleAttempts = 0;
216 auto *motion =
new Motion(si_);
219 while (nodeCount < numSamples_ && !ptc)
221 sampler_->sampleUniform(motion->getState());
224 bool collision_free = si_->isValid(motion->getState());
233 si_->freeState(motion->getState());
237 freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
238 si_->getStateSpace()->getMeasure();
244 while (
const base::State *goalState = pis_.nextGoal())
246 auto *gMotion =
new Motion(si_);
247 si_->copyState(gMotion->getState(), goalState);
249 std::vector<Motion *> nearGoal;
250 nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
253 if (nearGoal.empty())
256 if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
259 goalState_ = gMotion->getState();
263 si_->freeState(gMotion->getState());
269 goalState_ = nearGoal[0]->getState();
270 si_->freeState(gMotion->getState());
278 if (lastGoalMotion_ !=
nullptr)
280 OMPL_INFORM(
"solve() called before clear(); returning previous solution");
281 traceSolutionPathThroughTree(lastGoalMotion_);
282 OMPL_DEBUG(
"Final path cost: %f", lastGoalMotion_->getCost().value());
287 OMPL_INFORM(
"solve() called before clear(); no previous solution so starting afresh");
293 Motion *initMotion =
nullptr;
297 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
304 initMotion =
new Motion(si_);
305 si_->copyState(initMotion->
getState(), st);
306 Open_.insert(initMotion);
309 nn_->add(initMotion);
312 if (initMotion ==
nullptr)
320 sampler_ = si_->allocStateSampler();
322 assureGoalIsSampled(goal);
323 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
329 NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (
double)si_->getStateDimension()) *
330 (boost::math::constants::e<double>() / (
double)si_->getStateDimension()) *
331 log((
double)nn_->size()));
332 OMPL_DEBUG(
"Using nearest-neighbors k of %d", NNk_);
336 NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
341 bool plannerSuccess =
false;
342 bool successfulExpansion =
false;
348 if ((plannerSuccess = goal->isSatisfied(z->
getState())))
351 successfulExpansion = expandTreeFromNode(&z);
353 if (!extendedFMT_ && !successfulExpansion)
355 if (extendedFMT_ && !successfulExpansion)
358 std::vector<Motion *> nbh;
359 std::vector<base::Cost> costs;
360 std::vector<base::Cost> incCosts;
361 std::vector<std::size_t> sortedCostIndices;
366 auto *m =
new Motion(si_);
367 while (!ptc && Open_.empty())
369 sampler_->sampleUniform(m->getState());
371 if (!si_->isValid(m->getState()))
375 nn_->nearestK(m, NNk_, nbh);
377 nn_->nearestR(m, NNr_, nbh);
380 std::vector<Motion *> yNear;
381 yNear.reserve(nbh.size());
384 if (j->getSetType() == Motion::SET_CLOSED)
390 const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
392 opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
394 if (opt_->isCostBetterThan(worstCost, connCost))
411 if (costs.size() < yNear.size())
413 costs.resize(yNear.size());
414 incCosts.resize(yNear.size());
415 sortedCostIndices.resize(yNear.size());
423 for (std::size_t i = 0; i < yNear.size(); ++i)
425 incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
426 costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
433 for (std::size_t i = 0; i < yNear.size(); ++i)
434 sortedCostIndices[i] = i;
435 std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
438 for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
439 i != sortedCostIndices.begin() + yNear.size(); ++i)
441 if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
443 m->setParent(yNear[*i]);
444 yNear[*i]->getChildren().push_back(m);
445 const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
446 m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
447 m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
448 m->setSetType(Motion::SET_OPEN);
452 updateNeighborhood(m, nbh);
467 traceSolutionPathThroughTree(lastGoalMotion_);
469 OMPL_DEBUG(
"Final path cost: %f", lastGoalMotion_->getCost().value());
475 return {
false,
false};
480 std::vector<Motion *> mpath;
481 Motion *solution = goalMotion;
484 while (solution !=
nullptr)
486 mpath.push_back(solution);
491 auto path(std::make_shared<PathGeometric>(si_));
492 int mPathSize = mpath.size();
493 for (
int i = mPathSize - 1; i >= 0; --i)
494 path->append(mpath[i]->getState());
495 pdef_->addSolutionPath(path,
false, -1.0, getName());
502 std::vector<Motion *> xNear;
503 const std::vector<Motion *> &zNeighborhood = neighborhoods_[*z];
504 const unsigned int zNeighborhoodSize = zNeighborhood.size();
505 xNear.reserve(zNeighborhoodSize);
507 for (
unsigned int i = 0; i < zNeighborhoodSize; ++i)
509 Motion *x = zNeighborhood[i];
510 if (x->getSetType() == Motion::SET_UNVISITED)
517 const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
518 const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
520 if (opt_->isCostBetterThan(worstCost, connCost))
530 std::vector<Motion *> yNear;
531 std::vector<Motion *> Open_new;
532 const unsigned int xNearSize = xNear.size();
533 for (
unsigned int i = 0; i < xNearSize; ++i)
538 const std::vector<Motion *> &xNeighborhood = neighborhoods_[x];
540 const unsigned int xNeighborhoodSize = xNeighborhood.size();
541 yNear.reserve(xNeighborhoodSize);
542 for (
unsigned int j = 0; j < xNeighborhoodSize; ++j)
544 if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
545 yNear.push_back(xNeighborhood[j]);
550 Motion *yMin = getBestParent(x, yNear, cMin);
556 bool collision_free =
false;
561 collision_free = si_->checkMotion(yMin->
getState(), x->getState());
572 collision_free = si_->checkMotion(yMin->
getState(), x->getState());
580 x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
584 Open_new.push_back(x);
586 x->setSetType(Motion::SET_CLOSED);
593 (*z)->setSetType(Motion::SET_CLOSED);
596 unsigned int openNewSize = Open_new.size();
597 for (
unsigned int i = 0; i < openNewSize; ++i)
599 Open_.insert(Open_new[i]);
600 Open_new[i]->setSetType(Motion::SET_OPEN);
607 OMPL_INFORM(
"Open is empty before path was found --> no feasible path exists");
612 *z = Open_.top()->data;
621 const unsigned int neighborsSize = neighbors.size();
622 for (
unsigned int j = 0; j < neighborsSize; ++j)
626 const base::Cost cNew = opt_->combineCosts(neighbors[j]->getCost(), dist);
628 if (opt_->isCostBetterThan(cNew, cMin))
643 if (i->getSetType() == Motion::SET_CLOSED || neighborhoods_.find(i) != neighborhoods_.end())
646 const base::Cost worstCost = opt_->motionCost(neighborhoods_[i].back()->getState(), i->getState());
648 if (opt_->isCostBetterThan(worstCost, connCost))
652 std::vector<Motion *> &nbhToUpdate = neighborhoods_[i];
653 for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
656 const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
657 if (opt_->isCostBetterThan(connCost, cost))
659 nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
666 std::vector<Motion *> nbh2;
668 nn_->nearestK(m, NNk_, nbh2);
670 nn_->nearestR(m, NNr_, nbh2);
675 neighborhoods_[i] = std::vector<Motion *>(nbh2.size() - 1,
nullptr);
676 std::copy(nbh2.begin() + 1, nbh2.end(), neighborhoods_[i].begin());
681 neighborhoods_[i] = std::vector<Motion *>(0);
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
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,...
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...
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...
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...
Definition of an abstract state.
Representation of a motion.
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
void addCC(Motion *m)
Caches a failed collision check to m.
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
std::vector< Motion * > & getChildren()
Get the children of the motion.
base::State * getState() const
Get the state associated with the motion.
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Motion * getParent() const
Get the parent motion of the current motion.
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L....
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...
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...
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 ...
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.
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 freeMemory()
Free the memory allocated by this planner.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
bool expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
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 setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region....
#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.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.