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  {
24  specs_.approximateSolutions = false;
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  {
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.
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)
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);
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
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:132
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:234
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
Abstract representation of a container that can perform nearest neighbors queries.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Abstract definition of a goal region that can be sampled.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
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:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:423
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:420
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
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
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:440
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
Definition of an abstract state.
Definition: State.h:50
Representation of a bidirectional motion.
Definition: BFMT.h:274
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
base::State * getState() const
Get the state associated with the motion.
Definition: BFMT.h:420
base::Cost cost_[2]
The cost of this motion
Definition: BFMT.h:333
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition: BFMT.h:396
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition: BFMT.h:378
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition: BFMT.h:433
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition: BFMT.h:348
SetType currentSet_[2]
Current set in which the motion is included.
Definition: BFMT.h:327
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition: BFMT.h:384
base::Cost getCost() const
Set the state associated with the motion.
Definition: BFMT.h:342
BiDirMotion * getParent() const
Get the parent of the motion.
Definition: BFMT.h:366
TerminateType termination_
Termination strategy used.
Definition: BFMT.h:588
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:576
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:563
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition: BFMT.h:607
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BFMT.cpp:48
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition: BFMT.cpp:655
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
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition: BFMT.h:619
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition: BFMT.cpp:830
TreeType tree_
Active tree.
Definition: BFMT.h:582
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition: BFMT.cpp:763
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:570
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 freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:92
BiDirMotionBinHeap Open_[2]
A binary heap for storing explored motions in cost-to-come sorted order. The motions in Open have bee...
Definition: BFMT.h:604
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
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:594
double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition: BFMT.h:490
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
ExploreType exploration_
Exploration strategy used.
Definition: BFMT.h:585
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition: BFMT.cpp:261
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 heuristics_
Flag to activate the cost to go heuristics.
Definition: BFMT.h:616
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
base::StateSamplerPtr sampler_
State sampler.
Definition: BFMT.h:610
std::map< BiDirMotion *, BiDirMotionPtrs > neighborhoods_
A map linking a motion to all of the motions within a distance r of that motion.
Definition: BFMT.h:598
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:613
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition: BFMT.cpp:787
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
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:579
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
void useFwdTree()
Sets forward tree active.
Definition: BFMT.h:475
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
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:552
void useRevTree()
Sets reverse tree active.
Definition: BFMT.h:481
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition: BFMT.h:567
void swapTrees()
Change the active tree.
Definition: BFMT.cpp:841
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition: BFMT.h:591
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
TreeType
Tree identifier.
Definition: BFMT.h:87
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:622
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:573
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:625
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60