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
53ompl::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
60 specs_.approximateSolutions = false;
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
73ompl::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
194double 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
203double 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 {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(opt_->infiniteCost());
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
637void 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}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
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
Abstract definition of a goal region that can be sampled.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: FMT.h:215
void setSetType(const SetType currentSet)
Specify the set that this motion belongs to.
Definition: FMT.h:278
base::State * getState() const
Get the state associated with the motion.
Definition: FMT.h:248
void addCC(Motion *m)
Caches a failed collision check to m.
Definition: FMT.h:297
void setCost(const base::Cost cost)
Set the cost-to-come for the current motion.
Definition: FMT.h:266
std::vector< Motion * > & getChildren()
Get the children of the motion.
Definition: FMT.h:315
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
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
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
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:478
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
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
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 freeMemory()
Free the memory allocated by this planner.
Definition: FMT.cpp:120
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: FMT.cpp:135
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
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition: FMT.cpp:194
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 setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: FMT.cpp:78
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60