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....
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
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
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....
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
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: FMT.h:480
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
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:420
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
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: Cost.h:42
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
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
bool getExploration() const
Returns the exploration strategy.
Definition: BFMT.h:236
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
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 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
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
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
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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
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
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
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