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());
562  ++collisionChecks_;
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  {
571  ++collisionChecks_;
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 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 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 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
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...
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
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
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 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
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
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
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
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
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 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
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
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
bool getCacheCC() const
Get the state of the collision check caching.
Definition: FMT.h:181
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
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 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
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
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:78
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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