Loading...
Searching...
No Matches
BFMT.cpp
1
2#include <boost/math/constants/constants.hpp>
3#include <boost/math/distributions/binomial.hpp>
4#include <ompl/datastructures/BinaryHeap.h>
5#include <ompl/tools/config/SelfConfig.h>
6
7#include <ompl/datastructures/NearestNeighborsGNAT.h>
8#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
9#include <ompl/geometric/planners/fmt/BFMT.h>
10
11#include <fstream>
12#include <ompl/base/spaces/RealVectorStateSpace.h>
13
14namespace ompl
15{
16 namespace geometric
17 {
18 BFMT::BFMT(const base::SpaceInformationPtr &si)
19 : base::Planner(si, "BFMT")
20 , freeSpaceVolume_(si_->getStateSpace()->getMeasure()) // An upper bound on the free space volume is the
21 // total space volume; the free fraction is estimated
22 // in sampleFree
23 {
24 specs_.approximateSolutions = false;
25 specs_.directed = false;
26
27 ompl::base::Planner::declareParam<unsigned int>("num_samples", this, &BFMT::setNumSamples,
28 &BFMT::getNumSamples, "10:10:1000000");
29 ompl::base::Planner::declareParam<double>("radius_multiplier", this, &BFMT::setRadiusMultiplier,
30 &BFMT::getRadiusMultiplier, "0.1:0.05:50.");
31 ompl::base::Planner::declareParam<bool>("nearest_k", this, &BFMT::setNearestK, &BFMT::getNearestK, "0,1");
32 ompl::base::Planner::declareParam<bool>("balanced", this, &BFMT::setExploration, &BFMT::getExploration,
33 "0,1");
34 ompl::base::Planner::declareParam<bool>("optimality", this, &BFMT::setTermination, &BFMT::getTermination,
35 "0,1");
36 ompl::base::Planner::declareParam<bool>("heuristics", this, &BFMT::setHeuristics, &BFMT::getHeuristics,
37 "0,1");
38 ompl::base::Planner::declareParam<bool>("cache_cc", this, &BFMT::setCacheCC, &BFMT::getCacheCC, "0,1");
39 ompl::base::Planner::declareParam<bool>("extended_fmt", this, &BFMT::setExtendedFMT, &BFMT::getExtendedFMT,
40 "0,1");
41 }
42
43 ompl::geometric::BFMT::~BFMT()
44 {
45 freeMemory();
46 }
47
49 {
50 if (pdef_)
51 {
52 /* Setup the optimization objective. If no optimization objective was
53 specified, then default to optimizing path length as computed by the
54 distance() function in the state space */
55 if (pdef_->hasOptimizationObjective())
56 opt_ = pdef_->getOptimizationObjective();
57 else
58 {
59 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
60 getName().c_str());
61 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
62 // Store the new objective in the problem def'n
63 pdef_->setOptimizationObjective(opt_);
64 }
65 Open_[0].getComparisonOperator().opt_ = opt_.get();
66 Open_[0].getComparisonOperator().heuristics_ = heuristics_;
67 Open_[1].getComparisonOperator().opt_ = opt_.get();
68 Open_[1].getComparisonOperator().heuristics_ = heuristics_;
69
70 if (!nn_)
71 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<BiDirMotion *>(this));
72 nn_->setDistanceFunction([this](const BiDirMotion *a, const BiDirMotion *b)
73 {
74 return distanceFunction(a, b);
75 });
76
77 if (nearestK_ && !nn_->reportsSortedResults())
78 {
79 OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy "
80 "disabled.",
81 getName().c_str());
82 nearestK_ = false;
83 }
84 }
85 else
86 {
87 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
88 setup_ = false;
89 }
90 }
91
93 {
94 if (nn_)
95 {
96 BiDirMotionPtrs motions;
97 nn_->list(motions);
98 for (auto &motion : motions)
99 {
100 si_->freeState(motion->getState());
101 delete motion;
102 }
103 }
104 }
105
107 {
108 Planner::clear();
109 sampler_.reset();
110 freeMemory();
111 if (nn_)
112 nn_->clear();
113 Open_[FWD].clear();
114 Open_[REV].clear();
115 Open_elements[FWD].clear();
116 Open_elements[REV].clear();
117 neighborhoods_.clear();
119 }
120
122 {
124 BiDirMotionPtrs motions;
125 nn_->list(motions);
126
127 int numStartNodes = 0;
128 int numGoalNodes = 0;
129 int numEdges = 0;
130 int numFwdEdges = 0;
131 int numRevEdges = 0;
132
133 int fwd_tree_tag = 1;
134 int rev_tree_tag = 2;
135
136 for (auto motion : motions)
137 {
138 bool inFwdTree = (motion->currentSet_[FWD] != BiDirMotion::SET_UNVISITED);
139
140 // For samples added to the fwd tree, add incoming edges (from fwd tree parent)
141 if (inFwdTree)
142 {
143 if (motion->parent_[FWD] == nullptr)
144 {
145 // Motion is a forward tree root node
146 ++numStartNodes;
147 }
148 else
149 {
150 bool success =
151 data.addEdge(base::PlannerDataVertex(motion->parent_[FWD]->getState(), fwd_tree_tag),
152 base::PlannerDataVertex(motion->getState(), fwd_tree_tag));
153 if (success)
154 {
155 ++numFwdEdges;
156 ++numEdges;
157 }
158 }
159 }
160 }
161
162 // The edges in the goal tree are reversed so that they are in the same direction as start tree
163 for (auto motion : motions)
164 {
165 bool inRevTree = (motion->currentSet_[REV] != BiDirMotion::SET_UNVISITED);
166
167 // For samples added to a tree, add incoming edges (from fwd tree parent)
168 if (inRevTree)
169 {
170 if (motion->parent_[REV] == nullptr)
171 {
172 // Motion is a reverse tree root node
173 ++numGoalNodes;
174 }
175 else
176 {
177 bool success =
178 data.addEdge(base::PlannerDataVertex(motion->getState(), rev_tree_tag),
179 base::PlannerDataVertex(motion->parent_[REV]->getState(), rev_tree_tag));
180 if (success)
181 {
182 ++numRevEdges;
183 ++numEdges;
184 }
185 }
186 }
187 }
188 }
189
191 {
192 // Check if neighborhood has already been saved
193 if (neighborhoods_.find(m) == neighborhoods_.end())
194 {
195 BiDirMotionPtrs neighborhood;
196 if (nearestK_)
197 nn_->nearestK(m, NNk_, neighborhood);
198 else
199 nn_->nearestR(m, NNr_, neighborhood);
200
201 if (!neighborhood.empty())
202 {
203 // Save the neighborhood but skip the first element (m)
204 neighborhoods_[m] = std::vector<BiDirMotion *>(neighborhood.size() - 1, nullptr);
205 std::copy(neighborhood.begin() + 1, neighborhood.end(), neighborhoods_[m].begin());
206 }
207 else
208 {
209 // Save an empty neighborhood
210 neighborhoods_[m] = std::vector<BiDirMotion *>(0);
211 }
212 }
213 }
214
215 void BFMT::sampleFree(const std::shared_ptr<NearestNeighbors<BiDirMotion *>> &nn,
217 {
218 unsigned int nodeCount = 0;
219 unsigned int sampleAttempts = 0;
220 auto *motion = new BiDirMotion(si_, &tree_);
221
222 // Sample numSamples_ number of nodes from the free configuration space
223 while (nodeCount < numSamples_ && !ptc)
224 {
225 sampler_->sampleUniform(motion->getState());
226 sampleAttempts++;
227 if (si_->isValid(motion->getState()))
228 { // collision checking
229 ++nodeCount;
230 nn->add(motion);
231 motion = new BiDirMotion(si_, &tree_);
232 }
233 }
234 si_->freeState(motion->getState());
235 delete motion;
236
237 // 95% confidence limit for an upper bound for the true free space volume
239 boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) *
240 si_->getStateSpace()->getMeasure();
241 }
242
243 double BFMT::calculateUnitBallVolume(const unsigned int dimension) const
244 {
245 if (dimension == 0)
246 return 1.0;
247 if (dimension == 1)
248 return 2.0;
249 return 2.0 * boost::math::constants::pi<double>() / dimension * calculateUnitBallVolume(dimension - 2);
250 }
251
252 double BFMT::calculateRadius(const unsigned int dimension, const unsigned int n) const
253 {
254 double a = 1.0 / (double)dimension;
255 double unitBallVolume = calculateUnitBallVolume(dimension);
256
257 return radiusMultiplier_ * 2.0 * std::pow(a, a) * std::pow(freeSpaceVolume_ / unitBallVolume, a) *
258 std::pow(log((double)n) / (double)n, a);
259 }
260
262 {
264 if (!sampler_)
265 {
266 sampler_ = si_->allocStateSampler();
267 }
268 goal_s = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
269 }
270
272 {
274 initializeProblem(goal_s);
275 if (goal_s == nullptr)
276 {
277 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
279 }
280
281 useFwdTree();
282
283 // Add start states to Unvisitedfwd and Openfwd
284 bool valid_initMotion = false;
285 BiDirMotion *initMotion;
286 while (const base::State *st = pis_.nextStart())
287 {
288 initMotion = new BiDirMotion(si_, &tree_);
289 si_->copyState(initMotion->getState(), st);
290
291 initMotion->currentSet_[REV] = BiDirMotion::SET_UNVISITED;
292 nn_->add(initMotion); // S <-- {x_init}
293 if (si_->isValid(initMotion->getState()))
294 {
295 // Take the first valid initial state as the forward tree root
296 Open_elements[FWD][initMotion] = Open_[FWD].insert(initMotion);
297 initMotion->currentSet_[FWD] = BiDirMotion::SET_OPEN;
298 initMotion->cost_[FWD] = opt_->initialCost(initMotion->getState());
299 valid_initMotion = true;
300 heurGoalState_[1] = initMotion->getState();
301 }
302 }
303
304 if ((initMotion == nullptr) || !valid_initMotion)
305 {
306 OMPL_ERROR("Start state undefined or invalid.");
308 }
309
310 // Sample N free states in configuration state_
311 sampleFree(nn_, ptc); // S <-- SAMPLEFREE(N)
312 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
313 nn_->size());
314
315 // Calculate the nearest neighbor search radius
316 if (nearestK_)
317 {
318 NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
319 (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
320 log((double)nn_->size()));
321 OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
322 }
323 else
324 {
325 NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
326 OMPL_DEBUG("Using radius of %f", NNr_);
327 }
328
329 // Add goal states to Unvisitedrev and Openrev
330 bool valid_goalMotion = false;
331 BiDirMotion *goalMotion;
332 while (const base::State *st = pis_.nextGoal())
333 {
334 goalMotion = new BiDirMotion(si_, &tree_);
335 si_->copyState(goalMotion->getState(), st);
336
337 goalMotion->currentSet_[FWD] = BiDirMotion::SET_UNVISITED;
338 nn_->add(goalMotion); // S <-- {x_goal}
339 if (si_->isValid(goalMotion->getState()))
340 {
341 // Take the first valid goal state as the reverse tree root
342 Open_elements[REV][goalMotion] = Open_[REV].insert(goalMotion);
343 goalMotion->currentSet_[REV] = BiDirMotion::SET_OPEN;
344 goalMotion->cost_[REV] = opt_->terminalCost(goalMotion->getState());
345 valid_goalMotion = true;
346 heurGoalState_[0] = goalMotion->getState();
347 }
348 }
349
350 if ((goalMotion == nullptr) || !valid_goalMotion)
351 {
352 OMPL_ERROR("Goal state undefined or invalid.");
354 }
355
356 useRevTree();
357
358 // Plan a path
359 BiDirMotion *connection_point = nullptr;
360 bool earlyFailure = true;
361
362 if (initMotion != nullptr && goalMotion != nullptr)
363 {
364 earlyFailure = plan(initMotion, goalMotion, connection_point, ptc);
365 }
366 else
367 {
368 OMPL_ERROR("Initial/goal state(s) are undefined!");
369 }
370
371 if (earlyFailure)
372 {
373 return base::PlannerStatus(false, false);
374 }
375
376 // Save the best path (through z)
377 if (!ptc)
378 {
379 base::Cost fwd_cost, rev_cost, connection_cost;
380
381 // Construct the solution path
382 useFwdTree();
383 BiDirMotionPtrs path_fwd;
384 tracePath(connection_point, path_fwd);
385 fwd_cost = connection_point->getCost();
386
387 useRevTree();
388 BiDirMotionPtrs path_rev;
389 tracePath(connection_point, path_rev);
390 rev_cost = connection_point->getCost();
391
392 // ASSUMES FROM THIS POINT THAT z = path_fwd[0] = path_rev[0]
393 // Remove the first element, z, in the traced reverse path
394 // (the same as the first element in the traced forward path)
395 if (path_rev.size() > 1)
396 {
397 connection_cost = base::Cost(rev_cost.value() - path_rev[1]->getCost().value());
398 path_rev.erase(path_rev.begin());
399 }
400 else if (path_fwd.size() > 1)
401 {
402 connection_cost = base::Cost(fwd_cost.value() - path_fwd[1]->getCost().value());
403 path_fwd.erase(path_fwd.begin());
404 }
405 else
406 {
407 OMPL_ERROR("Solution path traced incorrectly or otherwise constructed improperly \
408 through forward/reverse trees (both paths are one node in length, each).");
409 }
410
411 // Adjust costs/parents in reverse tree nodes as cost/direction from forward tree root
412 useFwdTree();
413 path_rev[0]->setCost(base::Cost(path_fwd[0]->getCost().value() + connection_cost.value()));
414 path_rev[0]->setParent(path_fwd[0]);
415 for (unsigned int i = 1; i < path_rev.size(); ++i)
416 {
417 path_rev[i]->setCost(
418 base::Cost(fwd_cost.value() + (rev_cost.value() - path_rev[i]->getCost().value())));
419 path_rev[i]->setParent(path_rev[i - 1]);
420 }
421
422 BiDirMotionPtrs mpath;
423 std::reverse(path_rev.begin(), path_rev.end());
424 mpath.reserve(path_fwd.size() + path_rev.size()); // preallocate memory
425 mpath.insert(mpath.end(), path_rev.begin(), path_rev.end());
426 mpath.insert(mpath.end(), path_fwd.begin(), path_fwd.end());
427
428 // Set the solution path
429 auto path(std::make_shared<PathGeometric>(si_));
430 for (int i = mpath.size() - 1; i >= 0; --i)
431 {
432 path->append(mpath[i]->getState());
433 }
434
435 static const bool approximate = false;
436 static const double cost_difference_from_goal = 0.0;
437 pdef_->addSolutionPath(path, approximate, cost_difference_from_goal, getName());
438
439 OMPL_DEBUG("Total path cost: %f\n", fwd_cost.value() + rev_cost.value());
440 return base::PlannerStatus(true, false);
441 }
442
443 // Planner terminated without accomplishing goal
444 return {false, false};
445 }
446
448 {
449 // Define Opennew and set it to NULL
450 BiDirMotionPtrs Open_new;
451
452 // Define Znear as all unexplored nodes in the neighborhood around z
453 BiDirMotionPtrs zNear;
454 const BiDirMotionPtrs &zNeighborhood = neighborhoods_[z];
455
456 for (auto i : zNeighborhood)
457 {
458 if (i->getCurrentSet() == BiDirMotion::SET_UNVISITED)
459 {
460 zNear.push_back(i);
461 }
462 }
463
464 // For each node x in Znear
465 for (auto x : zNear)
466 {
467 if (!precomputeNN_)
468 saveNeighborhood(x); // nearest neighbors
469
470 // Define Xnear as all frontier nodes in the neighborhood around the unexplored node x
471 BiDirMotionPtrs xNear;
472 const BiDirMotionPtrs &xNeighborhood = neighborhoods_[x];
473 for (auto j : xNeighborhood)
474 {
475 if (j->getCurrentSet() == BiDirMotion::SET_OPEN)
476 {
477 xNear.push_back(j);
478 }
479 }
480 // Find the node in Xnear with minimum cost-to-come in the current tree
481 BiDirMotion *xMin = nullptr;
482 double cMin = std::numeric_limits<double>::infinity();
483 for (auto &j : xNear)
484 {
485 // check if node costs are smaller than minimum
486 double cNew = j->getCost().value() + distanceFunction(j, x);
487
488 if (cNew < cMin)
489 {
490 xMin = j;
491 cMin = cNew;
492 }
493 }
494
495 // xMin was found
496 if (xMin != nullptr)
497 {
498 bool collision_free = false;
499 if (cacheCC_)
500 {
501 if (!xMin->alreadyCC(x))
502 {
503 collision_free = si_->checkMotion(xMin->getState(), x->getState());
505 // Due to FMT3* design, it is only necessary to save unsuccesful
506 // connection attemps because of collision
507 if (!collision_free)
508 xMin->addCC(x);
509 }
510 }
511 else
512 {
514 collision_free = si_->checkMotion(xMin->getState(), x->getState());
515 }
516
517 if (collision_free)
518 { // motion between yMin and x is obstacle free
519 // add edge from xMin to x
520 x->setParent(xMin);
521 x->setCost(base::Cost(cMin));
522 xMin->getChildren().push_back(x);
523
524 if (heuristics_)
525 x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), heurGoalState_[tree_]));
526
527 // check if new node x is in the other tree; if so, save result
528 if (x->getOtherSet() != BiDirMotion::SET_UNVISITED)
529 {
530 if (connection_point == nullptr)
531 {
532 connection_point = x;
533 if (termination_ == FEASIBILITY)
534 {
535 break;
536 }
537 }
538 else
539 {
540 if ((connection_point->cost_[FWD].value() + connection_point->cost_[REV].value()) >
541 (x->cost_[FWD].value() + x->cost_[REV].value()))
542 {
543 connection_point = x;
544 }
545 }
546 }
547
548 Open_new.push_back(x); // add x to Open_new
549 x->setCurrentSet(BiDirMotion::SET_CLOSED); // remove x from Unvisited
550 }
551 }
552 } // End "for x in Znear"
553
554 // Remove motion z from binary heap and map
555 BiDirMotionBinHeap::Element *zElement = Open_elements[tree_][z];
556 Open_[tree_].remove(zElement);
557 Open_elements[tree_].erase(z);
558 z->setCurrentSet(BiDirMotion::SET_CLOSED);
559
560 // add nodes in Open_new to Open
561 for (auto &i : Open_new)
562 {
563 if(Open_elements[tree_][i] == nullptr)
564 {
566 i->setCurrentSet(BiDirMotion::SET_OPEN);
567 }
568 }
569 }
570
571 bool BFMT::plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point,
573 {
574 // If pre-computation, find neighborhoods for all N sample nodes plus initial
575 // and goal state(s). Otherwise compute the neighborhoods of the initial and
576 // goal states separately and compute the others as needed.
577 BiDirMotionPtrs sampleNodes;
578 nn_->list(sampleNodes);
581 if (precomputeNN_)
582 {
583 for (auto &sampleNode : sampleNodes)
584 {
585 saveNeighborhood(sampleNode); // nearest neighbors
586 }
587 }
588 else
589 {
590 saveNeighborhood(x_init); // nearest neighbors
591 saveNeighborhood(x_goal); // nearest neighbors
592 }
593
594 // Copy nodes in the sample set to Unvisitedfwd. Overwrite the label of the initial
595 // node with set Open for the forward tree, since it starts in set Openfwd.
596 useFwdTree();
597 for (auto &sampleNode : sampleNodes)
598 {
599 sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
600 }
601 x_init->setCurrentSet(BiDirMotion::SET_OPEN);
602
603 // Copy nodes in the sample set to Unvisitedrev. Overwrite the label of the goal
604 // node with set Open for the reverse tree, since it starts in set Openrev.
605 useRevTree();
606 for (auto &sampleNode : sampleNodes)
607 {
608 sampleNode->setCurrentSet(BiDirMotion::SET_UNVISITED);
609 }
610 x_goal->setCurrentSet(BiDirMotion::SET_OPEN);
611
612 // Expand the trees until reaching the termination condition
613 bool earlyFailure = false;
614 bool success = false;
615
616 useFwdTree();
617 BiDirMotion *z = x_init;
618
619 while (!success)
620 {
621 expandTreeFromNode(z, connection_point);
622
623 // Check if the algorithm should terminate. Possibly redefines connection_point.
624 if (termination(z, connection_point, ptc))
625 success = true;
626 else
627 {
628 if (Open_[tree_].empty()) // If this heap is empty...
629 {
630 if (!extendedFMT_) // ... eFMT not enabled...
631 {
632 if (Open_[(tree_ + 1) % 2].empty()) // ... and this one, failure.
633 {
634 OMPL_INFORM("Both Open are empty before path was found --> no feasible path exists");
635 earlyFailure = true;
636 return earlyFailure;
637 }
638 }
639 else // However, if eFMT is enabled, run it.
641 }
642
643 // This function will be always reached with at least one state in one heap.
644 // However, if ptc terminates, we should skip this.
645 if (!ptc)
647 else
648 return true;
649 }
650 }
651 earlyFailure = false;
652 return earlyFailure;
653 }
654
656 {
657 // Sample and connect samples to tree only if there is
658 // a possibility to connect to unvisited nodes.
659 std::vector<BiDirMotion *> nbh;
660 std::vector<base::Cost> costs;
661 std::vector<base::Cost> incCosts;
662 std::vector<std::size_t> sortedCostIndices;
663
664 // our functor for sorting nearest neighbors
665 CostIndexCompare compareFn(costs, *opt_);
666
667 auto *m = new BiDirMotion(si_, &tree_);
668 while (!ptc && Open_[tree_].empty()) //&& oneSample)
669 {
670 // Get new sample and check whether it is valid.
671 sampler_->sampleUniform(m->getState());
672 if (!si_->isValid(m->getState()))
673 continue;
674
675 // Get neighbours of the new sample.
676 std::vector<BiDirMotion *> yNear;
677 if (nearestK_)
678 nn_->nearestK(m, NNk_, nbh);
679 else
680 nn_->nearestR(m, NNr_, nbh);
681
682 yNear.reserve(nbh.size());
683 for (auto &j : nbh)
684 {
685 if (j->getCurrentSet() == BiDirMotion::SET_CLOSED)
686 {
687 if (nearestK_)
688 {
689 // Only include neighbors that are mutually k-nearest
690 // Relies on NN datastructure returning k-nearest in sorted order
691 const base::Cost connCost = opt_->motionCost(j->getState(), m->getState());
692 const base::Cost worstCost =
693 opt_->motionCost(neighborhoods_[j].back()->getState(), j->getState());
694
695 if (opt_->isCostBetterThan(worstCost, connCost))
696 continue;
697 yNear.push_back(j);
698 }
699 else
700 yNear.push_back(j);
701 }
702 }
703
704 // Sample again if the new sample does not connect to the tree.
705 if (yNear.empty())
706 continue;
707
708 // cache for distance computations
709 //
710 // Our cost caches only increase in size, so they're only
711 // resized if they can't fit the current neighborhood
712 if (costs.size() < yNear.size())
713 {
714 costs.resize(yNear.size());
715 incCosts.resize(yNear.size());
716 sortedCostIndices.resize(yNear.size());
717 }
718
719 // Finding the nearest neighbor to connect to
720 // By default, neighborhood states are sorted by cost, and collision checking
721 // is performed in increasing order of cost
722 //
723 // calculate all costs and distances
724 for (std::size_t i = 0; i < yNear.size(); ++i)
725 {
726 incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
727 costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
728 }
729
730 // sort the nodes
731 //
732 // we're using index-value pairs so that we can get at
733 // original, unsorted indices
734 for (std::size_t i = 0; i < yNear.size(); ++i)
735 sortedCostIndices[i] = i;
736 std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(), compareFn);
737
738 // collision check until a valid motion is found
739 for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
740 i != sortedCostIndices.begin() + yNear.size(); ++i)
741 {
743 if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
744 {
745 const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
746 m->setParent(yNear[*i]);
747 yNear[*i]->getChildren().push_back(m);
748 m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
749 m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), heurGoalState_[tree_]));
750 m->setCurrentSet(BiDirMotion::SET_OPEN);
752
753 nn_->add(m);
755 updateNeighborhood(m, nbh);
756
757 break;
758 }
759 }
760 } // While Open_[tree_] empty
761 }
762
763 bool BFMT::termination(BiDirMotion *&z, BiDirMotion *&connection_point,
765 {
766 bool terminate = false;
767 switch (termination_)
768 {
769 case FEASIBILITY:
770 // Test if a connection point was found during tree expansion
771 return (connection_point != nullptr || ptc);
772 break;
773
774 case OPTIMALITY:
775 // Test if z is in SET_CLOSED (interior) of other tree
776 if (ptc)
777 terminate = true;
778 else if (z->getOtherSet() == BiDirMotion::SET_CLOSED)
779 terminate = true;
780
781 break;
782 };
783 return terminate;
784 }
785
786 // Choose exploration tree and node z to expand
788 {
789 switch (exploration_)
790 {
791 case SWAP_EVERY_TIME:
792 if (Open_[(tree_ + 1) % 2].empty())
793 z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
794 // condition in plan())
795 else
796 {
797 z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
798 swapTrees(); // Swap to the opposite tree
799 }
800 break;
801
802 case CHOOSE_SMALLEST_Z:
803 BiDirMotion *z1, *z2;
804 if (Open_[(tree_ + 1) % 2].empty())
805 z = Open_[tree_].top()->data; // Continue expanding the current tree (not empty by exit
806 // condition in plan())
807 else if (Open_[tree_].empty())
808 {
809 z = Open_[(tree_ + 1) % 2].top()->data; // Take top of opposite tree heap as new z
810 swapTrees(); // Swap to the opposite tree
811 }
812 else
813 {
814 z1 = Open_[tree_].top()->data;
815 z2 = Open_[(tree_ + 1) % 2].top()->data;
816
817 if (z1->getCost().value() < z2->getOtherCost().value())
818 z = z1;
819 else
820 {
821 z = z2;
822 swapTrees();
823 }
824 }
825 break;
826 };
827 }
828
829 // Trace a path of nodes along a tree towards the root (forward or reverse)
830 void BFMT::tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
831 {
832 BiDirMotion *solution = z;
833
834 while (solution != nullptr)
835 {
836 path.push_back(solution);
837 solution = solution->getParent();
838 }
839 }
840
842 {
843 tree_ = (TreeType)((((int)tree_) + 1) % 2);
844 }
845
846 void BFMT::updateNeighborhood(BiDirMotion *m, const std::vector<BiDirMotion *> nbh)
847 {
848 // Neighborhoods are only updated if the new motion is within bounds (k nearest or within r).
849 for (auto i : nbh)
850 {
851 // If CLOSED, that neighborhood won't be used again.
852 // Else, if neighhboorhod already exists, we have to insert the node in
853 // the corresponding place of the neighborhood of the neighbor of m.
854 if (i->getCurrentSet() == BiDirMotion::SET_CLOSED)
855 continue;
856
857 auto it = neighborhoods_.find(i);
858 if (it != neighborhoods_.end())
859 {
860 if (it->second.empty())
861 continue;
862
863 const base::Cost connCost = opt_->motionCost(i->getState(), m->getState());
864 const base::Cost worstCost = opt_->motionCost(it->second.back()->getState(), i->getState());
865
866 if (opt_->isCostBetterThan(worstCost, connCost))
867 continue;
868
869 // insert the neighbor in the vector in the correct order
870 std::vector<BiDirMotion *> &nbhToUpdate = it->second;
871 for (std::size_t j = 0; j < nbhToUpdate.size(); ++j)
872 {
873 // If connection to the new state is better than the current neighbor tested, insert.
874 const base::Cost cost = opt_->motionCost(i->getState(), nbhToUpdate[j]->getState());
875 if (opt_->isCostBetterThan(connCost, cost))
876 {
877 nbhToUpdate.insert(nbhToUpdate.begin() + j, m);
878 break;
879 }
880 }
881 }
882 }
883 }
884 } // End "geometric" namespace
885} // End "ompl" namespace
Element * insert(const _T &data)
Add a new element.
Definition BinaryHeap.h:140
void remove(Element *element)
Remove a specific element.
Definition BinaryHeap.h:132
Element * top() const
Return the top element. nullptr for an empty heap.
Definition BinaryHeap.h:120
void clear()
Clear the heap.
Definition BinaryHeap.h:112
LessThan & getComparisonOperator()
Return a reference to the comparison operator.
Definition BinaryHeap.h:234
Abstract representation of a container that can perform nearest neighbors queries.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
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,...
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...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:237
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition Planner.cpp:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
Definition Planner.h:423
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:420
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:417
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:106
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:440
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition Planner.cpp:129
Definition of an abstract state.
Definition State.h:50
Representation of a bidirectional motion.
Definition BFMT.h:274
BiDirMotion * getParent() const
Get the parent of the motion.
Definition BFMT.h:366
bool alreadyCC(BiDirMotion *m)
Returns true if the connection to m has been already tested and failed because of a collision.
Definition BFMT.h:427
base::Cost cost_[2]
The cost of this motion
Definition BFMT.h:333
SetType getOtherSet() const
Get set of this motion in the inactive tree.
Definition BFMT.h:396
base::State * getState() const
Get the state associated with the motion.
Definition BFMT.h:420
BiDirMotionPtrs getChildren() const
Get the children of the motion.
Definition BFMT.h:378
void addCC(BiDirMotion *m)
Caches a failed collision check to m.
Definition BFMT.h:433
base::Cost getOtherCost() const
Get cost of this motion in the inactive tree.
Definition BFMT.h:348
SetType currentSet_[2]
Current set in which the motion is included.
Definition BFMT.h:327
void setCurrentSet(SetType set)
Set the current set of the motion.
Definition BFMT.h:384
base::Cost getCost() const
Set the state associated with the motion.
Definition BFMT.h:342
TerminateType termination_
Termination strategy used.
Definition BFMT.h:588
double NNr_
Radius employed in the nearestR strategy.
Definition BFMT.h:576
double radiusMultiplier_
This planner uses a nearest neighbor search radius proportional to the lower bound for optimality der...
Definition BFMT.h:563
std::map< BiDirMotion *, BiDirMotionBinHeap::Element * > Open_elements[2]
Map to know the corresponding heap element from the given motion.
Definition BFMT.h:607
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition BFMT.cpp:48
void insertNewSampleInOpen(const base::PlannerTerminationCondition &ptc)
Extended FMT strategy: inserts a new motion in open if the heap is empty.
Definition BFMT.cpp:655
bool plan(BiDirMotion *x_init, BiDirMotion *x_goal, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Executes the actual planning algorithm, swapping and expanding the trees.
Definition BFMT.cpp:571
base::State * heurGoalState_[2]
Goal state caching to accelerate cost to go heuristic computation.
Definition BFMT.h:619
void tracePath(BiDirMotion *z, BiDirMotionPtrs &path)
Trace the path along a tree towards the root (forward or reverse)
Definition BFMT.cpp:830
TreeType tree_
Active tree.
Definition BFMT.h:582
bool termination(BiDirMotion *&z, BiDirMotion *&connection_point, const base::PlannerTerminationCondition &ptc)
Checks if the termination condition is met.
Definition BFMT.cpp:763
unsigned int collisionChecks_
Number of collision checks performed by the algorithm.
Definition BFMT.h:570
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 BFMT.cpp:121
void freeMemory()
Free the memory allocated by this planner.
Definition BFMT.cpp:92
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
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
std::shared_ptr< NearestNeighbors< BiDirMotion * > > nn_
A nearest-neighbor datastructure containing the set of all motions.
Definition BFMT.h:594
double distanceFunction(const BiDirMotion *a, const BiDirMotion *b) const
Compute the distance between two motions as the cost between their contained states....
Definition BFMT.h:490
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
ExploreType exploration_
Exploration strategy used.
Definition BFMT.h:585
void initializeProblem(base::GoalSampleableRegion *&goal_s)
Carries out some planner checks.
Definition BFMT.cpp:261
bool heuristics_
Flag to activate the cost to go heuristics.
Definition BFMT.h:616
double calculateUnitBallVolume(unsigned int dimension) const
Compute the volume of the unit ball in a given dimension.
Definition BFMT.cpp:243
base::StateSamplerPtr sampler_
State sampler.
Definition BFMT.h:610
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
base::OptimizationObjectivePtr opt_
The cost objective function.
Definition BFMT.h:613
void chooseTreeAndExpansionNode(BiDirMotion *&z)
Chooses and expand a tree according to the exploration strategy.
Definition BFMT.cpp:787
void saveNeighborhood(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 NNk_
K used in the nearestK strategy.
Definition BFMT.h:579
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:846
void useFwdTree()
Sets forward tree active.
Definition BFMT.h:475
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::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 BFMT.cpp:271
unsigned int numSamples_
The number of samples to use when planning.
Definition BFMT.h:552
void useRevTree()
Sets reverse tree active.
Definition BFMT.h:481
double freeSpaceVolume_
The volume of numSathe free configuration space, computed as an upper bound with 95% confidence.
Definition BFMT.h:567
void swapTrees()
Change the active tree.
Definition BFMT.cpp:841
bool precomputeNN_
If true all the nearest neighbors maps are precomputed before solving.
Definition BFMT.h:591
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition BFMT.cpp:106
TreeType
Tree identifier.
Definition BFMT.h:87
bool cacheCC_
Flag to activate the collision check caching.
Definition BFMT.h:622
bool nearestK_
Flag to activate the K nearest neighbors strategy.
Definition BFMT.h:573
bool extendedFMT_
Add new samples if the tree was not able to find a solution.
Definition BFMT.h:625
#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
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.