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