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 base::PlannerStatus(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(nn_, 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  Open_elements[tree_][i] = Open_[tree_].insert(i);
564  i->setCurrentSet(BiDirMotion::SET_OPEN);
565  }
566  }
567 
568  bool BFMT::plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point,
570  {
571  // If pre-computation, find neighborhoods for all N sample nodes plus initial
572  // and goal state(s). Otherwise compute the neighborhoods of the initial and
573  // goal states separately and compute the others as needed.
574  BiDirMotionPtrs sampleNodes;
575  nn_->list(sampleNodes);
578  if (precomputeNN_)
579  {
580  for (auto &sampleNode : sampleNodes)
581  {
582  saveNeighborhood(nn_, sampleNode); // nearest neighbors
583  }
584  }
585  else
586  {
587  saveNeighborhood(nn_, x_init); // nearest neighbors
588  saveNeighborhood(nn_, x_goal); // nearest neighbors
589  }
590 
591  // Copy nodes in the sample set to Unvisitedfwd. Overwrite the label of the initial
592  // node with set Open for the forward tree, since it starts in set Openfwd.
593  useFwdTree();
594  for (auto &sampleNode : sampleNodes)
595  {
596  sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
597  }
598  x_init->setCurrentSet(BiDirMotion::SET_OPEN);
599 
600  // Copy nodes in the sample set to Unvisitedrev. Overwrite the label of the goal
601  // node with set Open for the reverse tree, since it starts in set Openrev.
602  useRevTree();
603  for (auto &sampleNode : sampleNodes)
604  {
605  sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
606  }
607  x_goal->setCurrentSet(BiDirMotion::SET_OPEN);
608 
609  // Expand the trees until reaching the termination condition
610  bool earlyFailure = false;
611  bool success = false;
612 
613  useFwdTree();
614  BiDirMotion *z = x_init;
615 
616  while (!success)
617  {
618  expandTreeFromNode(z, connection_point);
619 
620  // Check if the algorithm should terminate. Possibly redefines connection_point.
621  if (termination(z, connection_point, ptc))
622  success = true;
623  else
624  {
625  if (Open_[tree_].empty()) // If this heap is empty...
626  {
627  if (!extendedFMT_) // ... eFMT not enabled...
628  {
629  if (Open_[(tree_ + 1) % 2].empty()) // ... and this one, failure.
630  {
631  OMPL_INFORM("Both Open are empty before path was found --> no feasible path exists");
632  earlyFailure = true;
633  return earlyFailure;
634  }
635  }
636  else // However, if eFMT is enabled, run it.
637  insertNewSampleInOpen(ptc);
638  }
639 
640  // This function will be always reached with at least one state in one heap.
641  // However, if ptc terminates, we should skip this.
642  if (!ptc)
643  chooseTreeAndExpansionNode(z);
644  else
645  return true;
646  }
647  }
648  earlyFailure = false;
649  return earlyFailure;
650  }
651 
653  {
654  // Sample and connect samples to tree only if there is
655  // a possibility to connect to unvisited nodes.
656  std::vector<BiDirMotion *> nbh;
657  std::vector<base::Cost> costs;
658  std::vector<base::Cost> incCosts;
659  std::vector<std::size_t> sortedCostIndices;
660 
661  // our functor for sorting nearest neighbors
662  CostIndexCompare compareFn(costs, *opt_);
663 
664  auto *m = new BiDirMotion(si_, &tree_);
665  while (!ptc && Open_[tree_].empty()) //&& oneSample)
666  {
667  // Get new sample and check whether it is valid.
668  sampler_->sampleUniform(m->getState());
669  if (!si_->isValid(m->getState()))
670  continue;
671 
672  // Get neighbours of the new sample.
673  std::vector<BiDirMotion *> yNear;
674  if (nearestK_)
675  nn_->nearestK(m, NNk_, nbh);
676  else
677  nn_->nearestR(m, NNr_, nbh);
678 
679  yNear.reserve(nbh.size());
680  for (auto &j : nbh)
681  {
682  if (j->getCurrentSet() == BiDirMotion::SET_CLOSED)
683  {
684  if (nearestK_)
685  {
686  // Only include neighbors that are mutually k-nearest
687  // Relies on NN datastructure returning k-nearest in sorted order
688  const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
689  const base::Cost worstCost =
690  opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
691 
692  if (opt_->isCostBetterThan(worstCost, connCost))
693  continue;
694  yNear.push_back(j);
695  }
696  else
697  yNear.push_back(j);
698  }
699  }
700 
701  // Sample again if the new sample does not connect to the tree.
702  if (yNear.empty())
703  continue;
704 
705  // cache for distance computations
706  //
707  // Our cost caches only increase in size, so they're only
708  // resized if they can't fit the current neighborhood
709  if (costs.size() < yNear.size())
710  {
711  costs.resize(yNear.size());
712  incCosts.resize(yNear.size());
713  sortedCostIndices.resize(yNear.size());
714  }
715 
716  // Finding the nearest neighbor to connect to
717  // By default, neighborhood states are sorted by cost, and collision checking
718  // is performed in increasing order of cost
719  //
720  // calculate all costs and distances
721  for (std::size_t i = 0; i < yNear.size(); ++i)
722  {
723  incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
724  costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
725  }
726 
727  // sort the nodes
728  //
729  // we're using index-value pairs so that we can get at
730  // original, unsorted indices
731  for (std::size_t i = 0; i < yNear.size(); ++i)
732  sortedCostIndices[i] = i;
733  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
734 
735  // collision check until a valid motion is found
736  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
737  i != sortedCostIndices.begin() + yNear.size(); ++i)
738  {
740  if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
741  {
742  const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
743  m->setParent(yNear[*i]);
744  yNear[*i]->getChildren().push_back(m);
745  m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
746  m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), heurGoalState_[tree_]));
747  m->setCurrentSet(BiDirMotion::SET_OPEN);
748  Open_elements[tree_][m] = Open_[tree_].insert(m);
749 
750  nn_->add(m);
751  saveNeighborhood(nn_, m);
752  updateNeighborhood(m, nbh);
753 
754  break;
755  }
756  }
757  } // While Open_[tree_] empty
758  }
759 
760  bool BFMT::termination(BiDirMotion *&z, BiDirMotion *&connection_point,
762  {
763  bool terminate = false;
764  switch (termination_)
765  {
766  case FEASIBILITY:
767  // Test if a connection point was found during tree expansion
768  return (connection_point != nullptr || ptc);
769  break;
770 
771  case OPTIMALITY:
772  // Test if z is in SET_CLOSED (interior) of other tree
773  if (ptc)
774  terminate = true;
775  else if (z->getOtherSet() == BiDirMotion::SET_CLOSED)
776  terminate = true;
777 
778  break;
779  };
780  return terminate;
781  }
782 
783  // Choose exploration tree and node z to expand
785  {
786  switch (exploration_)
787  {
788  case SWAP_EVERY_TIME:
789  if (Open_[(tree_ + 1) % 2].empty())
790  z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
791  // condition in plan())
792  else
793  {
794  z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
795  swapTrees(); // Swap to the opposite tree
796  }
797  break;
798 
799  case CHOOSE_SMALLEST_Z:
800  BiDirMotion *z1, *z2;
801  if (Open_[(tree_ + 1) % 2].empty())
802  z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
803  // condition in plan())
804  else if (Open_[tree_].empty())
805  {
806  z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
807  swapTrees(); // Swap to the opposite tree
808  }
809  else
810  {
811  z1 = Open_[tree_].top()->data;
812  z2 = Open_[(tree_ + 1) % 2].top()->data;
813 
814  if (z1->getCost().value() < z2->getOtherCost().value())
815  z = z1;
816  else
817  {
818  z = z2;
819  swapTrees();
820  }
821  }
822  break;
823  };
824  }
825 
826  // Trace a path of nodes along a tree towards the root (forward or reverse)
827  void BFMT::tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
828  {
829  BiDirMotion *solution = z;
830 
831  while (solution != nullptr)
832  {
833  path.push_back(solution);
834  solution = solution->getParent();
835  }
836  }
837 
839  {
840  tree_ = (TreeType)((((int)tree_) + 1) % 2);
841  }
842 
843  void BFMT::updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh)
844  {
845  // Neighborhoods are only updated if the new motion is within bounds (k nearest or within r).
846  for (auto i : nbh)
847  {
848  // If CLOSED, that neighborhood won't be used again.
849  // Else, if neighhboorhod already exists, we have to insert the node in
850  // the corresponding place of the neighborhood of the neighbor of m.
851  if (i->getCurrentSet() == BiDirMotion::SET_CLOSED)
852  continue;
853 
854  auto it = neighborhoods_.find(i);
855  if (it != neighborhoods_.end())
856  {
857  if (it->second.empty())
858  continue;
859 
860  const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
861  const base::Cost worstCost = opt_->motionCost(it->second.back()->getState(), i->getState());
862 
863  if (opt_->isCostBetterThan(worstCost, connCost))
864  continue;
865 
866  // insert the neighbor in the vector in the correct order
867  std::vector<BiDirMotion *> &nbhToUpdate = it->second;
868  for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
869  {
870  // If connection to the new state is better than the current neighbor tested, insert.
871  const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
872  if (opt_->isCostBetterThan(connCost, cost))
873  {
874  nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
875  break;
876  }
877  }
878  }
879  }
880  }
881  } // End "geometric" namespace
882 } // End "ompl" namespace
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: FMT.h:443
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:378
bool cacheCC_
Flag to activate the collision check caching.
Definition: FMT.h:446
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
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.
Definition: FMT.h:366
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:652
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
unsigned int numSamples_
The number of samples to use when planning.
Definition: FMT.h:437
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:384
virtual Cost initialCost(const State *s) const
Returns a cost value corresponding to starting at a state s. No optimal planners currently support th...
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
void setTermination(bool optimality)
Sets the termination strategy: optimality true finishes when the best possible path is found...
Definition: BFMT.h:244
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
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
base::StateSamplerPtr sampler_
State sampler.
Definition: FMT.h:477
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
double NNr_
Radius employed in the nearestR strategy.
Definition: FMT.h:452
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: FMT.h:489
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:132
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
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
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: BFMT.h:219
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
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
double freeSpaceVolume_
The volume of the free configuration space, computed as an upper bound with 95% confidence.
Definition: FMT.h:459
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.
Definition: FMT.h:474
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: BFMT.h:188
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: BFMT.h:135
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: FMT.h:440
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:760
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:120
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
base::Cost cost_[2]
The cost of this motion.
Definition: BFMT.h:333
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
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:843
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:420
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: BFMT.h:213
SetType currentSet_[2]
Current set in which the motion is included.
Definition: BFMT.h:327
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:92
virtual Cost motionCostHeuristic(const State *s1, const State *s2) const
Defines an admissible estimate on the optimal cost on the motion between states s1 and s2...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
bool getCacheCC() const
Get the state of the collision check caching.
Definition: BFMT.h:194
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:827
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision...
Definition: BFMT.h:427
Abstract definition of a goal region that can be sampled.
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:348
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
unsigned int NNk_
K used in the nearestK strategy.
Definition: FMT.h:455
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:119
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: BFMT.h:164
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:433
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: BFMT.h:129
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:396
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:236
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
bool getTermination() const
Returns the termination strategy.
Definition: BFMT.h:254
void swapTrees()
Change the active tree.
Definition: BFMT.cpp:838
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h: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 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
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:155
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 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
Definition of an abstract state.
Definition: State.h:49
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:101
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
Representation of a bidirectional motion.
Definition: BFMT.h:273
Abstract representation of a container that can perform nearest neighbors queries.
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:568
TreeType
Tree identifier.
Definition: BFMT.h:86
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void saveNeighborhood(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, BiDirMotion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: BFMT.cpp:190
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
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:784
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
virtual Cost terminalCost(const State *s) const
Returns a cost value corresponding to a path ending at a state s. No optimal planners currently suppo...
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
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
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:234
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
double value() const
The value of the cost.
Definition: Cost.h:56
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:342
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: FMT.h:471
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: BFMT.h:207
void setExploration(bool balanced)
Sets exploration strategy: balanced true expands one tree every iteration. False will select the tree...
Definition: BFMT.h:226
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: BFMT.h:200
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: BFMT.h:141
virtual Cost combineCosts(Cost c1, Cost c2) const
Get the cost that corresponds to combining the costs c1 and c2. Default implementation defines this c...
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:366
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: FMT.h:449
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
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:123
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68