BFMT.cpp
1 
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>
6 
7 #include <ompl/datastructures/NearestNeighborsGNAT.h>
8 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
9 #include <ompl/geometric/planners/fmt/BFMT.h>
10 
11 #include <fstream>
12 #include <ompl/base/spaces/RealVectorStateSpace.h>
13 
14 namespace ompl
15 {
16  namespace geometric
17  {
18  BFMT::BFMT(const base::SpaceInformationPtr &si)
19  : base::Planner(si, "BFMT")
20  , freeSpaceVolume_(si_->getStateSpace()->getMeasure()) // An upper bound on the free space volume is the
21  // total space volume; the free fraction is estimated
22  // in sampleFree
23  {
25  specs_.directed = false;
26 
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,
33  "0,1");
34  ompl::base::Planner::declareParam<bool>("optimality", this, &BFMT::setTermination, &BFMT::getTermination,
35  "0,1");
36  ompl::base::Planner::declareParam<bool>("heuristics", this, &BFMT::setHeuristics, &BFMT::getHeuristics,
37  "0,1");
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,
40  "0,1");
41  }
42 
43  ompl::geometric::BFMT::~BFMT()
44  {
45  freeMemory();
46  }
47 
48  void BFMT::setup()
49  {
50  if (pdef_)
51  {
52  /* Setup the optimization objective. If no optimization objective was
53  specified, then default to optimizing path length as computed by the
54  distance() function in the state space */
55  if (pdef_->hasOptimizationObjective())
56  opt_ = pdef_->getOptimizationObjective();
57  else
58  {
59  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
60  getName().c_str());
61  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
62  // Store the new objective in the problem def'n
63  pdef_->setOptimizationObjective(opt_);
64  }
65  Open_[0].getComparisonOperator().opt_ = opt_.get();
66  Open_[0].getComparisonOperator().heuristics_ = heuristics_;
67  Open_[1].getComparisonOperator().opt_ = opt_.get();
68  Open_[1].getComparisonOperator().heuristics_ = heuristics_;
69 
70  if (!nn_)
71  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<BiDirMotion *>(this));
72  nn_->setDistanceFunction([this](const BiDirMotion *a, const BiDirMotion *b)
73  {
74  return distanceFunction(a, b);
75  });
76 
77  if (nearestK_ && !nn_->reportsSortedResults())
78  {
79  OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
80  "disabled.",
81  getName().c_str());
82  nearestK_ = false;
83  }
84  }
85  else
86  {
87  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
88  setup_ = false;
89  }
90  }
91 
93  {
94  if (nn_)
95  {
96  BiDirMotionPtrs motions;
97  nn_->list(motions);
98  for (auto &motion : motions)
99  {
100  si_->freeState(motion->getState());
101  delete motion;
102  }
103  }
104  }
105 
106  void BFMT::clear()
107  {
108  Planner::clear();
109  sampler_.reset();
110  freeMemory();
111  if (nn_)
112  nn_->clear();
113  Open_[FWD].clear();
114  Open_[REV].clear();
115  Open_elements[FWD].clear();
116  Open_elements[REV].clear();
117  neighborhoods_.clear();
118  collisionChecks_ = 0;
119  }
120 
122  {
124  BiDirMotionPtrs motions;
125  nn_->list(motions);
126 
127  int numStartNodes = 0;
128  int numGoalNodes = 0;
129  int numEdges = 0;
130  int numFwdEdges = 0;
131  int numRevEdges = 0;
132 
133  int fwd_tree_tag = 1;
134  int rev_tree_tag = 2;
135 
136  for (auto motion : motions)
137  {
138  bool inFwdTree = (motion->currentSet_[FWD] != BiDirMotion::SET_UNVISITED);
139 
140  // For samples added to the fwd tree, add incoming edges (from fwd tree parent)
141  if (inFwdTree)
142  {
143  if (motion->parent_[FWD] == nullptr)
144  {
145  // Motion is a forward tree root node
146  ++numStartNodes;
147  }
148  else
149  {
150  bool success =
151  data.addEdge(base::PlannerDataVertex(motion->parent_[FWD]->getState(), fwd_tree_tag),
152  base::PlannerDataVertex(motion->getState(), fwd_tree_tag));
153  if (success)
154  {
155  ++numFwdEdges;
156  ++numEdges;
157  }
158  }
159  }
160  }
161 
162  // The edges in the goal tree are reversed so that they are in the same direction as start tree
163  for (auto motion : motions)
164  {
165  bool inRevTree = (motion->currentSet_[REV] != BiDirMotion::SET_UNVISITED);
166 
167  // For samples added to a tree, add incoming edges (from fwd tree parent)
168  if (inRevTree)
169  {
170  if (motion->parent_[REV] == nullptr)
171  {
172  // Motion is a reverse tree root node
173  ++numGoalNodes;
174  }
175  else
176  {
177  bool success =
178  data.addEdge(base::PlannerDataVertex(motion->getState(), rev_tree_tag),
179  base::PlannerDataVertex(motion->parent_[REV]->getState(), rev_tree_tag));
180  if (success)
181  {
182  ++numRevEdges;
183  ++numEdges;
184  }
185  }
186  }
187  }
188  }
189 
191  {
192  // Check if neighborhood has already been saved
193  if (neighborhoods_.find(m) == neighborhoods_.end())
194  {
195  BiDirMotionPtrs neighborhood;
196  if (nearestK_)
197  nn_->nearestK(m, NNk_, neighborhood);
198  else
199  nn_->nearestR(m, NNr_, neighborhood);
200 
201  if (!neighborhood.empty())
202  {
203  // Save the neighborhood but skip the first element (m)
204  neighborhoods_[m] = std::vector<BiDirMotion *>(neighborhood.size() - 1, nullptr);
205  std::copy(neighborhood.begin() + 1, neighborhood.end(), neighborhoods_[m].begin());
206  }
207  else
208  {
209  // Save an empty neighborhood
210  neighborhoods_[m] = std::vector<BiDirMotion *>(0);
211  }
212  }
213  }
214 
215  void BFMT::sampleFree(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn,
217  {
218  unsigned int nodeCount = 0;
219  unsigned int sampleAttempts = 0;
220  auto *motion = new BiDirMotion(si_, &tree_);
221 
222  // Sample numSamples_ number of nodes from the free configuration space
223  while (nodeCount < numSamples_ && !ptc)
224  {
225  sampler_->sampleUniform(motion->getState());
226  sampleAttempts++;
227  if (si_->isValid(motion->getState()))
228  { // collision checking
229  ++nodeCount;
230  nn->add(motion);
231  motion = new BiDirMotion(si_, &tree_);
232  }
233  }
234  si_->freeState(motion->getState());
235  delete motion;
236 
237  // 95% confidence limit for an upper bound for the true free space volume
239  boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
240  si_->getStateSpace()->getMeasure();
241  }
242 
243  double BFMT::calculateUnitBallVolume(const unsigned int dimension) const
244  {
245  if (dimension == 0)
246  return 1.0;
247  if (dimension == 1)
248  return 2.0;
249  return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
250  }
251 
252  double BFMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
253  {
254  double a = 1.0 / (double)dimension;
255  double unitBallVolume = calculateUnitBallVolume(dimension);
256 
257  return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
258  std::pow(log((double)n) / (double)n, a);
259  }
260 
262  {
263  checkValidity();
264  if (!sampler_)
265  {
266  sampler_ = si_->allocStateSampler();
267  }
268  goal_s = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
269  }
270 
272  {
274  initializeProblem(goal_s);
275  if (goal_s == nullptr)
276  {
277  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
279  }
280 
281  useFwdTree();
282 
283  // Add start states to Unvisitedfwd and Openfwd
284  bool valid_initMotion = false;
285  BiDirMotion *initMotion;
286  while (const base::State *st = pis_.nextStart())
287  {
288  initMotion = new BiDirMotion(si_, &tree_);
289  si_->copyState(initMotion->getState(), st);
290 
291  initMotion->currentSet_[REV] = BiDirMotion::SET_UNVISITED;
292  nn_->add(initMotion); // S <-- {x_init}
293  if (si_->isValid(initMotion->getState()))
294  {
295  // Take the first valid initial state as the forward tree root
296  Open_elements[FWD][initMotion] = Open_[FWD].insert(initMotion);
297  initMotion->currentSet_[FWD] = BiDirMotion::SET_OPEN;
298  initMotion->cost_[FWD] = opt_->initialCost(initMotion->getState());
299  valid_initMotion = true;
300  heurGoalState_[1] = initMotion->getState();
301  }
302  }
303 
304  if ((initMotion == nullptr) || !valid_initMotion)
305  {
306  OMPL_ERROR("Start state undefined or invalid.");
308  }
309 
310  // Sample N free states in configuration state_
311  sampleFree(nn_, ptc); // S <-- SAMPLEFREE(N)
312  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
313  nn_->size());
314 
315  // Calculate the nearest neighbor search radius
316  if (nearestK_)
317  {
318  NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
319  (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
320  log((double)nn_->size()));
321  OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
322  }
323  else
324  {
325  NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
326  OMPL_DEBUG("Using radius of %f", NNr_);
327  }
328 
329  // Add goal states to Unvisitedrev and Openrev
330  bool valid_goalMotion = false;
331  BiDirMotion *goalMotion;
332  while (const base::State *st = pis_.nextGoal())
333  {
334  goalMotion = new BiDirMotion(si_, &tree_);
335  si_->copyState(goalMotion->getState(), st);
336 
337  goalMotion->currentSet_[FWD] = BiDirMotion::SET_UNVISITED;
338  nn_->add(goalMotion); // S <-- {x_goal}
339  if (si_->isValid(goalMotion->getState()))
340  {
341  // Take the first valid goal state as the reverse tree root
342  Open_elements[REV][goalMotion] = Open_[REV].insert(goalMotion);
343  goalMotion->currentSet_[REV] = BiDirMotion::SET_OPEN;
344  goalMotion->cost_[REV] = opt_->terminalCost(goalMotion->getState());
345  valid_goalMotion = true;
346  heurGoalState_[0] = goalMotion->getState();
347  }
348  }
349 
350  if ((goalMotion == nullptr) || !valid_goalMotion)
351  {
352  OMPL_ERROR("Goal state undefined or invalid.");
354  }
355 
356  useRevTree();
357 
358  // Plan a path
359  BiDirMotion *connection_point = nullptr;
360  bool earlyFailure = true;
361 
362  if (initMotion != nullptr && goalMotion != nullptr)
363  {
364  earlyFailure = plan(initMotion, goalMotion, connection_point, ptc);
365  }
366  else
367  {
368  OMPL_ERROR("Initial/goal state(s) are undefined!");
369  }
370 
371  if (earlyFailure)
372  {
373  return base::PlannerStatus(false, false);
374  }
375 
376  // Save the best path (through z)
377  if (!ptc)
378  {
379  base::Cost fwd_cost, rev_cost, connection_cost;
380 
381  // Construct the solution path
382  useFwdTree();
383  BiDirMotionPtrs path_fwd;
384  tracePath(connection_point, path_fwd);
385  fwd_cost = connection_point->getCost();
386 
387  useRevTree();
388  BiDirMotionPtrs path_rev;
389  tracePath(connection_point, path_rev);
390  rev_cost = connection_point->getCost();
391 
392  // ASSUMES FROM THIS POINT THAT z = path_fwd[0] = path_rev[0]
393  // Remove the first element, z, in the traced reverse path
394  // (the same as the first element in the traced forward path)
395  if (path_rev.size() > 1)
396  {
397  connection_cost = base::Cost(rev_cost.value() - path_rev[1]->getCost().value());
398  path_rev.erase(path_rev.begin());
399  }
400  else if (path_fwd.size() > 1)
401  {
402  connection_cost = base::Cost(fwd_cost.value() - path_fwd[1]->getCost().value());
403  path_fwd.erase(path_fwd.begin());
404  }
405  else
406  {
407  OMPL_ERROR("Solution path traced incorrectly or otherwise constructed improperly \
408  through forward/reverse trees (both paths are one node in length, each).");
409  }
410 
411  // Adjust costs/parents in reverse tree nodes as cost/direction from forward tree root
412  useFwdTree();
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)
416  {
417  path_rev[i]->setCost(
418  base::Cost(fwd_cost.value() + (rev_cost.value() - path_rev[i]->getCost().value())));
419  path_rev[i]->setParent(path_rev[i - 1]);
420  }
421 
422  BiDirMotionPtrs mpath;
423  std::reverse(path_rev.begin(), path_rev.end());
424  mpath.reserve(path_fwd.size() + path_rev.size()); // preallocate memory
425  mpath.insert(mpath.end(), path_rev.begin(), path_rev.end());
426  mpath.insert(mpath.end(), path_fwd.begin(), path_fwd.end());
427 
428  // Set the solution path
429  auto path(std::make_shared<PathGeometric>(si_));
430  for (int i = mpath.size() - 1; i >= 0; --i)
431  {
432  path->append(mpath[i]->getState());
433  }
434 
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());
438 
439  OMPL_DEBUG("Total path cost: %f\n", fwd_cost.value() + rev_cost.value());
440  return base::PlannerStatus(true, false);
441  }
442 
443  // Planner terminated without accomplishing goal
444  return {false, false};
445  }
446 
447  void BFMT::expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
448  {
449  // Define Opennew and set it to NULL
450  BiDirMotionPtrs Open_new;
451 
452  // Define Znear as all unexplored nodes in the neighborhood around z
453  BiDirMotionPtrs zNear;
454  const BiDirMotionPtrs &zNeighborhood = neighborhoods_[z];
455 
456  for (auto i : zNeighborhood)
457  {
458  if (i->getCurrentSet() == BiDirMotion::SET_UNVISITED)
459  {
460  zNear.push_back(i);
461  }
462  }
463 
464  // For each node x in Znear
465  for (auto x : zNear)
466  {
467  if (!precomputeNN_)
468  saveNeighborhood(x); // nearest neighbors
469 
470  // Define Xnear as all frontier nodes in the neighborhood around the unexplored node x
471  BiDirMotionPtrs xNear;
472  const BiDirMotionPtrs &xNeighborhood = neighborhoods_[x];
473  for (auto j : xNeighborhood)
474  {
475  if (j->getCurrentSet() == BiDirMotion::SET_OPEN)
476  {
477  xNear.push_back(j);
478  }
479  }
480  // Find the node in Xnear with minimum cost-to-come in the current tree
481  BiDirMotion *xMin = nullptr;
482  double cMin = std::numeric_limits<double>::infinity();
483  for (auto &j : xNear)
484  {
485  // check if node costs are smaller than minimum
486  double cNew = j->getCost().value() + distanceFunction(j, x);
487 
488  if (cNew < cMin)
489  {
490  xMin = j;
491  cMin = cNew;
492  }
493  }
494 
495  // xMin was found
496  if (xMin != nullptr)
497  {
498  bool collision_free = false;
499  if (cacheCC_)
500  {
501  if (!xMin->alreadyCC(x))
502  {
503  collision_free = si_->checkMotion(xMin->getState(), x->getState());
505  // Due to FMT3* design, it is only necessary to save unsuccesful
506  // connection attemps because of collision
507  if (!collision_free)
508  xMin->addCC(x);
509  }
510  }
511  else
512  {
514  collision_free = si_->checkMotion(xMin->getState(), x->getState());
515  }
516 
517  if (collision_free)
518  { // motion between yMin and x is obstacle free
519  // add edge from xMin to x
520  x->setParent(xMin);
521  x->setCost(base::Cost(cMin));
522  xMin->getChildren().push_back(x);
523 
524  if (heuristics_)
525  x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), heurGoalState_[tree_]));
526 
527  // check if new node x is in the other tree; if so, save result
528  if (x->getOtherSet() != BiDirMotion::SET_UNVISITED)
529  {
530  if (connection_point == nullptr)
531  {
532  connection_point = x;
533  if (termination_ == FEASIBILITY)
534  {
535  break;
536  }
537  }
538  else
539  {
540  if ((connection_point->cost_[FWD].value() + connection_point->cost_[REV].value()) >
541  (x->cost_[FWD].value() + x->cost_[REV].value()))
542  {
543  connection_point = x;
544  }
545  }
546  }
547 
548  Open_new.push_back(x); // add x to Open_new
549  x->setCurrentSet(BiDirMotion::SET_CLOSED); // remove x from Unvisited
550  }
551  }
552  } // End "for x in Znear"
553 
554  // Remove motion z from binary heap and map
555  BiDirMotionBinHeap::Element *zElement = Open_elements[tree_][z];
556  Open_[tree_].remove(zElement);
557  Open_elements[tree_].erase(z);
558  z->setCurrentSet(BiDirMotion::SET_CLOSED);
559 
560  // add nodes in Open_new to Open
561  for (auto &i : Open_new)
562  {
563  if(Open_elements[tree_][i] == nullptr)
564  {
565  Open_elements[tree_][i] = Open_[tree_].insert(i);
566  i->setCurrentSet(BiDirMotion::SET_OPEN);
567  }
568  }
569  }
570 
571  bool BFMT::plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point,
573  {
574  // If pre-computation, find neighborhoods for all N sample nodes plus initial
575  // and goal state(s). Otherwise compute the neighborhoods of the initial and
576  // goal states separately and compute the others as needed.
577  BiDirMotionPtrs sampleNodes;
578  nn_->list(sampleNodes);
581  if (precomputeNN_)
582  {
583  for (auto &sampleNode : sampleNodes)
584  {
585  saveNeighborhood(sampleNode); // nearest neighbors
586  }
587  }
588  else
589  {
590  saveNeighborhood(x_init); // nearest neighbors
591  saveNeighborhood(x_goal); // nearest neighbors
592  }
593 
594  // Copy nodes in the sample set to Unvisitedfwd. Overwrite the label of the initial
595  // node with set Open for the forward tree, since it starts in set Openfwd.
596  useFwdTree();
597  for (auto &sampleNode : sampleNodes)
598  {
599  sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
600  }
601  x_init->setCurrentSet(BiDirMotion::SET_OPEN);
602 
603  // Copy nodes in the sample set to Unvisitedrev. Overwrite the label of the goal
604  // node with set Open for the reverse tree, since it starts in set Openrev.
605  useRevTree();
606  for (auto &sampleNode : sampleNodes)
607  {
608  sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
609  }
610  x_goal->setCurrentSet(BiDirMotion::SET_OPEN);
611 
612  // Expand the trees until reaching the termination condition
613  bool earlyFailure = false;
614  bool success = false;
615 
616  useFwdTree();
617  BiDirMotion *z = x_init;
618 
619  while (!success)
620  {
621  expandTreeFromNode(z, connection_point);
622 
623  // Check if the algorithm should terminate. Possibly redefines connection_point.
624  if (termination(z, connection_point, ptc))
625  success = true;
626  else
627  {
628  if (Open_[tree_].empty()) // If this heap is empty...
629  {
630  if (!extendedFMT_) // ... eFMT not enabled...
631  {
632  if (Open_[(tree_ + 1) % 2].empty()) // ... and this one, failure.
633  {
634  OMPL_INFORM("Both Open are empty before path was found --> no feasible path exists");
635  earlyFailure = true;
636  return earlyFailure;
637  }
638  }
639  else // However, if eFMT is enabled, run it.
640  insertNewSampleInOpen(ptc);
641  }
642 
643  // This function will be always reached with at least one state in one heap.
644  // However, if ptc terminates, we should skip this.
645  if (!ptc)
646  chooseTreeAndExpansionNode(z);
647  else
648  return true;
649  }
650  }
651  earlyFailure = false;
652  return earlyFailure;
653  }
654 
656  {
657  // Sample and connect samples to tree only if there is
658  // a possibility to connect to unvisited nodes.
659  std::vector<BiDirMotion *> nbh;
660  std::vector<base::Cost> costs;
661  std::vector<base::Cost> incCosts;
662  std::vector<std::size_t> sortedCostIndices;
663 
664  // our functor for sorting nearest neighbors
665  CostIndexCompare compareFn(costs, *opt_);
666 
667  auto *m = new BiDirMotion(si_, &tree_);
668  while (!ptc && Open_[tree_].empty()) //&& oneSample)
669  {
670  // Get new sample and check whether it is valid.
671  sampler_->sampleUniform(m->getState());
672  if (!si_->isValid(m->getState()))
673  continue;
674 
675  // Get neighbours of the new sample.
676  std::vector<BiDirMotion *> yNear;
677  if (nearestK_)
678  nn_->nearestK(m, NNk_, nbh);
679  else
680  nn_->nearestR(m, NNr_, nbh);
681 
682  yNear.reserve(nbh.size());
683  for (auto &j : nbh)
684  {
685  if (j->getCurrentSet() == BiDirMotion::SET_CLOSED)
686  {
687  if (nearestK_)
688  {
689  // Only include neighbors that are mutually k-nearest
690  // Relies on NN datastructure returning k-nearest in sorted order
691  const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
692  const base::Cost worstCost =
693  opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
694 
695  if (opt_->isCostBetterThan(worstCost, connCost))
696  continue;
697  yNear.push_back(j);
698  }
699  else
700  yNear.push_back(j);
701  }
702  }
703 
704  // Sample again if the new sample does not connect to the tree.
705  if (yNear.empty())
706  continue;
707 
708  // cache for distance computations
709  //
710  // Our cost caches only increase in size, so they're only
711  // resized if they can't fit the current neighborhood
712  if (costs.size() < yNear.size())
713  {
714  costs.resize(yNear.size());
715  incCosts.resize(yNear.size());
716  sortedCostIndices.resize(yNear.size());
717  }
718 
719  // Finding the nearest neighbor to connect to
720  // By default, neighborhood states are sorted by cost, and collision checking
721  // is performed in increasing order of cost
722  //
723  // calculate all costs and distances
724  for (std::size_t i = 0; i < yNear.size(); ++i)
725  {
726  incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
727  costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
728  }
729 
730  // sort the nodes
731  //
732  // we're using index-value pairs so that we can get at
733  // original, unsorted indices
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);
737 
738  // collision check until a valid motion is found
739  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
740  i != sortedCostIndices.begin() + yNear.size(); ++i)
741  {
743  if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
744  {
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));
749  m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), heurGoalState_[tree_]));
750  m->setCurrentSet(BiDirMotion::SET_OPEN);
751  Open_elements[tree_][m] = Open_[tree_].insert(m);
752 
753  nn_->add(m);
754  saveNeighborhood(m);
755  updateNeighborhood(m, nbh);
756 
757  break;
758  }
759  }
760  } // While Open_[tree_] empty
761  }
762 
763  bool BFMT::termination(BiDirMotion *&z, BiDirMotion *&connection_point,
765  {
766  bool terminate = false;
767  switch (termination_)
768  {
769  case FEASIBILITY:
770  // Test if a connection point was found during tree expansion
771  return (connection_point != nullptr || ptc);
772  break;
773 
774  case OPTIMALITY:
775  // Test if z is in SET_CLOSED (interior) of other tree
776  if (ptc)
777  terminate = true;
778  else if (z->getOtherSet() == BiDirMotion::SET_CLOSED)
779  terminate = true;
780 
781  break;
782  };
783  return terminate;
784  }
785 
786  // Choose exploration tree and node z to expand
788  {
789  switch (exploration_)
790  {
791  case SWAP_EVERY_TIME:
792  if (Open_[(tree_ + 1) % 2].empty())
793  z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
794  // condition in plan())
795  else
796  {
797  z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
798  swapTrees(); // Swap to the opposite tree
799  }
800  break;
801 
802  case CHOOSE_SMALLEST_Z:
803  BiDirMotion *z1, *z2;
804  if (Open_[(tree_ + 1) % 2].empty())
805  z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
806  // condition in plan())
807  else if (Open_[tree_].empty())
808  {
809  z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
810  swapTrees(); // Swap to the opposite tree
811  }
812  else
813  {
814  z1 = Open_[tree_].top()->data;
815  z2 = Open_[(tree_ + 1) % 2].top()->data;
816 
817  if (z1->getCost().value() < z2->getOtherCost().value())
818  z = z1;
819  else
820  {
821  z = z2;
822  swapTrees();
823  }
824  }
825  break;
826  };
827  }
828 
829  // Trace a path of nodes along a tree towards the root (forward or reverse)
830  void BFMT::tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
831  {
832  BiDirMotion *solution = z;
833 
834  while (solution != nullptr)
835  {
836  path.push_back(solution);
837  solution = solution->getParent();
838  }
839  }
840 
842  {
843  tree_ = (TreeType)((((int)tree_) + 1) % 2);
844  }
845 
846  void BFMT::updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh)
847  {
848  // Neighborhoods are only updated if the new motion is within bounds (k nearest or within r).
849  for (auto i : nbh)
850  {
851  // If CLOSED, that neighborhood won't be used again.
852  // Else, if neighhboorhod already exists, we have to insert the node in
853  // the corresponding place of the neighborhood of the neighbor of m.
854  if (i->getCurrentSet() == BiDirMotion::SET_CLOSED)
855  continue;
856 
857  auto it = neighborhoods_.find(i);
858  if (it != neighborhoods_.end())
859  {
860  if (it->second.empty())
861  continue;
862 
863  const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
864  const base::Cost worstCost = opt_->motionCost(it->second.back()->getState(), i->getState());
865 
866  if (opt_->isCostBetterThan(worstCost, connCost))
867  continue;
868 
869  // insert the neighbor in the vector in the correct order
870  std::vector<BiDirMotion *> &nbhToUpdate = it->second;
871  for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
872  {
873  // If connection to the new state is better than the current neighbor tested, insert.
874  const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
875  if (opt_->isCostBetterThan(connCost, cost))
876  {
877  nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
878  break;
879  }
880  }
881  }
882  }
883  }
884  } // End "geometric" namespace
885 } // End "ompl" namespace
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: FMT.h:576
MotionBinHeap Open_
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: FMT.h:526
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: FMT.h:536
unsigned int NNk_
K used in the nearestK strategy.
Definition: FMT.h:551
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: FMT.h:570
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:120
void saveNeighborhood(BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:190
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: BFMT.h:315
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
base::Cost cost_[2]
The cost of this motion
Definition: BFMT.h:429
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:298
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: FMT.h:545
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:184
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:474
unsigned int numSamples_
The number of samples to use when planning.
Definition: FMT.h:533
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:260
Definition of an abstract state.
Definition: State.h:113
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: BFMT.h:237
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:530
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:477
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:309
double value() const
The value of the cost.
Definition: Cost.h:152
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...
Definition: Console.cpp:120
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:303
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:529
void clear()
Clear the heap.
Definition: BinaryHeap.h:176
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: FMT.h:585
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:655
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:567
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:225
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:228
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for BFMT...
Definition: BFMT.h:251
bool getCacheCC() const
Get the state of the collision check caching.
Definition: BFMT.h:290
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
@ INVALID_GOAL
Invalid goal state.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
bool cacheCC_
Flag to activate the collision check caching.
Definition: FMT.h:542
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
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:462
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
A class to store the exit status of Planner::solve()
TreeType
Tree identifier.
Definition: BFMT.h:182
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:231
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:265
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:296
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:438
Abstract representation of a container that can perform nearest neighbors queries.
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: FMT.h:539
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found....
Definition: BFMT.h:340
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:332
base::StateSamplerPtr sampler_
State sampler.
Definition: FMT.h:573
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:497
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
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 ...
Definition: BFMT.cpp:846
bool getTermination() const
Returns the termination strategy.
Definition: BFMT.h:350
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: BFMT.cpp:121
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition: FMT.h:555
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
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:480
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:462
void swapTrees()
Change the active tree.
Definition: BFMT.cpp:841
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:204
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...
Definition: BFMT.cpp:447
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: BFMT.cpp:252
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:196
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: BFMT.cpp:271
SetType currentSet_[2]
Current set in which the motion is included.
Definition: BFMT.h:423
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:129
double NNr_
Radius employed in the nearestR strategy.
Definition: FMT.h:548
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...
Definition: BFMT.cpp:215
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Definition: BFMT.h:523
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:480
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:763
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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.
Definition: BFMT.cpp:571
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
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:516
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:787
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:92
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
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...
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: BFMT.h:219
Abstract definition of a goal region that can be sampled.
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: BFMT.h:284
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:830
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:492
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:444
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
@ INVALID_START
Invalid start state or no start state specified.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:322
Main namespace. Contains everything in this library.
Representation of a bidirectional motion.
Definition: BFMT.h:369