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 {
56  // An upper bound on the free space volume is the total space volume; the free fraction is estimated in sampleFree
57  freeSpaceVolume_ = si_->getStateSpace()->getMeasure();
58  lastGoalMotion_ = nullptr;
59 
61  specs_.directed = false;
62 
63  ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &FMT::setNumSamples, &FMT::getNumSamples,
64  "10:10:1000000");
65  ompl::base::Planner::declareParam<double>("radius_multiplier", this, &FMT::setRadiusMultiplier,
66  &FMT::getRadiusMultiplier, "0.1:0.05:50.");
67  ompl::base::Planner::declareParam<bool>("nearest_k", this, &FMT::setNearestK, &FMT::getNearestK, "0,1");
68  ompl::base::Planner::declareParam<bool>("cache_cc", this, &FMT::setCacheCC, &FMT::getCacheCC, "0,1");
69  ompl::base::Planner::declareParam<bool>("heuristics", this, &FMT::setHeuristics, &FMT::getHeuristics, "0,1");
70  ompl::base::Planner::declareParam<bool>("extended_fmt", this, &FMT::setExtendedFMT, &FMT::getExtendedFMT, "0,1");
71 }
72 
73 ompl::geometric::FMT::~FMT()
74 {
75  freeMemory();
76 }
77 
79 {
80  if (pdef_)
81  {
82  /* Setup the optimization objective. If no optimization objective was
83  specified, then default to optimizing path length as computed by the
84  distance() function in the state space */
85  if (pdef_->hasOptimizationObjective())
86  opt_ = pdef_->getOptimizationObjective();
87  else
88  {
89  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
90  getName().c_str());
91  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
92  // Store the new objective in the problem def'n
93  pdef_->setOptimizationObjective(opt_);
94  }
95  Open_.getComparisonOperator().opt_ = opt_.get();
96  Open_.getComparisonOperator().heuristics_ = heuristics_;
97 
98  if (!nn_)
99  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
100  nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
101  {
102  return distanceFunction(a, b);
103  });
104 
105  if (nearestK_ && !nn_->reportsSortedResults())
106  {
107  OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
108  "disabled.",
109  getName().c_str());
110  nearestK_ = false;
111  }
112  }
113  else
114  {
115  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
116  setup_ = false;
117  }
118 }
119 
121 {
122  if (nn_)
123  {
124  std::vector<Motion *> motions;
125  motions.reserve(nn_->size());
126  nn_->list(motions);
127  for (auto &motion : motions)
128  {
129  si_->freeState(motion->getState());
130  delete motion;
131  }
132  }
133 }
134 
136 {
137  Planner::clear();
138  lastGoalMotion_ = nullptr;
139  sampler_.reset();
140  freeMemory();
141  if (nn_)
142  nn_->clear();
143  Open_.clear();
144  neighborhoods_.clear();
145 
146  collisionChecks_ = 0;
147 }
148 
150 {
151  Planner::getPlannerData(data);
152  std::vector<Motion *> motions;
153  nn_->list(motions);
154 
155  if (lastGoalMotion_ != nullptr)
156  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->getState()));
157 
158  unsigned int size = motions.size();
159  for (unsigned int i = 0; i < size; ++i)
160  {
161  if (motions[i]->getParent() == nullptr)
162  data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
163  else
164  data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
165  base::PlannerDataVertex(motions[i]->getState()));
166  }
167 }
168 
170 {
171  // Check to see if neighborhood has not been saved yet
172  if (neighborhoods_.find(m) == neighborhoods_.end())
173  {
174  std::vector<Motion *> nbh;
175  if (nearestK_)
176  nn_->nearestK(m, NNk_, nbh);
177  else
178  nn_->nearestR(m, NNr_, nbh);
179  if (!nbh.empty())
180  {
181  // Save the neighborhood but skip the first element, since it will be motion m
182  neighborhoods_[m] = std::vector<Motion *>(nbh.size() - 1, nullptr);
183  std::copy(nbh.begin() + 1, nbh.end(), neighborhoods_[m].begin());
184  }
185  else
186  {
187  // Save an empty neighborhood
188  neighborhoods_[m] = std::vector<Motion *>(0);
189  }
190  } // If neighborhood hadn't been saved yet
191 }
192 
193 // Calculate the unit ball volume for a given dimension
194 double ompl::geometric::FMT::calculateUnitBallVolume(const unsigned int dimension) const
195 {
196  if (dimension == 0)
197  return 1.0;
198  if (dimension == 1)
199  return 2.0;
200  return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
201 }
202 
203 double ompl::geometric::FMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
204 {
205  double a = 1.0 / (double)dimension;
206  double unitBallVolume = calculateUnitBallVolume(dimension);
207 
208  return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
209  std::pow(log((double)n) / (double)n, a);
210 }
211 
213 {
214  unsigned int nodeCount = 0;
215  unsigned int sampleAttempts = 0;
216  auto *motion = new Motion(si_);
217 
218  // Sample numSamples_ number of nodes from the free configuration space
219  while (nodeCount < numSamples_ && !ptc)
220  {
221  sampler_->sampleUniform(motion->getState());
222  sampleAttempts++;
223 
224  bool collision_free = si_->isValid(motion->getState());
225 
226  if (collision_free)
227  {
228  nodeCount++;
229  nn_->add(motion);
230  motion = new Motion(si_);
231  } // If collision free
232  } // While nodeCount < numSamples
233  si_->freeState(motion->getState());
234  delete motion;
235 
236  // 95% confidence limit for an upper bound for the true free space volume
237  freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
238  si_->getStateSpace()->getMeasure();
239 }
240 
242 {
243  // Ensure that there is at least one node near each goal
244  while (const base::State *goalState = pis_.nextGoal())
245  {
246  auto *gMotion = new Motion(si_);
247  si_->copyState(gMotion->getState(), goalState);
248 
249  std::vector<Motion *> nearGoal;
250  nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
251 
252  // If there is no node in the goal region, insert one
253  if (nearGoal.empty())
254  {
255  OMPL_DEBUG("No state inside goal region");
256  if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
257  {
258  nn_->add(gMotion);
259  goalState_ = gMotion->getState();
260  }
261  else
262  {
263  si_->freeState(gMotion->getState());
264  delete gMotion;
265  }
266  }
267  else // There is already a sample in the goal region
268  {
269  goalState_ = nearGoal[0]->getState();
270  si_->freeState(gMotion->getState());
271  delete gMotion;
272  }
273  } // For each goal
274 }
275 
277 {
278  if (lastGoalMotion_ != nullptr)
279  {
280  OMPL_INFORM("solve() called before clear(); returning previous solution");
281  traceSolutionPathThroughTree(lastGoalMotion_);
282  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
283  return base::PlannerStatus(true, false);
284  }
285  if (!Open_.empty())
286  {
287  OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
288  clear();
289  }
290 
291  checkValidity();
292  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
293  Motion *initMotion = nullptr;
294 
295  if (goal == nullptr)
296  {
297  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
299  }
300 
301  // Add start states to V (nn_) and Open
302  while (const base::State *st = pis_.nextStart())
303  {
304  initMotion = new Motion(si_);
305  si_->copyState(initMotion->getState(), st);
306  Open_.insert(initMotion);
307  initMotion->setSetType(Motion::SET_OPEN);
308  initMotion->setCost(opt_->initialCost(initMotion->getState()));
309  nn_->add(initMotion); // V <-- {x_init}
310  }
311 
312  if (initMotion == nullptr)
313  {
314  OMPL_ERROR("Start state undefined");
316  }
317 
318  // Sample N free states in the configuration space
319  if (!sampler_)
320  sampler_ = si_->allocStateSampler();
321  sampleFree(ptc);
322  assureGoalIsSampled(goal);
323  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
324 
325  // Calculate the nearest neighbor search radius
327  if (nearestK_)
328  {
329  NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
330  (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
331  log((double)nn_->size()));
332  OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
333  }
334  else
335  {
336  NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
337  OMPL_DEBUG("Using radius of %f", NNr_);
338  }
339 
340  // Execute the planner, and return early if the planner returns a failure
341  bool plannerSuccess = false;
342  bool successfulExpansion = false;
343  Motion *z = initMotion; // z <-- xinit
344  saveNeighborhood(z);
345 
346  while (!ptc)
347  {
348  if ((plannerSuccess = goal->isSatisfied(z->getState())))
349  break;
350 
351  successfulExpansion = expandTreeFromNode(&z);
352 
353  if (!extendedFMT_ && !successfulExpansion)
354  break;
355  if (extendedFMT_ && !successfulExpansion)
356  {
357  // Apply RRT*-like connections: sample and connect samples to tree
358  std::vector<Motion *> nbh;
359  std::vector<base::Cost> costs;
360  std::vector<base::Cost> incCosts;
361  std::vector<std::size_t> sortedCostIndices;
362 
363  // our functor for sorting nearest neighbors
364  CostIndexCompare compareFn(costs, *opt_);
365 
366  auto *m = new Motion(si_);
367  while (!ptc && Open_.empty())
368  {
369  sampler_->sampleUniform(m->getState());
370 
371  if (!si_->isValid(m->getState()))
372  continue;
373 
374  if (nearestK_)
375  nn_->nearestK(m, NNk_, nbh);
376  else
377  nn_->nearestR(m, NNr_, nbh);
378 
379  // Get neighbours in the tree.
380  std::vector<Motion *> yNear;
381  yNear.reserve(nbh.size());
382  for (auto &j : nbh)
383  {
384  if (j->getSetType() == Motion::SET_CLOSED)
385  {
386  if (nearestK_)
387  {
388  // Only include neighbors that are mutually k-nearest
389  // Relies on NN datastructure returning k-nearest in sorted order
390  const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
391  const base::Cost worstCost =
392  opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
393 
394  if (opt_->isCostBetterThan(worstCost, connCost))
395  continue;
396  yNear.push_back(j);
397  }
398  else
399  yNear.push_back(j);
400  }
401  }
402 
403  // Sample again if the new sample does not connect to the tree.
404  if (yNear.empty())
405  continue;
406 
407  // cache for distance computations
408  //
409  // Our cost caches only increase in size, so they're only
410  // resized if they can't fit the current neighborhood
411  if (costs.size() < yNear.size())
412  {
413  costs.resize(yNear.size());
414  incCosts.resize(yNear.size());
415  sortedCostIndices.resize(yNear.size());
416  }
417 
418  // Finding the nearest neighbor to connect to
419  // By default, neighborhood states are sorted by cost, and collision checking
420  // is performed in increasing order of cost
421  //
422  // calculate all costs and distances
423  for (std::size_t i = 0; i < yNear.size(); ++i)
424  {
425  incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
426  costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
427  }
428 
429  // sort the nodes
430  //
431  // we're using index-value pairs so that we can get at
432  // original, unsorted indices
433  for (std::size_t i = 0; i < yNear.size(); ++i)
434  sortedCostIndices[i] = i;
435  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
436 
437  // collision check until a valid motion is found
438  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
439  i != sortedCostIndices.begin() + yNear.size(); ++i)
440  {
441  if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
442  {
443  m->setParent(yNear[*i]);
444  yNear[*i]->getChildren().push_back(m);
445  const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
446  m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
447  m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
448  m->setSetType(Motion::SET_OPEN);
449 
450  nn_->add(m);
451  saveNeighborhood(m);
452  updateNeighborhood(m, nbh);
453 
454  Open_.insert(m);
455  z = m;
456  break;
457  }
458  }
459  } // while (!ptc && Open_.empty())
460  } // else if (extendedFMT_ && !successfulExpansion)
461  } // While not at goal
462 
463  if (plannerSuccess)
464  {
465  // Return the path to z, since by definition of planner success, z is in the goal region
466  lastGoalMotion_ = z;
467  traceSolutionPathThroughTree(lastGoalMotion_);
468 
469  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
470 
471  return base::PlannerStatus(true, false);
472  } // if plannerSuccess
473 
474  // Planner terminated without accomplishing goal
475  return base::PlannerStatus(false, false);
476 }
477 
479 {
480  std::vector<Motion *> mpath;
481  Motion *solution = goalMotion;
482 
483  // Construct the solution path
484  while (solution != nullptr)
485  {
486  mpath.push_back(solution);
487  solution = solution->getParent();
488  }
489 
490  // Set the solution path
491  auto path(std::make_shared<PathGeometric>(si_));
492  int mPathSize = mpath.size();
493  for (int i = mPathSize - 1; i >= 0; --i)
494  path->append(mpath[i]->getState());
495  pdef_->addSolutionPath(path, false, -1.0, getName());
496 }
497 
499 {
500  // Find all nodes that are near z, and also in set Unvisited
501 
502  std::vector<Motion *> xNear;
503  const std::vector<Motion *> &zNeighborhood = neighborhoods_[*z];
504  const unsigned int zNeighborhoodSize = zNeighborhood.size();
505  xNear.reserve(zNeighborhoodSize);
506 
507  for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
508  {
509  Motion *x = zNeighborhood[i];
510  if (x->getSetType() == Motion::SET_UNVISITED)
511  {
512  saveNeighborhood(x);
513  if (nearestK_)
514  {
515  // Only include neighbors that are mutually k-nearest
516  // Relies on NN datastructure returning k-nearest in sorted order
517  const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
518  const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
519 
520  if (opt_->isCostBetterThan(worstCost, connCost))
521  continue;
522  xNear.push_back(x);
523  }
524  else
525  xNear.push_back(x);
526  }
527  }
528 
529  // For each node near z and in set Unvisited, attempt to connect it to set Open
530  std::vector<Motion *> yNear;
531  std::vector<Motion *> Open_new;
532  const unsigned int xNearSize = xNear.size();
533  for (unsigned int i = 0; i < xNearSize; ++i)
534  {
535  Motion *x = xNear[i];
536 
537  // Find all nodes that are near x and in set Open
538  const std::vector<Motion *> &xNeighborhood = neighborhoods_[x];
539 
540  const unsigned int xNeighborhoodSize = xNeighborhood.size();
541  yNear.reserve(xNeighborhoodSize);
542  for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
543  {
544  if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
545  yNear.push_back(xNeighborhood[j]);
546  }
547 
548  // Find the lowest cost-to-come connection from Open to x
549  base::Cost cMin(std::numeric_limits<double>::infinity());
550  Motion *yMin = getBestParent(x, yNear, cMin);
551  yNear.clear();
552 
553  // If an optimal connection from Open to x was found
554  if (yMin != nullptr)
555  {
556  bool collision_free = false;
557  if (cacheCC_)
558  {
559  if (!yMin->alreadyCC(x))
560  {
561  collision_free = si_->checkMotion(yMin->getState(), x->getState());
563  // Due to FMT* design, it is only necessary to save unsuccesful
564  // connection attemps because of collision
565  if (!collision_free)
566  yMin->addCC(x);
567  }
568  }
569  else
570  {
572  collision_free = si_->checkMotion(yMin->getState(), x->getState());
573  }
574 
575  if (collision_free)
576  {
577  // Add edge from yMin to x
578  x->setParent(yMin);
579  x->setCost(cMin);
580  x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
581  yMin->getChildren().push_back(x);
582 
583  // Add x to Open
584  Open_new.push_back(x);
585  // Remove x from Unvisited
586  x->setSetType(Motion::SET_CLOSED);
587  }
588  } // An optimal connection from Open to x was found
589  } // For each node near z and in set Unvisited, try to connect it to set Open
590 
591  // Update Open
592  Open_.pop();
593  (*z)->setSetType(Motion::SET_CLOSED);
594 
595  // Add the nodes in Open_new to Open
596  unsigned int openNewSize = Open_new.size();
597  for (unsigned int i = 0; i < openNewSize; ++i)
598  {
599  Open_.insert(Open_new[i]);
600  Open_new[i]->setSetType(Motion::SET_OPEN);
601  }
602  Open_new.clear();
603 
604  if (Open_.empty())
605  {
606  if (!extendedFMT_)
607  OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
608  return false;
609  }
610 
611  // Take the top of Open as the new z
612  *z = Open_.top()->data;
613 
614  return true;
615 }
616 
618  base::Cost &cMin)
619 {
620  Motion *min = nullptr;
621  const unsigned int neighborsSize = neighbors.size();
622  for (unsigned int j = 0; j < neighborsSize; ++j)
623  {
624  const base::State *s = neighbors[j]->getState();
625  const base::Cost dist = opt_->motionCost(s, m->getState());
626  const base::Cost cNew = opt_->combineCosts(neighbors[j]->getCost(), dist);
627 
628  if (opt_->isCostBetterThan(cNew, cMin))
629  {
630  min = neighbors[j];
631  cMin = cNew;
632  }
633  }
634  return min;
635 }
636 
637 void ompl::geometric::FMT::updateNeighborhood(Motion *m, const std::vector<Motion *> nbh)
638 {
639  for (auto i : nbh)
640  {
641  // If CLOSED, the neighborhood already exists. If neighborhood already exists, we have
642  // to insert the node in the corresponding place of the neighborhood of the neighbor of m.
643  if (i->getSetType() == Motion::SET_CLOSED || neighborhoods_.find(i) != neighborhoods_.end())
644  {
645  const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
646  const base::Cost worstCost = opt_->motionCost(neighborhoods_[i].back()->getState(), i->getState());
647 
648  if (opt_->isCostBetterThan(worstCost, connCost))
649  continue;
650 
651  // Insert the neighbor in the vector in the correct order
652  std::vector<Motion *> &nbhToUpdate = neighborhoods_[i];
653  for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
654  {
655  // If connection to the new state is better than the current neighbor tested, insert.
656  const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
657  if (opt_->isCostBetterThan(connCost, cost))
658  {
659  nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
660  break;
661  }
662  }
663  }
664  else
665  {
666  std::vector<Motion *> nbh2;
667  if (nearestK_)
668  nn_->nearestK(m, NNk_, nbh2);
669  else
670  nn_->nearestR(m, NNr_, nbh2);
671 
672  if (!nbh2.empty())
673  {
674  // Save the neighborhood but skip the first element, since it will be motion m
675  neighborhoods_[i] = std::vector<Motion *>(nbh2.size() - 1, nullptr);
676  std::copy(nbh2.begin() + 1, nbh2.end(), neighborhoods_[i].begin());
677  }
678  else
679  {
680  // Save an empty neighborhood
681  neighborhoods_[i] = std::vector<Motion *>(0);
682  }
683  }
684  }
685 }
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:203
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:212
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:135
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BFMT.cpp:106
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:254
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition: BFMT.h:563
SetType getSetType() const
Get the set that this motion belongs to.
Definition: FMT.h:284
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:315
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
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:149
void addCC(Motion *m)
Caches a failed collision check to m.
Definition: FMT.h:297
void sampleFree(const std::shared_ptr< NearestNeighbors< BiDirMotion *>> &nn, const base::PlannerTerminationCondition &ptc)
Sample a state from the free configuration space and save it into the nearest neighbors data structur...
Definition: BFMT.cpp:215
base::StateSamplerPtr sampler_
State sampler.
Definition: BFMT.h:610
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
Definition: FMT.cpp:203
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:490
Representation of a motion.
Definition: FMT.h:214
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:120
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:409
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
unsigned int NNk_
K used in the nearestK strategy.
Definition: BFMT.h:579
bool expandTreeFromNode(Motion **z)
Complete one iteration of the main loop of the FMT* algorithm: Find K nearest nodes in set Unvisited ...
Definition: FMT.cpp:498
void updateNeighborhood(BiDirMotion *m, std::vector< BiDirMotion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: BFMT.cpp:843
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:478
void freeMemory()
Free the memory allocated by this planner.
Definition: BFMT.cpp:92
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition: BFMT.h:613
Abstract definition of a goal region that can be sampled.
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
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
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:617
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:278
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:594
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:248
void saveNeighborhood(Motion *m)
Save the neighbors within a neighborhood of a given state. The strategy used (nearestK or nearestR de...
Definition: FMT.cpp:169
void pop()
Remove the top element.
Definition: BinaryHeap.h:126
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
void expandTreeFromNode(BiDirMotion *&z, BiDirMotion *&connection_point)
Complete one iteration of the main loop of the BFMT* algorithm: Find K nearest nodes in set Unvisited...
Definition: BFMT.cpp:447
double calculateRadius(unsigned int dimension, unsigned int n) const
Calculate the radius to use for nearest neighbor searches, using the bound given in [L...
Definition: BFMT.cpp:252
void updateNeighborhood(Motion *m, std::vector< Motion *> nbh)
For a motion m, updates the stored neighborhoods of all its neighbors by by inserting m (maintaining ...
Definition: FMT.cpp:637
bool cacheCC_
Flag to activate the collision check caching.
Definition: BFMT.h:622
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:570
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:266
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
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:625
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:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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
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:190
unsigned int numSamples_
The number of samples to use when planning.
Definition: BFMT.h:552
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:241
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
void setHeuristicCost(const base::Cost h)
Set the cost to go heuristic cost.
Definition: FMT.h:303
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:276
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:234
double NNr_
Radius employed in the nearestR strategy.
Definition: BFMT.h:576
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence...
Definition: BFMT.h:567
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition: BFMT.h:573
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:78
bool heuristics_
Flag to activate the cost to go heuristics.
Definition: BFMT.h:616
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: BFMT.cpp:243
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
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:291
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:260
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68