FMT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Autonomous Systems Laboratory, Stanford University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Stanford University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Authors: Ashley Clark (Stanford) and Wolfgang Pointner (AIT) */
36 /* Co-developers: Brice Rebsamen (Stanford), Tim Wheeler (Stanford)
37  Edward Schmerling (Stanford), and Javier V. Gómez (UC3M - Stanford)*/
38 /* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */
39 /* Acknowledgements for insightful comments: Oren Salzman (Tel Aviv University),
40  * Joseph Starek (Stanford) */
41 
42 #include <limits>
43 #include <iostream>
44 
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/math/distributions/binomial.hpp>
47 
48 #include <ompl/datastructures/BinaryHeap.h>
49 #include <ompl/tools/config/SelfConfig.h>
50 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
51 #include <ompl/geometric/planners/fmt/FMT.h>
52 
53 ompl::geometric::FMT::FMT(const base::SpaceInformationPtr &si)
54  : base::Planner(si, "FMT")
55  , numSamples_(1000)
56  , collisionChecks_(0)
57  , nearestK_(true)
58  , cacheCC_(true)
59  , heuristics_(false)
60  , radiusMultiplier_(1.1)
61  , extendedFMT_(true)
62 {
63  // An upper bound on the free space volume is the total space volume; the free fraction is estimated in sampleFree
64  freeSpaceVolume_ = si_->getStateSpace()->getMeasure();
65  lastGoalMotion_ = nullptr;
66 
68  specs_.directed = false;
69 
70  ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples,
71  "10:10:1000000");
72  ompl::base::Planner::declareParam<double>("radius_multiplier", this, &FMT::setRadiusMultiplier,
73  &FMT::getRadiusMultiplier, "0.1:0.05:50.");
74  ompl::base::Planner::declareParam<bool>("nearest_k", this, &FMT::setNearestK, &FMT::getNearestK, "0,1");
75  ompl::base::Planner::declareParam<bool>("cache_cc", this, &FMT::setCacheCC, &FMT::getCacheCC, "0,1");
76  ompl::base::Planner::declareParam<bool>("heuristics", this, &FMT::setHeuristics, &FMT::getHeuristics, "0,1");
77  ompl::base::Planner::declareParam<bool>("extended_fmt", this, &FMT::setExtendedFMT, &FMT::getExtendedFMT, "0,1");
78 }
79 
80 ompl::geometric::FMT::~FMT()
81 {
82  freeMemory();
83 }
84 
86 {
87  if (pdef_)
88  {
89  /* Setup the optimization objective. If no optimization objective was
90  specified, then default to optimizing path length as computed by the
91  distance() function in the state space */
92  if (pdef_->hasOptimizationObjective())
93  opt_ = pdef_->getOptimizationObjective();
94  else
95  {
96  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
97  getName().c_str());
98  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
99  // Store the new objective in the problem def'n
100  pdef_->setOptimizationObjective(opt_);
101  }
102  Open_.getComparisonOperator().opt_ = opt_.get();
103  Open_.getComparisonOperator().heuristics_ = heuristics_;
104 
105  if (!nn_)
106  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
107  nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
108  {
109  return distanceFunction(a, b);
110  });
111 
112  if (nearestK_ && !nn_->reportsSortedResults())
113  {
114  OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
115  "disabled.",
116  getName().c_str());
117  nearestK_ = false;
118  }
119  }
120  else
121  {
122  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
123  setup_ = false;
124  }
125 }
126 
128 {
129  if (nn_)
130  {
131  std::vector<Motion *> motions;
132  motions.reserve(nn_->size());
133  nn_->list(motions);
134  for (auto &motion : motions)
135  {
136  si_->freeState(motion->getState());
137  delete motion;
138  }
139  }
140 }
141 
143 {
144  Planner::clear();
145  lastGoalMotion_ = nullptr;
146  sampler_.reset();
147  freeMemory();
148  if (nn_)
149  nn_->clear();
150  Open_.clear();
151  neighborhoods_.clear();
152 
153  collisionChecks_ = 0;
154 }
155 
157 {
158  Planner::getPlannerData(data);
159  std::vector<Motion *> motions;
160  nn_->list(motions);
161 
162  if (lastGoalMotion_)
163  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->getState()));
164 
165  unsigned int size = motions.size();
166  for (unsigned int i = 0; i < size; ++i)
167  {
168  if (motions[i]->getParent() == nullptr)
169  data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
170  else
171  data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
172  base::PlannerDataVertex(motions[i]->getState()));
173  }
174 }
175 
177 {
178  // Check to see if neighborhood has not been saved yet
179  if (neighborhoods_.find(m) == neighborhoods_.end())
180  {
181  std::vector<Motion *> nbh;
182  if (nearestK_)
183  nn_->nearestK(m, NNk_, nbh);
184  else
185  nn_->nearestR(m, NNr_, nbh);
186  if (!nbh.empty())
187  {
188  // Save the neighborhood but skip the first element, since it will be motion m
189  neighborhoods_[m] = std::vector<Motion *>(nbh.size() - 1, nullptr);
190  std::copy(nbh.begin() + 1, nbh.end(), neighborhoods_[m].begin());
191  }
192  else
193  {
194  // Save an empty neighborhood
195  neighborhoods_[m] = std::vector<Motion *>(0);
196  }
197  } // If neighborhood hadn't been saved yet
198 }
199 
200 // Calculate the unit ball volume for a given dimension
201 double ompl::geometric::FMT::calculateUnitBallVolume(const unsigned int dimension) const
202 {
203  if (dimension == 0)
204  return 1.0;
205  else if (dimension == 1)
206  return 2.0;
207  return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
208 }
209 
210 double ompl::geometric::FMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
211 {
212  double a = 1.0 / (double)dimension;
213  double unitBallVolume = calculateUnitBallVolume(dimension);
214 
215  return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
216  std::pow(log((double)n) / (double)n, a);
217 }
218 
220 {
221  unsigned int nodeCount = 0;
222  unsigned int sampleAttempts = 0;
223  auto *motion = new Motion(si_);
224 
225  // Sample numSamples_ number of nodes from the free configuration space
226  while (nodeCount < numSamples_ && !ptc)
227  {
228  sampler_->sampleUniform(motion->getState());
229  sampleAttempts++;
230 
231  bool collision_free = si_->isValid(motion->getState());
232 
233  if (collision_free)
234  {
235  nodeCount++;
236  nn_->add(motion);
237  motion = new Motion(si_);
238  } // If collision free
239  } // While nodeCount < numSamples
240  si_->freeState(motion->getState());
241  delete motion;
242 
243  // 95% confidence limit for an upper bound for the true free space volume
244  freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
245  si_->getStateSpace()->getMeasure();
246 }
247 
249 {
250  // Ensure that there is at least one node near each goal
251  while (const base::State *goalState = pis_.nextGoal())
252  {
253  auto *gMotion = new Motion(si_);
254  si_->copyState(gMotion->getState(), goalState);
255 
256  std::vector<Motion *> nearGoal;
257  nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
258 
259  // If there is no node in the goal region, insert one
260  if (nearGoal.empty())
261  {
262  OMPL_DEBUG("No state inside goal region");
263  if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
264  {
265  nn_->add(gMotion);
266  goalState_ = gMotion->getState();
267  }
268  else
269  {
270  si_->freeState(gMotion->getState());
271  delete gMotion;
272  }
273  }
274  else // There is already a sample in the goal region
275  {
276  goalState_ = nearGoal[0]->getState();
277  si_->freeState(gMotion->getState());
278  delete gMotion;
279  }
280  } // For each goal
281 }
282 
284 {
285  if (lastGoalMotion_)
286  {
287  OMPL_INFORM("solve() called before clear(); returning previous solution");
288  traceSolutionPathThroughTree(lastGoalMotion_);
289  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
290  return base::PlannerStatus(true, false);
291  }
292  else if (Open_.size() > 0)
293  {
294  OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
295  clear();
296  }
297 
298  checkValidity();
299  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
300  Motion *initMotion = nullptr;
301 
302  if (!goal)
303  {
304  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
306  }
307 
308  // Add start states to V (nn_) and Open
309  while (const base::State *st = pis_.nextStart())
310  {
311  initMotion = new Motion(si_);
312  si_->copyState(initMotion->getState(), st);
313  Open_.insert(initMotion);
314  initMotion->setSetType(Motion::SET_OPEN);
315  initMotion->setCost(opt_->initialCost(initMotion->getState()));
316  nn_->add(initMotion); // V <-- {x_init}
317  }
318 
319  if (!initMotion)
320  {
321  OMPL_ERROR("Start state undefined");
323  }
324 
325  // Sample N free states in the configuration space
326  if (!sampler_)
327  sampler_ = si_->allocStateSampler();
328  sampleFree(ptc);
329  assureGoalIsSampled(goal);
330  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
331 
332  // Calculate the nearest neighbor search radius
334  if (nearestK_)
335  {
336  NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
337  (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
338  log((double)nn_->size()));
339  OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
340  }
341  else
342  {
343  NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
344  OMPL_DEBUG("Using radius of %f", NNr_);
345  }
346 
347  // Execute the planner, and return early if the planner returns a failure
348  bool plannerSuccess = false;
349  bool successfulExpansion = false;
350  Motion *z = initMotion; // z <-- xinit
351  saveNeighborhood(z);
352 
353  while (!ptc)
354  {
355  if ((plannerSuccess = goal->isSatisfied(z->getState())))
356  break;
357 
358  successfulExpansion = expandTreeFromNode(&z);
359 
360  if (!extendedFMT_ && !successfulExpansion)
361  break;
362  else if (extendedFMT_ && !successfulExpansion)
363  {
364  // Apply RRT*-like connections: sample and connect samples to tree
365  std::vector<Motion *> nbh;
366  std::vector<base::Cost> costs;
367  std::vector<base::Cost> incCosts;
368  std::vector<std::size_t> sortedCostIndices;
369 
370  // our functor for sorting nearest neighbors
371  CostIndexCompare compareFn(costs, *opt_);
372 
373  auto *m = new Motion(si_);
374  while (!ptc && Open_.empty())
375  {
376  sampler_->sampleUniform(m->getState());
377 
378  if (!si_->isValid(m->getState()))
379  continue;
380 
381  if (nearestK_)
382  nn_->nearestK(m, NNk_, nbh);
383  else
384  nn_->nearestR(m, NNr_, nbh);
385 
386  // Get neighbours in the tree.
387  std::vector<Motion *> yNear;
388  yNear.reserve(nbh.size());
389  for (auto &j : nbh)
390  {
391  if (j->getSetType() == Motion::SET_CLOSED)
392  {
393  if (nearestK_)
394  {
395  // Only include neighbors that are mutually k-nearest
396  // Relies on NN datastructure returning k-nearest in sorted order
397  const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
398  const base::Cost worstCost =
399  opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
400 
401  if (opt_->isCostBetterThan(worstCost, connCost))
402  continue;
403  else
404  yNear.push_back(j);
405  }
406  else
407  yNear.push_back(j);
408  }
409  }
410 
411  // Sample again if the new sample does not connect to the tree.
412  if (yNear.empty())
413  continue;
414 
415  // cache for distance computations
416  //
417  // Our cost caches only increase in size, so they're only
418  // resized if they can't fit the current neighborhood
419  if (costs.size() < yNear.size())
420  {
421  costs.resize(yNear.size());
422  incCosts.resize(yNear.size());
423  sortedCostIndices.resize(yNear.size());
424  }
425 
426  // Finding the nearest neighbor to connect to
427  // By default, neighborhood states are sorted by cost, and collision checking
428  // is performed in increasing order of cost
429  //
430  // calculate all costs and distances
431  for (std::size_t i = 0; i < yNear.size(); ++i)
432  {
433  incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
434  costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
435  }
436 
437  // sort the nodes
438  //
439  // we're using index-value pairs so that we can get at
440  // original, unsorted indices
441  for (std::size_t i = 0; i < yNear.size(); ++i)
442  sortedCostIndices[i] = i;
443  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
444 
445  // collision check until a valid motion is found
446  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
447  i != sortedCostIndices.begin() + yNear.size(); ++i)
448  {
449  if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
450  {
451  m->setParent(yNear[*i]);
452  yNear[*i]->getChildren().push_back(m);
453  const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
454  m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
455  m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
456  m->setSetType(Motion::SET_OPEN);
457 
458  nn_->add(m);
459  saveNeighborhood(m);
460  updateNeighborhood(m, nbh);
461 
462  Open_.insert(m);
463  z = m;
464  break;
465  }
466  }
467  } // while (!ptc && Open_.empty())
468  } // else if (extendedFMT_ && !successfulExpansion)
469  } // While not at goal
470 
471  if (plannerSuccess)
472  {
473  // Return the path to z, since by definition of planner success, z is in the goal region
474  lastGoalMotion_ = z;
475  traceSolutionPathThroughTree(lastGoalMotion_);
476 
477  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
478 
479  return base::PlannerStatus(true, false);
480  } // if plannerSuccess
481  else
482  {
483  // Planner terminated without accomplishing goal
484  return base::PlannerStatus(false, false);
485  }
486 }
487 
489 {
490  std::vector<Motion *> mpath;
491  Motion *solution = goalMotion;
492 
493  // Construct the solution path
494  while (solution != nullptr)
495  {
496  mpath.push_back(solution);
497  solution = solution->getParent();
498  }
499 
500  // Set the solution path
501  auto path(std::make_shared<PathGeometric>(si_));
502  int mPathSize = mpath.size();
503  for (int i = mPathSize - 1; i >= 0; --i)
504  path->append(mpath[i]->getState());
505  pdef_->addSolutionPath(path, false, -1.0, getName());
506 }
507 
509 {
510  // Find all nodes that are near z, and also in set Unvisited
511 
512  std::vector<Motion *> xNear;
513  const std::vector<Motion *> &zNeighborhood = neighborhoods_[*z];
514  const unsigned int zNeighborhoodSize = zNeighborhood.size();
515  xNear.reserve(zNeighborhoodSize);
516 
517  for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
518  {
519  Motion *x = zNeighborhood[i];
520  if (x->getSetType() == Motion::SET_UNVISITED)
521  {
522  saveNeighborhood(x);
523  if (nearestK_)
524  {
525  // Only include neighbors that are mutually k-nearest
526  // Relies on NN datastructure returning k-nearest in sorted order
527  const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
528  const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
529 
530  if (opt_->isCostBetterThan(worstCost, connCost))
531  continue;
532  else
533  xNear.push_back(x);
534  }
535  else
536  xNear.push_back(x);
537  }
538  }
539 
540  // For each node near z and in set Unvisited, attempt to connect it to set Open
541  std::vector<Motion *> yNear;
542  std::vector<Motion *> Open_new;
543  const unsigned int xNearSize = xNear.size();
544  for (unsigned int i = 0; i < xNearSize; ++i)
545  {
546  Motion *x = xNear[i];
547 
548  // Find all nodes that are near x and in set Open
549  const std::vector<Motion *> &xNeighborhood = neighborhoods_[x];
550 
551  const unsigned int xNeighborhoodSize = xNeighborhood.size();
552  yNear.reserve(xNeighborhoodSize);
553  for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
554  {
555  if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
556  yNear.push_back(xNeighborhood[j]);
557  }
558 
559  // Find the lowest cost-to-come connection from Open to x
560  base::Cost cMin(std::numeric_limits<double>::infinity());
561  Motion *yMin = getBestParent(x, yNear, cMin);
562  yNear.clear();
563 
564  // If an optimal connection from Open to x was found
565  if (yMin != nullptr)
566  {
567  bool collision_free = false;
568  if (cacheCC_)
569  {
570  if (!yMin->alreadyCC(x))
571  {
572  collision_free = si_->checkMotion(yMin->getState(), x->getState());
574  // Due to FMT* design, it is only necessary to save unsuccesful
575  // connection attemps because of collision
576  if (!collision_free)
577  yMin->addCC(x);
578  }
579  }
580  else
581  {
583  collision_free = si_->checkMotion(yMin->getState(), x->getState());
584  }
585 
586  if (collision_free)
587  {
588  // Add edge from yMin to x
589  x->setParent(yMin);
590  x->setCost(cMin);
591  x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
592  yMin->getChildren().push_back(x);
593 
594  // Add x to Open
595  Open_new.push_back(x);
596  // Remove x from Unvisited
597  x->setSetType(Motion::SET_CLOSED);
598  }
599  } // An optimal connection from Open to x was found
600  } // For each node near z and in set Unvisited, try to connect it to set Open
601 
602  // Update Open
603  Open_.pop();
604  (*z)->setSetType(Motion::SET_CLOSED);
605 
606  // Add the nodes in Open_new to Open
607  unsigned int openNewSize = Open_new.size();
608  for (unsigned int i = 0; i < openNewSize; ++i)
609  {
610  Open_.insert(Open_new[i]);
611  Open_new[i]->setSetType(Motion::SET_OPEN);
612  }
613  Open_new.clear();
614 
615  if (Open_.empty())
616  {
617  if (!extendedFMT_)
618  OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
619  return false;
620  }
621 
622  // Take the top of Open as the new z
623  *z = Open_.top()->data;
624 
625  return true;
626 }
627 
629  base::Cost &cMin)
630 {
631  Motion *min = nullptr;
632  const unsigned int neighborsSize = neighbors.size();
633  for (unsigned int j = 0; j < neighborsSize; ++j)
634  {
635  const base::State *s = neighbors[j]->getState();
636  const base::Cost dist = opt_->motionCost(s, m->getState());
637  const base::Cost cNew = opt_->combineCosts(neighbors[j]->getCost(), dist);
638 
639  if (opt_->isCostBetterThan(cNew, cMin))
640  {
641  min = neighbors[j];
642  cMin = cNew;
643  }
644  }
645  return min;
646 }
647 
648 void ompl::geometric::FMT::updateNeighborhood(Motion *m, const std::vector<Motion *> nbh)
649 {
650  for (auto i : nbh)
651  {
652  // If CLOSED, the neighborhood already exists. If neighborhood already exists, we have
653  // to insert the node in the corresponding place of the neighborhood of the neighbor of m.
654  if (i->getSetType() == Motion::SET_CLOSED || neighborhoods_.find(i) != neighborhoods_.end())
655  {
656  const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
657  const base::Cost worstCost = opt_->motionCost(neighborhoods_[i].back()->getState(), i->getState());
658 
659  if (opt_->isCostBetterThan(worstCost, connCost))
660  continue;
661  else
662  {
663  // Insert the neighbor in the vector in the correct order
664  std::vector<Motion *> &nbhToUpdate = neighborhoods_[i];
665  for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
666  {
667  // If connection to the new state is better than the current neighbor tested, insert.
668  const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
669  if (opt_->isCostBetterThan(connCost, cost))
670  {
671  nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
672  break;
673  }
674  }
675  }
676  }
677  else
678  {
679  std::vector<Motion *> nbh2;
680  if (nearestK_)
681  nn_->nearestK(m, NNk_, nbh2);
682  else
683  nn_->nearestR(m, NNr_, nbh2);
684 
685  if (!nbh2.empty())
686  {
687  // Save the neighborhood but skip the first element, since it will be motion m
688  neighborhoods_[i] = std::vector<Motion *>(nbh2.size() - 1, nullptr);
689  std::copy(nbh2.begin() + 1, nbh2.end(), neighborhoods_[i].begin());
690  }
691  else
692  {
693  // Save an empty neighborhood
694  neighborhoods_[i] = std::vector<Motion *>(0);
695  }
696  }
697  }
698 }
void setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: FMT.h:200
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:256
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: FMT.h:128
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: FMT.h:206
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: FMT.cpp:142
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:119
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 setParent(Motion *parent)
Set the parent motion of the current motion.
Definition: FMT.h:256
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:566
SetType getSetType() const
Get the set that this motion belongs to.
Definition: FMT.h:286
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
void setRadiusMultiplier(const double radiusMultiplier)
The planner searches for neighbors of a node within a cost r, where r is the value described for FMT*...
Definition: FMT.h:142
std::vector< Motion * > & getChildren()
Get the children of the motion.
Definition: FMT.h:319
void clear()
Clear the heap.
Definition: BinaryHeap.h:111
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: FMT.cpp:156
void addCC(Motion *m)
Caches a failed collision check to m.
Definition: FMT.h:301
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: BFMT.h:613
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
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
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double distanceFunction(const BiDirMotion *a, const BiDirMotion *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: BFMT.h:493
Representation of a motion.
Definition: FMT.h:214
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:127
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: FMT.h:110
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
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:582
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
void traceSolutionPathThroughTree(Motion *goalMotion)
Trace the path from a goal state back to the start state and save the result as a solution in the Pro...
Definition: FMT.cpp:488
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:105
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:194
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
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:616
Abstract definition of a goal region that can be sampled.
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:607
Motion * getBestParent(Motion *m, std::vector< Motion *> &neighbors, base::Cost &cMin)
Returns the best parent and the connection cost in the neighborhood of a motion m.
Definition: FMT.cpp:628
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:151
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: FMT.h:175
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:280
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:116
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition: BFMT.h:597
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:250
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
void pop()
Remove the top element.
Definition: BinaryHeap.h:125
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
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
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:625
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
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
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition: BFMT.h:573
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:268
#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
bool getCacheCC() const
Get the state of the collision check caching.
Definition: FMT.h:181
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition: BFMT.h:628
double getThreshold() const
Get the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:82
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
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:601
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: FMT.h:187
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: FMT.h:122
#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 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
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:555
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region. If not, add a goal state from that region directly into the set of vertices. In this way, FMT is able to find a solution, if one exists. If no sampled nodes are within a goal region, there would be no way for the algorithm to successfully find a path to that region.
Definition: FMT.cpp:248
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:119
double calculateUnitBallVolume(const unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:201
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: FMT.h:307
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: FMT.cpp:283
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition: BinaryHeap.h:233
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:579
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence...
Definition: BFMT.h:570
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 nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:576
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:85
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: BFMT.h:619
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:200
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
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: FMT.h:194
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision...
Definition: FMT.h:293
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:262
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68