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>("use_k_nearest", 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  if (!goal->couldSample())
302  {
303  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
305  }
306 
307  // Add start states to V (nn_) and Open
308  while (const base::State *st = pis_.nextStart())
309  {
310  initMotion = new Motion(si_);
311  si_->copyState(initMotion->getState(), st);
312  Open_.insert(initMotion);
313  initMotion->setSetType(Motion::SET_OPEN);
314  initMotion->setCost(opt_->initialCost(initMotion->getState()));
315  nn_->add(initMotion); // V <-- {x_init}
316  }
317 
318  if (initMotion == nullptr)
319  {
320  OMPL_ERROR("Start state undefined");
322  }
323 
324  // Sample N free states in the configuration space
325  if (!sampler_)
326  sampler_ = si_->allocStateSampler();
327  sampleFree(ptc);
328  assureGoalIsSampled(goal);
329  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
330 
331  // Calculate the nearest neighbor search radius
333  if (nearestK_)
334  {
335  NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
336  (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
337  log((double)nn_->size()));
338  OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
339  }
340  else
341  {
342  NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
343  OMPL_DEBUG("Using radius of %f", NNr_);
344  }
345 
346  // Execute the planner, and return early if the planner returns a failure
347  bool plannerSuccess = false;
348  bool successfulExpansion = false;
349  Motion *z = initMotion; // z <-- xinit
350  saveNeighborhood(z);
351 
352  while (!ptc)
353  {
354  if ((plannerSuccess = goal->isSatisfied(z->getState())))
355  break;
356 
357  successfulExpansion = expandTreeFromNode(&z);
358 
359  if (!extendedFMT_ && !successfulExpansion)
360  break;
361  if (extendedFMT_ && !successfulExpansion)
362  {
363  // Apply RRT*-like connections: sample and connect samples to tree
364  std::vector<Motion *> nbh;
365  std::vector<base::Cost> costs;
366  std::vector<base::Cost> incCosts;
367  std::vector<std::size_t> sortedCostIndices;
368 
369  // our functor for sorting nearest neighbors
370  CostIndexCompare compareFn(costs, *opt_);
371 
372  auto *m = new Motion(si_);
373  while (!ptc && Open_.empty())
374  {
375  sampler_->sampleUniform(m->getState());
376 
377  if (!si_->isValid(m->getState()))
378  continue;
379 
380  if (nearestK_)
381  nn_->nearestK(m, NNk_, nbh);
382  else
383  nn_->nearestR(m, NNr_, nbh);
384 
385  // Get neighbours in the tree.
386  std::vector<Motion *> yNear;
387  yNear.reserve(nbh.size());
388  for (auto &j : nbh)
389  {
390  if (j->getSetType() == Motion::SET_CLOSED)
391  {
392  if (nearestK_)
393  {
394  // Only include neighbors that are mutually k-nearest
395  // Relies on NN datastructure returning k-nearest in sorted order
396  const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
397  const base::Cost worstCost =
398  opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
399 
400  if (opt_->isCostBetterThan(worstCost, connCost))
401  continue;
402  yNear.push_back(j);
403  }
404  else
405  yNear.push_back(j);
406  }
407  }
408 
409  // Sample again if the new sample does not connect to the tree.
410  if (yNear.empty())
411  continue;
412 
413  // cache for distance computations
414  //
415  // Our cost caches only increase in size, so they're only
416  // resized if they can't fit the current neighborhood
417  if (costs.size() < yNear.size())
418  {
419  costs.resize(yNear.size());
420  incCosts.resize(yNear.size());
421  sortedCostIndices.resize(yNear.size());
422  }
423 
424  // Finding the nearest neighbor to connect to
425  // By default, neighborhood states are sorted by cost, and collision checking
426  // is performed in increasing order of cost
427  //
428  // calculate all costs and distances
429  for (std::size_t i = 0; i < yNear.size(); ++i)
430  {
431  incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
432  costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
433  }
434 
435  // sort the nodes
436  //
437  // we're using index-value pairs so that we can get at
438  // original, unsorted indices
439  for (std::size_t i = 0; i < yNear.size(); ++i)
440  sortedCostIndices[i] = i;
441  std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
442 
443  // collision check until a valid motion is found
444  for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
445  i != sortedCostIndices.begin() + yNear.size(); ++i)
446  {
447  if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
448  {
449  m->setParent(yNear[*i]);
450  yNear[*i]->getChildren().push_back(m);
451  const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
452  m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
453  m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
454  m->setSetType(Motion::SET_OPEN);
455 
456  nn_->add(m);
457  saveNeighborhood(m);
458  updateNeighborhood(m, nbh);
459 
460  Open_.insert(m);
461  z = m;
462  break;
463  }
464  }
465  } // while (!ptc && Open_.empty())
466  } // else if (extendedFMT_ && !successfulExpansion)
467  } // While not at goal
468 
469  if (plannerSuccess)
470  {
471  // Return the path to z, since by definition of planner success, z is in the goal region
472  lastGoalMotion_ = z;
473  traceSolutionPathThroughTree(lastGoalMotion_);
474 
475  OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
476 
477  return base::PlannerStatus(true, false);
478  } // if plannerSuccess
479 
480  // Planner terminated without accomplishing goal
481  return {false, false};
482 }
483 
485 {
486  std::vector<Motion *> mpath;
487  Motion *solution = goalMotion;
488 
489  // Construct the solution path
490  while (solution != nullptr)
491  {
492  mpath.push_back(solution);
493  solution = solution->getParent();
494  }
495 
496  // Set the solution path
497  auto path(std::make_shared<PathGeometric>(si_));
498  int mPathSize = mpath.size();
499  for (int i = mPathSize - 1; i >= 0; --i)
500  path->append(mpath[i]->getState());
501  pdef_->addSolutionPath(path, false, -1.0, getName());
502 }
503 
505 {
506  // Find all nodes that are near z, and also in set Unvisited
507 
508  std::vector<Motion *> xNear;
509  const std::vector<Motion *> &zNeighborhood = neighborhoods_[*z];
510  const unsigned int zNeighborhoodSize = zNeighborhood.size();
511  xNear.reserve(zNeighborhoodSize);
512 
513  for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
514  {
515  Motion *x = zNeighborhood[i];
516  if (x->getSetType() == Motion::SET_UNVISITED)
517  {
518  saveNeighborhood(x);
519  if (nearestK_)
520  {
521  // Only include neighbors that are mutually k-nearest
522  // Relies on NN datastructure returning k-nearest in sorted order
523  const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
524  const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
525 
526  if (opt_->isCostBetterThan(worstCost, connCost))
527  continue;
528  xNear.push_back(x);
529  }
530  else
531  xNear.push_back(x);
532  }
533  }
534 
535  // For each node near z and in set Unvisited, attempt to connect it to set Open
536  std::vector<Motion *> yNear;
537  std::vector<Motion *> Open_new;
538  const unsigned int xNearSize = xNear.size();
539  for (unsigned int i = 0; i < xNearSize; ++i)
540  {
541  Motion *x = xNear[i];
542 
543  // Find all nodes that are near x and in set Open
544  const std::vector<Motion *> &xNeighborhood = neighborhoods_[x];
545 
546  const unsigned int xNeighborhoodSize = xNeighborhood.size();
547  yNear.reserve(xNeighborhoodSize);
548  for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
549  {
550  if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
551  yNear.push_back(xNeighborhood[j]);
552  }
553 
554  // Find the lowest cost-to-come connection from Open to x
555  base::Cost cMin(opt_->infiniteCost());
556  Motion *yMin = getBestParent(x, yNear, cMin);
557  yNear.clear();
558 
559  // If an optimal connection from Open to x was found
560  if (yMin != nullptr)
561  {
562  bool collision_free = false;
563  if (cacheCC_)
564  {
565  if (!yMin->alreadyCC(x))
566  {
567  collision_free = si_->checkMotion(yMin->getState(), x->getState());
568  ++collisionChecks_;
569  // Due to FMT* design, it is only necessary to save unsuccesful
570  // connection attemps because of collision
571  if (!collision_free)
572  yMin->addCC(x);
573  }
574  }
575  else
576  {
577  ++collisionChecks_;
578  collision_free = si_->checkMotion(yMin->getState(), x->getState());
579  }
580 
581  if (collision_free)
582  {
583  // Add edge from yMin to x
584  x->setParent(yMin);
585  x->setCost(cMin);
586  x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
587  yMin->getChildren().push_back(x);
588 
589  // Add x to Open
590  Open_new.push_back(x);
591  // Remove x from Unvisited
592  x->setSetType(Motion::SET_CLOSED);
593  }
594  } // An optimal connection from Open to x was found
595  } // For each node near z and in set Unvisited, try to connect it to set Open
596 
597  // Update Open
598  Open_.pop();
599  (*z)->setSetType(Motion::SET_CLOSED);
600 
601  // Add the nodes in Open_new to Open
602  unsigned int openNewSize = Open_new.size();
603  for (unsigned int i = 0; i < openNewSize; ++i)
604  {
605  Open_.insert(Open_new[i]);
606  Open_new[i]->setSetType(Motion::SET_OPEN);
607  }
608  Open_new.clear();
609 
610  if (Open_.empty())
611  {
612  if (!extendedFMT_)
613  OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
614  return false;
615  }
616 
617  // Take the top of Open as the new z
618  *z = Open_.top()->data;
619 
620  return true;
621 }
622 
624  base::Cost &cMin)
625 {
626  Motion *min = nullptr;
627  const unsigned int neighborsSize = neighbors.size();
628  for (unsigned int j = 0; j < neighborsSize; ++j)
629  {
630  const base::State *s = neighbors[j]->getState();
631  const base::Cost dist = opt_->motionCost(s, m->getState());
632  const base::Cost cNew = opt_->combineCosts(neighbors[j]->getCost(), dist);
633 
634  if (opt_->isCostBetterThan(cNew, cMin))
635  {
636  min = neighbors[j];
637  cMin = cNew;
638  }
639  }
640  return min;
641 }
642 
643 void ompl::geometric::FMT::updateNeighborhood(Motion *m, const std::vector<Motion *> nbh)
644 {
645  for (auto i : nbh)
646  {
647  // If CLOSED, the neighborhood already exists. If neighborhood already exists, we have
648  // to insert the node in the corresponding place of the neighborhood of the neighbor of m.
649  if (i->getSetType() == Motion::SET_CLOSED || neighborhoods_.find(i) != neighborhoods_.end())
650  {
651  const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
652  const base::Cost worstCost = opt_->motionCost(neighborhoods_[i].back()->getState(), i->getState());
653 
654  if (opt_->isCostBetterThan(worstCost, connCost))
655  continue;
656 
657  // Insert the neighbor in the vector in the correct order
658  std::vector<Motion *> &nbhToUpdate = neighborhoods_[i];
659  for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
660  {
661  // If connection to the new state is better than the current neighbor tested, insert.
662  const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
663  if (opt_->isCostBetterThan(connCost, cost))
664  {
665  nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
666  break;
667  }
668  }
669  }
670  else
671  {
672  std::vector<Motion *> nbh2;
673  if (nearestK_)
674  nn_->nearestK(m, NNk_, nbh2);
675  else
676  nn_->nearestR(m, NNr_, nbh2);
677 
678  if (!nbh2.empty())
679  {
680  // Save the neighborhood but skip the first element, since it will be motion m
681  neighborhoods_[i] = std::vector<Motion *>(nbh2.size() - 1, nullptr);
682  std::copy(nbh2.begin() + 1, nbh2.end(), neighborhoods_[i].begin());
683  }
684  else
685  {
686  // Save an empty neighborhood
687  neighborhoods_[i] = std::vector<Motion *>(0);
688  }
689  }
690  }
691 }
void freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:120
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
bool getHeuristics() const
Returns true if the heap is ordered taking into account cost to go heuristics.
Definition: FMT.h:290
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 setExtendedFMT(bool e)
Activates the extended FMT*: adding new samples if planner does not finish successfully.
Definition: FMT.h:296
bool getCacheCC() const
Get the state of the collision check caching.
Definition: FMT.h:277
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
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:484
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:206
Definition of an abstract state.
Definition: State.h:113
Motion * getParent() const
Get the parent motion of the current motion.
Definition: FMT.h:356
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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:362
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:78
std::vector< Motion * > & getChildren()
Get the children of the motion.
Definition: FMT.h:411
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:135
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition: BFMT.h:663
@ INVALID_GOAL
Invalid goal state.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
For each goal region, check to see if any of the sampled states fall within that region....
Definition: FMT.cpp:241
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
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:504
A class to store the exit status of Planner::solve()
bool getExtendedFMT() const
Returns true if the extended FMT* is activated.
Definition: FMT.h:302
void setHeuristics(bool h)
Activates the cost to go heuristics when ordering the heap.
Definition: FMT.h:283
void setCacheCC(bool ccc)
Sets the collision check caching to save calls to the collision checker with slightly memory usage as...
Definition: FMT.h:271
unsigned int getNumSamples() const
Get the number of states that the planner will sample.
Definition: FMT.h:212
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double getRadiusMultiplier() const
Get the multiplier used for the nearest neighbors search radius.
Definition: FMT.h:247
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:374
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
void setNearestK(bool nearestK)
If nearestK is true, FMT will be run using the Knearest strategy.
Definition: FMT.h:218
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...
Representation of a motion.
Definition: FMT.h:310
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:643
bool getNearestK() const
Get the state of the nearestK strategy.
Definition: FMT.h:224
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:623
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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:238
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
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
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...
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...
bool alreadyCC(Motion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Definition: FMT.h:387
Abstract definition of a goal region that can be sampled.
void addCC(Motion *m)
Caches a failed collision check to m.
Definition: FMT.h:393
@ INVALID_START
Invalid start state or no start state specified.
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:344
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122