Loading...
Searching...
No Matches
BITstar.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 the University of Toronto 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: Jonathan Gammell, Marlin Strub */
36
37#include "ompl/geometric/planners/informedtrees/BITstar.h"
38
39#include <sstream>
40#include <iomanip>
41#include <memory>
42#include <boost/range/adaptor/reversed.hpp>
43
44#include "ompl/util/Console.h"
45#include "ompl/util/Exception.h"
46#include "ompl/util/String.h"
47#include "ompl/geometric/PathGeometric.h"
48#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
49
50#include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
51#include "ompl/geometric/planners/informedtrees/bitstar/IdGenerator.h"
52#include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
53#include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
54#include "ompl/geometric/planners/informedtrees/bitstar/ImplicitGraph.h"
55#include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
56
57#ifdef BITSTAR_DEBUG
58#warning Compiling BIT* with debug-level asserts.
59#endif // BITSTAR_DEBUG
60
61namespace ompl
62{
63 namespace geometric
64 {
65 BITstar::BITstar(const ompl::base::SpaceInformationPtr &si, const std::string &name /*= "BITstar"*/)
66 : ompl::base::Planner(si, name)
67 {
68#ifdef BITSTAR_DEBUG
69 OMPL_WARN("%s: Compiled with debug-level asserts.", Planner::getName().c_str());
70#endif // BITSTAR_DEBUG
71
72 // Allocate my helper classes, they hold settings and must never be deallocated. Give them a pointer to my
73 // name, so they can output helpful error messages
74 costHelpPtr_ = std::make_shared<CostHelper>();
75 graphPtr_ = std::make_shared<ImplicitGraph>([this]() { return getName(); });
76 queuePtr_ = std::make_shared<SearchQueue>([this]() { return getName(); });
77
78 // Specify my planner specs:
79 Planner::specs_.recognizedGoal = ompl::base::GOAL_SAMPLEABLE_REGION;
80 Planner::specs_.multithreaded = false;
81 // Approximate solutions are supported but must be enabled with the appropriate configuration parameter.
82 Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
83 Planner::specs_.optimizingPaths = true;
84 Planner::specs_.directed = true;
85 Planner::specs_.provingSolutionNonExistence = false;
86 Planner::specs_.canReportIntermediateSolutions = true;
87
88 // Register my setting callbacks
89 Planner::declareParam<double>("rewire_factor", this, &BITstar::setRewireFactor, &BITstar::getRewireFactor,
90 "1.0:0.01:3.0");
91 Planner::declareParam<unsigned int>("samples_per_batch", this, &BITstar::setSamplesPerBatch,
92 &BITstar::getSamplesPerBatch, "1:1:1000000");
93 Planner::declareParam<bool>("use_k_nearest", this, &BITstar::setUseKNearest, &BITstar::getUseKNearest,
94 "0,"
95 "1");
96 Planner::declareParam<bool>("use_graph_pruning", this, &BITstar::setPruning, &BITstar::getPruning,
97 "0,"
98 "1");
99 Planner::declareParam<double>("prune_threshold_as_fractional_cost_change", this,
101 "0.0:0.01:1.0");
102 Planner::declareParam<bool>("delay_rewiring_to_first_solution", this,
105 Planner::declareParam<bool>("use_just_in_time_sampling", this, &BITstar::setJustInTimeSampling,
107 Planner::declareParam<bool>("drop_unconnected_samples_on_prune", this, &BITstar::setDropSamplesOnPrune,
109 Planner::declareParam<bool>("stop_on_each_solution_improvement", this, &BITstar::setStopOnSolnImprovement,
111 Planner::declareParam<bool>("use_strict_queue_ordering", this, &BITstar::setStrictQueueOrdering,
113 Planner::declareParam<bool>("find_approximate_solutions", this, &BITstar::setConsiderApproximateSolutions,
115
116 // Register my progress info:
117 addPlannerProgressProperty("best cost DOUBLE", [this] { return bestCostProgressProperty(); });
118 addPlannerProgressProperty("number of segments in solution path INTEGER",
119 [this] { return bestLengthProgressProperty(); });
120 addPlannerProgressProperty("state collision checks INTEGER",
121 [this] { return stateCollisionCheckProgressProperty(); });
122 addPlannerProgressProperty("edge collision checks INTEGER",
123 [this] { return edgeCollisionCheckProgressProperty(); });
124 addPlannerProgressProperty("nearest neighbour calls INTEGER",
125 [this] { return nearestNeighbourProgressProperty(); });
126
127 // Extra progress info that aren't necessary for every day use. Uncomment if desired.
128 /*
129 addPlannerProgressProperty("edge queue size INTEGER", [this]
130 {
131 return edgeQueueSizeProgressProperty();
132 });
133 addPlannerProgressProperty("iterations INTEGER", [this]
134 {
135 return iterationProgressProperty();
136 });
137 addPlannerProgressProperty("batches INTEGER", [this]
138 {
139 return batchesProgressProperty();
140 });
141 addPlannerProgressProperty("graph prunings INTEGER", [this]
142 {
143 return pruningProgressProperty();
144 });
145 addPlannerProgressProperty("total states generated INTEGER", [this]
146 {
147 return totalStatesCreatedProgressProperty();
148 });
149 addPlannerProgressProperty("vertices constructed INTEGER", [this]
150 {
151 return verticesConstructedProgressProperty();
152 });
153 addPlannerProgressProperty("states pruned INTEGER", [this]
154 {
155 return statesPrunedProgressProperty();
156 });
157 addPlannerProgressProperty("graph vertices disconnected INTEGER", [this]
158 {
159 return verticesDisconnectedProgressProperty();
160 });
161 addPlannerProgressProperty("rewiring edges INTEGER", [this]
162 {
163 return rewiringProgressProperty();
164 });
165 */
166 }
167
169 {
170 // Call the base class setup. Marks Planner::setup_ as true.
171 Planner::setup();
172
173 // Check if we have a problem definition
174 if (static_cast<bool>(Planner::pdef_))
175 {
176 // We do, do some initialization work.
177 // See if we have an optimization objective
178 if (!Planner::pdef_->hasOptimizationObjective())
179 {
180 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
181 Planner::getName().c_str());
182 Planner::pdef_->setOptimizationObjective(
183 std::make_shared<base::PathLengthOptimizationObjective>(Planner::si_));
184 }
185 // No else, we were given one.
186
187 // Initialize the best cost found so far to be infinite.
188 bestCost_ = Planner::pdef_->getOptimizationObjective()->infiniteCost();
189
190 // If the problem definition *has* a goal, make sure it is of appropriate type
191 if (static_cast<bool>(Planner::pdef_->getGoal()))
192 {
193 if (!Planner::pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION))
194 {
195 OMPL_ERROR("%s::setup() BIT* currently only supports goals that can be cast to a sampleable "
196 "goal region.",
197 Planner::getName().c_str());
198 // Mark as not setup:
199 Planner::setup_ = false;
200 return;
201 }
202 // No else, of correct type.
203 }
204 // No else, called without a goal. Is this MoveIt?
205
206 // Setup the CostHelper, it provides everything I need from optimization objective plus some frills
207 costHelpPtr_->setup(Planner::pdef_->getOptimizationObjective(), graphPtr_.get());
208
209 // Setup the queue
210 queuePtr_->setup(costHelpPtr_.get(), graphPtr_.get());
211
212 // Setup the graph, it does not hold a copy of this or Planner::pis_, but uses them to create a NN
213 // struct and check for starts/goals, respectively.
214 graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_.get(), queuePtr_.get(), this,
215 Planner::pis_);
216 graphPtr_->setPruning(isPruningEnabled_);
217
218 // Set the best and pruned costs to the proper objective-based values:
219 bestCost_ = costHelpPtr_->infiniteCost();
220 prunedCost_ = costHelpPtr_->infiniteCost();
221
222 // Get the measure of the problem
223 prunedMeasure_ = Planner::si_->getSpaceMeasure();
224
225 // If the planner is default named, we change it:
226 if (!graphPtr_->getUseKNearest() && Planner::getName() == "kBITstar")
227 {
228 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
229 OMPL_WARN("BIT*: An r-disc version of BIT* can not be named 'kBITstar', as this name is reserved "
230 "for the k-nearest version. Changing the name to 'BITstar'.");
231 Planner::setName("BITstar");
232 }
233 else if (graphPtr_->getUseKNearest() && Planner::getName() == "BITstar")
234 {
235 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
236 OMPL_WARN("BIT*: A k-nearest version of BIT* can not be named 'BITstar', as this name is reserved "
237 "for the r-disc version. Changing the name to 'kBITstar'.");
238 Planner::setName("kBITstar");
239 }
240 else if (!graphPtr_->getUseKNearest() && Planner::getName() == "kABITstar")
241 {
242 // It's current the default k-nearest ABIT* name, and we're toggling, so set to the default r-disc
243 OMPL_WARN("ABIT*: An r-disc version of ABIT* can not be named 'kABITstar', as this name is "
244 "reserved for the k-nearest version. Changing the name to 'ABITstar'.");
245 Planner::setName("ABITstar");
246 }
247 else if (graphPtr_->getUseKNearest() && Planner::getName() == "ABITstar")
248 {
249 // It's current the default r-disc ABIT* name, and we're toggling, so set to the default k-nearest
250 OMPL_WARN("ABIT*: A k-nearest version of ABIT* can not be named 'ABITstar', as this name is "
251 "reserved for the r-disc version. Changing the name to 'kABITstar'.");
252 Planner::setName("kABITstar");
253 }
254 // It's not default named, don't change it
255 }
256 else
257 {
258 // We don't, so we can't setup. Make sure that is explicit.
259 Planner::setup_ = false;
260 }
261 }
262
264 {
265 // Clear all the variables.
266 // Keep this in the order of the constructors:
267
268 // The various helper classes. DO NOT reset the pointers, they hold configuration parameters:
269 costHelpPtr_->reset();
270 graphPtr_->reset();
271 queuePtr_->reset();
272
273 // Reset the various calculations and convenience containers. Will be recalculated on setup
274 curGoalVertex_.reset();
275 bestCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
276 bestLength_ = 0u;
277 prunedCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
278 prunedMeasure_ = 0.0;
279 hasExactSolution_ = false;
280 stopLoop_ = false;
281 numBatches_ = 0u;
282 numPrunings_ = 0u;
283 numIterations_ = 0u;
284 numEdgeCollisionChecks_ = 0u;
285 numRewirings_ = 0u;
286
287 // DO NOT reset the configuration parameters:
288 // samplesPerBatch_
289 // usePruning_
290 // pruneFraction_
291 // stopOnSolnChange_
292
293 // Mark as not setup:
294 Planner::setup_ = false;
295
296 // Call my base clear:
297 Planner::clear();
298 }
299
301 {
302 // Check that Planner::setup_ is true, if not call this->setup()
303 Planner::checkValidity();
304
305 // Assert setup succeeded
306 if (!Planner::setup_)
307 {
308 throw ompl::Exception("%s::solve() failed to set up the planner. Has a problem definition been set?",
309 Planner::getName().c_str());
310 }
311 // No else
312
313 OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
314
315 // Reset the manual stop to the iteration loop:
316 stopLoop_ = false;
317
318 // If we don't have a goal yet, recall updateStartAndGoalStates, but wait for the first goal (or until the
319 // PTC comes true and we give up):
320 if (!graphPtr_->hasAGoal())
321 {
322 graphPtr_->updateStartAndGoalStates(Planner::pis_, ptc);
323 }
324
325 // Warn if we are missing a start
326 if (!graphPtr_->hasAStart())
327 {
328 // We don't have a start, since there's no way to wait for one to appear, so we will not be solving this
329 // "problem" today
330 OMPL_WARN("%s: A solution cannot be found as no valid start states are available.",
331 Planner::getName().c_str());
332 }
333 // No else, it's a start
334
335 // Warn if we are missing a goal
336 if (!graphPtr_->hasAGoal())
337 {
338 // We don't have a goal (and we waited as long as ptc allowed us for one to appear), so we will not be
339 // solving this "problem" today
340 OMPL_WARN("%s: A solution cannot be found as no valid goal states are available.",
341 Planner::getName().c_str());
342 }
343 // No else, there's a goal to all of this
344
345 // Insert the outgoing edges of the start vertices.
346 if (numIterations_ == 0u)
347 {
348 queuePtr_->insertOutgoingEdgesOfStartVertices();
349 }
350
351 /* Iterate as long as:
352 - We're allowed (ptc == false && stopLoop_ == false), AND
353 - We haven't found a good enough solution (costHelpPtr_->isSatisfied(bestCost) == false),
354 - AND
355 - There is a theoretically better solution (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(),
356 bestCost_) == true), OR
357 - There is are start/goal states we've yet to consider (pis_.haveMoreStartStates() == true ||
358 pis_.haveMoreGoalStates() == true)
359 */
360 while (!ptc && !stopLoop_ && !costHelpPtr_->isSatisfied(bestCost_) &&
361 (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(), bestCost_) ||
362 Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates()))
363 {
364 this->iterate();
365 }
366
367 // Announce
368 if (hasExactSolution_)
369 {
370 this->endSuccessMessage();
371 }
372 else
373 {
374 this->endFailureMessage();
375 }
376
377 // Publish
378 if (hasExactSolution_ || graphPtr_->getTrackApproximateSolutions())
379 {
380 // Any solution
381 this->publishSolution();
382 }
383 // No else, no solution to publish
384
385 // From OMPL's point-of-view, BIT* can always have an approximate solution, so mark solution true if either
386 // we have an exact solution or are finding approximate ones.
387 // Our returned solution will only be approximate if it is not exact and we are finding approximate
388 // solutions.
389 // PlannerStatus(addedSolution, approximate)
390 return {hasExactSolution_ || graphPtr_->getTrackApproximateSolutions(),
391 !hasExactSolution_ && graphPtr_->getTrackApproximateSolutions()};
392 }
393
395 {
396 // Get the base planner class data:
397 Planner::getPlannerData(data);
398
399 // Add the samples (the graph)
400 graphPtr_->getGraphAsPlannerData(data);
401
402 // Did we find a solution?
403 if (hasExactSolution_)
404 {
405 // Exact solution
406 data.markGoalState(curGoalVertex_->state());
407 }
408 else if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
409 {
410 // Approximate solution
411 data.markGoalState(graphPtr_->closestVertexToGoal()->state());
412 }
413 // No else, no solution
414 }
415
416 std::pair<ompl::base::State const *, ompl::base::State const *> BITstar::getNextEdgeInQueue()
417 {
418 // Variable:
419 // The next edge as a basic pair of states
420 std::pair<ompl::base::State const *, ompl::base::State const *> nextEdge;
421
422 if (!queuePtr_->isEmpty())
423 {
424 // Variable
425 // The edge in the front of the queue
426 VertexConstPtrPair frontEdge = queuePtr_->getFrontEdge();
427
428 // The next edge in the queue:
429 nextEdge = std::make_pair(frontEdge.first->state(), frontEdge.second->state());
430 }
431 else
432 {
433 // An empty edge:
434 nextEdge = std::make_pair<ompl::base::State *, ompl::base::State *>(nullptr, nullptr);
435 }
436
437 return nextEdge;
438 }
439
441 {
442 // Variable
443 // The cost of the next edge
444 ompl::base::Cost nextCost;
445
446 if (!queuePtr_->isEmpty())
447 {
448 // The next cost in the queue:
449 nextCost = queuePtr_->getFrontEdgeValue().at(0u);
450 }
451 else
452 {
453 // An infinite cost:
454 nextCost = costHelpPtr_->infiniteCost();
455 }
456
457 return nextCost;
458 }
459
461 {
462 queuePtr_->getEdges(edgesInQueue);
463 }
464
465 unsigned int BITstar::numIterations() const
466 {
467 return numIterations_;
468 }
469
471 {
472 return bestCost_;
473 }
474
475 unsigned int BITstar::numBatches() const
476 {
477 return numBatches_;
478 }
480
482 // Protected functions:
483 void BITstar::iterate()
484 {
485 // Keep track of how many iterations we've performed.
486 ++numIterations_;
487
488 // If the search is done or the queue is empty, we need to populate the queue.
489 if (isSearchDone_ || queuePtr_->isEmpty())
490 {
491 // Check whether we've exhausted the current approximation.
492 if (isFinalSearchOnBatch_ || !hasExactSolution_)
493 {
494 // Prune the graph if enabled.
495 if (isPruningEnabled_)
496 {
497 this->prune();
498 }
499
500 // Add a new batch.
501 this->newBatch();
502
503 // Set the inflation factor to an initial value.
504 queuePtr_->setInflationFactor(initialInflationFactor_);
505
506 // Clear the search queue.
507 queuePtr_->clear();
508
509 // Restart the queue, adding the outgoing edges of the start vertices to the queue.
510 queuePtr_->insertOutgoingEdgesOfStartVertices();
511
512 // Set flag to false.
513 isFinalSearchOnBatch_ = false;
514
515 // Set the new truncation factor.
516 truncationFactor_ =
517 1.0 + truncationScalingParameter_ /
518 (static_cast<float>(graphPtr_->numVertices() + graphPtr_->numSamples()));
519 }
520 else
521 {
522 // Exhaust the current approximation by performing an uninflated search.
523 queuePtr_->setInflationFactor(
524 1.0 + inflationScalingParameter_ /
525 (static_cast<float>(graphPtr_->numVertices() + graphPtr_->numSamples())));
526 queuePtr_->rebuildEdgeQueue();
527 queuePtr_->insertOutgoingEdgesOfInconsistentVertices();
528 queuePtr_->clearInconsistentSet();
529 isFinalSearchOnBatch_ = true;
530 }
531
532 isSearchDone_ = false;
533 }
534 else
535 {
536 // Get the most promising edge.
537 VertexPtrPair edge = queuePtr_->popFrontEdge();
538
539 // If this edge is already part of the search tree it's a freebie.
540 if (edge.second->hasParent() && edge.second->getParent()->getId() == edge.first->getId())
541 {
542 if (!edge.first->isExpandedOnCurrentSearch())
543 {
544 edge.first->registerExpansion();
545 }
546 queuePtr_->insertOutgoingEdges(edge.second);
547 }
548 // In the best case, can this edge improve our solution given the current graph?
549 // g_t(v) + c_hat(v,x) + h_hat(x) < g_t(x_g)?
550 else if (costHelpPtr_->isCostBetterThan(
551 costHelpPtr_->inflateCost(costHelpPtr_->currentHeuristicEdge(edge), truncationFactor_),
552 bestCost_))
553 {
554 // What about improving the current graph?
555 // g_t(v) + c_hat(v,x) < g_t(x)?
556 if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicToTarget(edge),
557 edge.second->getCost()))
558 {
559 // Ok, so it *could* be a useful edge. Do the work of calculating its cost for real
560
561 // Get the true cost of the edge
562 ompl::base::Cost trueEdgeCost = costHelpPtr_->trueEdgeCost(edge);
563
564 // Can this actual edge ever improve our solution?
565 // g_hat(v) + c(v,x) + h_hat(x) < g_t(x_g)?
566 if (costHelpPtr_->isCostBetterThan(
567 costHelpPtr_->combineCosts(costHelpPtr_->costToComeHeuristic(edge.first), trueEdgeCost,
568 costHelpPtr_->costToGoHeuristic(edge.second)),
569 bestCost_))
570 {
571 // Does this edge have a collision?
572 if (this->checkEdge(edge))
573 {
574 // Remember that this edge has passed the collision checks.
575 this->whitelistEdge(edge);
576
577 // Does the current edge improve our graph?
578 // g_t(v) + c(v,x) < g_t(x)?
579 if (costHelpPtr_->isCostBetterThan(
580 costHelpPtr_->combineCosts(edge.first->getCost(), trueEdgeCost),
581 edge.second->getCost()))
582 {
583 // YAAAAH. Add the edge! Allowing for the sample to be removed from free if it is
584 // not currently connected and otherwise propagate cost updates to descendants.
585 // addEdge will update the queue and handle the extra work that occurs if this edge
586 // improves the solution.
587 this->addEdge(edge, trueEdgeCost);
588
589 // If the path to the goal has changed, we will need to update the cached info about
590 // the solution cost or solution length:
591 this->updateGoalVertex();
592
593 // If this is the first edge that's being expanded in the current search, remember
594 // the cost-to-come and the search / approximation ids.
595 if (!edge.first->isExpandedOnCurrentSearch())
596 {
597 edge.first->registerExpansion();
598 }
599 }
600 // No else, this edge may be useful at some later date.
601 }
602 else // Remember that this edge is in collision.
603 {
604 this->blacklistEdge(edge);
605 }
606 }
607 // No else, we failed
608 }
609 // No else, we failed
610 }
611 else
612 {
613 isSearchDone_ = true;
614 }
615 } // Search queue not empty.
616 }
617
618 void BITstar::newBatch()
619 {
620 // Increment the batch counter.
621 ++numBatches_;
622
623 // Do we need to update our starts or goals?
624 if (Planner::pis_.haveMoreStartStates() || Planner::pis_.haveMoreGoalStates())
625 {
626 // There are new starts/goals to get.
627 graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition());
628 }
629 // No else, we have enough of a problem to do some work, and everything's up to date.
630
631 // Add a new batch of samples.
632 graphPtr_->addNewSamples(samplesPerBatch_);
633 }
634
635 void BITstar::prune()
636 {
637#ifdef BITSTAR_DEBUG
638 if (!isPruningEnabled_)
639 {
640 throw ompl::Exception("Pruning is not enabled, but prune() is called nonetheless.");
641 }
642#endif // BITSTAR_DEBUG
643
644 // If we don't have an exact solution, we can't prune sensibly.
645 if (hasExactSolution_)
646 {
647 /* Profiling reveals that pruning is very expensive, mainly because the nearest neighbour structure of
648 * the samples has to be updated. On the other hand, nearest neighbour lookup gets more expensive the
649 * bigger the structure, so it's a tradeoff. Pruning on every cost update seems insensible, but so does
650 * never pruning at all. The criteria to prune should depend on how many vertices/samples there are and
651 * how many of them could be pruned, as the decrease in cost associated with nearest neighbour lookup
652 * for fewer samples must justify the cost of pruning. It turns out that counting is affordable, so we
653 * don't need to use any proxy here. */
654
655 // Count the number of samples that could be pruned.
656 auto samples = graphPtr_->getCopyOfSamples();
657 auto numSamplesThatCouldBePruned =
658 std::count_if(samples.begin(), samples.end(), [this](const auto& sample){
659 return graphPtr_->canSampleBePruned(sample);
660 });
661
662 // Only prune if the decrease in number of samples and the associated decrease in nearest neighbour
663 // lookup cost justifies the cost of pruning. There has to be a way to make this more formal, and less
664 // knob-turney, right?
665 if (static_cast<float>(numSamplesThatCouldBePruned) /
666 static_cast<float>(graphPtr_->numSamples() + graphPtr_->numVertices()) >=
667 pruneFraction_)
668 {
669 // Get the current informed measure of the problem space.
670 double informedMeasure = graphPtr_->getInformedMeasure(bestCost_);
671
672 // Increment the pruning counter:
673 ++numPrunings_;
674
675 // Prune the graph.
676 std::pair<unsigned int, unsigned int> numPruned = graphPtr_->prune(informedMeasure);
677
678 // Store the cost at which we pruned:
679 prunedCost_ = bestCost_;
680
681 // Also store the measure.
682 prunedMeasure_ = informedMeasure;
683
684 OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
685 Planner::getName().c_str(), numPruned.first, numPruned.second);
686 }
687 }
688 // No else.
689 }
690
691 void BITstar::blacklistEdge(const VertexPtrPair &edge) const
692 {
693 // We store the actual blacklist with the parent vertex for efficient lookup.
694 edge.first->blacklistChild(edge.second);
695 }
696
697 void BITstar::whitelistEdge(const VertexPtrPair &edge) const
698 {
699 // We store the actual whitelist with the parent vertex for efficient lookup.
700 edge.first->whitelistChild(edge.second);
701 }
702
703 void BITstar::publishSolution()
704 {
705 // Variable
706 // The reverse path of state pointers
707 std::vector<const ompl::base::State *> reversePath;
708 // Allocate a path geometric
709 auto pathGeoPtr = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
710
711 // Get the reversed path
712 reversePath = this->bestPathFromGoalToStart();
713
714 // Now iterate that vector in reverse, putting the states into the path geometric
715 for (const auto &solnState : boost::adaptors::reverse(reversePath))
716 {
717 pathGeoPtr->append(solnState);
718 }
719
720 // Now create the solution
721 ompl::base::PlannerSolution soln(pathGeoPtr);
722
723 // Mark the name:
724 soln.setPlannerName(Planner::getName());
725
726 // Mark as approximate if not exact:
727 if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
728 {
729 soln.setApproximate(graphPtr_->smallestDistanceToGoal());
730 }
731
732 // Mark whether the solution met the optimization objective:
733 soln.setOptimized(Planner::pdef_->getOptimizationObjective(), bestCost_,
734 Planner::pdef_->getOptimizationObjective()->isSatisfied(bestCost_));
735
736 // Add the solution to the Problem Definition:
737 Planner::pdef_->addSolutionPath(soln);
738 }
739
740 std::vector<const ompl::base::State *> BITstar::bestPathFromGoalToStart() const
741 {
742 // Variables:
743 // A vector of states from goal->start:
744 std::vector<const ompl::base::State *> reversePath;
745 // The vertex used to ascend up from the goal:
746 VertexConstPtr curVertex;
747
748 // Iterate up the chain from the goal, creating a backwards vector:
749 if (hasExactSolution_)
750 {
751 // Start at vertex in the goal
752 curVertex = curGoalVertex_;
753 }
754 else if (!hasExactSolution_ && graphPtr_->getTrackApproximateSolutions())
755 {
756 // Start at the vertex closest to the goal
757 curVertex = graphPtr_->closestVertexToGoal();
758 }
759 else
760 {
761 throw ompl::Exception("bestPathFromGoalToStart called without an exact or approximate solution.");
762 }
763
764 // Insert the goal into the path
765 reversePath.push_back(curVertex->state());
766
767 // Then, use the vertex pointer like an iterator. Starting at the goal, we iterate up the chain pushing the
768 // *parent* of the iterator into the vector until the vertex has no parent.
769 // This will allows us to add the start (as the parent of the first child) and then stop when we get to the
770 // start itself, avoiding trying to find its nonexistent child
771 for (/*Already allocated & initialized*/; !curVertex->isRoot(); curVertex = curVertex->getParent())
772 {
773#ifdef BITSTAR_DEBUG
774 // Check the case where the chain ends incorrectly.
775 if (curVertex->hasParent() == false)
776 {
777 throw ompl::Exception("The path to the goal does not originate at a start state. Something went "
778 "wrong.");
779 }
780#endif // BITSTAR_DEBUG
781
782 // Push back the parent into the vector as a state pointer:
783 reversePath.push_back(curVertex->getParent()->state());
784 }
785 return reversePath;
786 }
787
788 bool BITstar::checkEdge(const VertexConstPtrPair &edge)
789 {
790#ifdef BITSTAR_DEBUG
791 if (edge.first->isBlacklistedAsChild(edge.second))
792 {
793 throw ompl::Exception("A blacklisted edge made it into the edge queue.");
794 }
795#endif // BITSTAR_DEBUG
796 // If this is a whitelisted edge, there's no need to do (repeat) the collision checking.
797 if (edge.first->isWhitelistedAsChild(edge.second))
798 {
799 return true;
800 }
801 else // This is a new edge, we need to check whether it is feasible.
802 {
803 ++numEdgeCollisionChecks_;
804 return Planner::si_->checkMotion(edge.first->state(), edge.second->state());
805 }
806 }
807
808 void BITstar::addEdge(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost)
809 {
810#ifdef BITSTAR_DEBUG
811 if (edge.first->isInTree() == false)
812 {
813 throw ompl::Exception("Adding an edge from a vertex not connected to the graph");
814 }
815 if (costHelpPtr_->isCostEquivalentTo(costHelpPtr_->trueEdgeCost(edge), edgeCost) == false)
816 {
817 throw ompl::Exception("You have passed the wrong edge cost to addEdge.");
818 }
819#endif // BITSTAR_DEBUG
820
821 // If the child already has a parent, this is a rewiring.
822 if (edge.second->hasParent())
823 {
824 // Replace the old parent.
825 this->replaceParent(edge, edgeCost);
826 } // If not, we add the vertex without replaceing a parent.
827 else
828 {
829 // Add a parent to the child.
830 edge.second->addParent(edge.first, edgeCost);
831
832 // Add a child to the parent.
833 edge.first->addChild(edge.second);
834
835 // Add the vertex to the set of vertices.
836 graphPtr_->registerAsVertex(edge.second);
837 }
838
839 // If the vertex hasn't already been expanded, insert its outgoing edges
840 if (!edge.second->isExpandedOnCurrentSearch())
841 {
842 queuePtr_->insertOutgoingEdges(edge.second);
843 }
844 else // If the vertex has already been expanded, remember it as inconsistent.
845 {
846 queuePtr_->addToInconsistentSet(edge.second);
847 }
848 }
849
850 void BITstar::replaceParent(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost)
851 {
852#ifdef BITSTAR_DEBUG
853 if (edge.second->getParent()->getId() == edge.first->getId())
854 {
855 throw ompl::Exception("The new and old parents of the given rewiring are the same.");
856 }
857 if (costHelpPtr_->isCostBetterThan(edge.second->getCost(),
858 costHelpPtr_->combineCosts(edge.first->getCost(), edgeCost)) == true)
859 {
860 throw ompl::Exception("The new edge will increase the cost-to-come of the vertex!");
861 }
862#endif // BITSTAR_DEBUG
863
864 // Increment our counter:
865 ++numRewirings_;
866
867 // Remove the child from the parent, not updating costs
868 edge.second->getParent()->removeChild(edge.second);
869
870 // Remove the parent from the child, not updating costs
871 edge.second->removeParent(false);
872
873 // Add the parent to the child. updating the downstream costs.
874 edge.second->addParent(edge.first, edgeCost);
875
876 // Add the child to the parent.
877 edge.first->addChild(edge.second);
878 }
879
880 void BITstar::updateGoalVertex()
881 {
882 // Variable
883 // Whether we've updated the goal, be pessimistic.
884 bool goalUpdated = false;
885 // The new goal, start with the current goal
886 VertexConstPtr newBestGoal = curGoalVertex_;
887 // The new cost, start as the current bestCost_
888 ompl::base::Cost newCost = bestCost_;
889
890 // Iterate through the vector of goals, and see if the solution has changed
891 for (auto it = graphPtr_->goalVerticesBeginConst(); it != graphPtr_->goalVerticesEndConst(); ++it)
892 {
893 // First, is this goal even in the tree?
894 if ((*it)->isInTree())
895 {
896 // Next, is there currently a solution?
897 if (static_cast<bool>(newBestGoal))
898 {
899 // There is already a solution, is it to this goal?
900 if ((*it)->getId() == newBestGoal->getId())
901 {
902 // Ah-ha, We meet again! Are we doing any better? We check the length as sometimes the path
903 // length changes with minimal change in cost.
904 if (!costHelpPtr_->isCostEquivalentTo((*it)->getCost(), newCost) ||
905 ((*it)->getDepth() + 1u) != bestLength_)
906 {
907 // The path to the current best goal has changed, so we need to update it.
908 goalUpdated = true;
909 newBestGoal = *it;
910 newCost = newBestGoal->getCost();
911 }
912 // No else, no change
913 }
914 else
915 {
916 // It is not to this goal, we have a second solution! What an easy problem... but is it
917 // better?
918 if (costHelpPtr_->isCostBetterThan((*it)->getCost(), newCost))
919 {
920 // It is! Save this as a better goal:
921 goalUpdated = true;
922 newBestGoal = *it;
923 newCost = newBestGoal->getCost();
924 }
925 // No else, not a better solution
926 }
927 }
928 else
929 {
930 // There isn't a preexisting solution, that means that any goal is an update:
931 goalUpdated = true;
932 newBestGoal = *it;
933 newCost = newBestGoal->getCost();
934 }
935 }
936 // No else, can't be a better solution if it's not in the spanning tree, can it?
937 }
938
939 // Did we update the goal?
940 if (goalUpdated)
941 {
942 // Mark that we have a solution
943 hasExactSolution_ = true;
944
945 // Store the current goal
946 curGoalVertex_ = newBestGoal;
947
948 // Update the best cost:
949 bestCost_ = newCost;
950
951 // and best length
952 bestLength_ = curGoalVertex_->getDepth() + 1u;
953
954 // Tell everyone else about it.
955 queuePtr_->registerSolutionCost(bestCost_);
956 graphPtr_->registerSolutionCost(bestCost_);
957
958 // Stop the solution loop if enabled:
959 stopLoop_ = stopOnSolutionChange_;
960
961 // Brag:
962 this->goalMessage();
963
964 // If enabled, pass the intermediate solution back through the call back:
965 if (static_cast<bool>(Planner::pdef_->getIntermediateSolutionCallback()))
966 {
967 // The form of path passed to the intermediate solution callback is not well documented, but it
968 // *appears* that it's not supposed
969 // to include the start or goal; however, that makes no sense for multiple start/goal problems, so
970 // we're going to include it anyway (sorry).
971 // Similarly, it appears to be ordered as (goal, goal-1, goal-2,...start+1, start) which
972 // conveniently allows us to reuse code.
973 Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
974 }
975 }
976 // No else, the goal didn't change
977 }
978
979 void BITstar::goalMessage() const
980 {
981 OMPL_INFORM("%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u "
982 "edges (%u collision checked) to create %u vertices and perform %u rewirings. The graph "
983 "currently has %u vertices.",
984 Planner::getName().c_str(), numIterations_, bestCost_.value(), bestLength_,
985 graphPtr_->numStatesGenerated(), queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_,
986 graphPtr_->numVerticesConnected(), numRewirings_, graphPtr_->numVertices());
987 }
988
989 void BITstar::endSuccessMessage() const
990 {
991 OMPL_INFORM("%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing "
992 "%u edges (%u collision checked) to create %u vertices and perform %u rewirings. The final "
993 "graph has %u vertices.",
994 Planner::getName().c_str(), bestCost_.value(), bestLength_, graphPtr_->numStatesGenerated(),
995 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
996 numRewirings_, graphPtr_->numVertices());
997 }
998
999 void BITstar::endFailureMessage() const
1000 {
1001 if (graphPtr_->getTrackApproximateSolutions())
1002 {
1003 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
1004 "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph "
1005 "has %u vertices. The best approximate solution was %.4f from the goal and has a cost of "
1006 "%.4f.",
1007 Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
1008 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
1009 numRewirings_, graphPtr_->numVertices(), graphPtr_->smallestDistanceToGoal(),
1010 graphPtr_->closestVertexToGoal()->getCost().value());
1011 }
1012 else
1013 {
1014 OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
1015 "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph "
1016 "has %u vertices.",
1017 Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
1018 queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
1019 numRewirings_, graphPtr_->numVertices());
1020 }
1021 }
1022
1023 void BITstar::statusMessage(const ompl::msg::LogLevel &logLevel, const std::string &status) const
1024 {
1025 // Check if we need to create the message
1026 if (logLevel >= ompl::msg::getLogLevel())
1027 {
1028 // Variable
1029 // The message as a stream:
1030 std::stringstream outputStream;
1031
1032 // Create the stream:
1033 // The name of the planner
1034 outputStream << Planner::getName();
1035 outputStream << " (";
1036 // The current path cost:
1037 outputStream << "l: " << std::setw(6) << std::setfill(' ') << std::setprecision(5) << bestCost_.value();
1038 // The number of batches:
1039 outputStream << ", b: " << std::setw(5) << std::setfill(' ') << numBatches_;
1040 // The number of iterations
1041 outputStream << ", i: " << std::setw(5) << std::setfill(' ') << numIterations_;
1042 // The number of states current in the graph
1043 outputStream << ", g: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVertices();
1044 // The number of free states
1045 outputStream << ", f: " << std::setw(5) << std::setfill(' ') << graphPtr_->numSamples();
1046 // The number edges in the queue:
1047 outputStream << ", q: " << std::setw(5) << std::setfill(' ') << queuePtr_->numEdges();
1048 // The total number of edges taken out of the queue:
1049 outputStream << ", t: " << std::setw(5) << std::setfill(' ') << queuePtr_->numEdgesPopped();
1050 // The number of samples generated
1051 outputStream << ", s: " << std::setw(5) << std::setfill(' ') << graphPtr_->numStatesGenerated();
1052 // The number of vertices ever added to the graph:
1053 outputStream << ", v: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVerticesConnected();
1054 // The number of prunings:
1055 outputStream << ", p: " << std::setw(5) << std::setfill(' ') << numPrunings_;
1056 // The number of rewirings:
1057 outputStream << ", r: " << std::setw(5) << std::setfill(' ') << numRewirings_;
1058 // The number of nearest-neighbour calls
1059 outputStream << ", n: " << std::setw(5) << std::setfill(' ') << graphPtr_->numNearestLookups();
1060 // The number of state collision checks:
1061 outputStream << ", c(s): " << std::setw(5) << std::setfill(' ') << graphPtr_->numStateCollisionChecks();
1062 // The number of edge collision checks:
1063 outputStream << ", c(e): " << std::setw(5) << std::setfill(' ') << numEdgeCollisionChecks_;
1064 outputStream << "): ";
1065 // The message:
1066 outputStream << status;
1067
1068 if (logLevel == ompl::msg::LOG_DEBUG)
1069 {
1070 OMPL_DEBUG("%s: ", outputStream.str().c_str());
1071 }
1072 else if (logLevel == ompl::msg::LOG_INFO)
1073 {
1074 OMPL_INFORM("%s: ", outputStream.str().c_str());
1075 }
1076 else if (logLevel == ompl::msg::LOG_WARN)
1077 {
1078 OMPL_WARN("%s: ", outputStream.str().c_str());
1079 }
1080 else if (logLevel == ompl::msg::LOG_ERROR)
1081 {
1082 OMPL_ERROR("%s: ", outputStream.str().c_str());
1083 }
1084 else
1085 {
1086 throw ompl::Exception("Log level not recognized");
1087 }
1088 }
1089 // No else, this message is below the log level
1090 }
1092
1094 // Boring sets/gets (Public) and progress properties (Protected):
1096 {
1097 initialInflationFactor_ = factor;
1098 queuePtr_->setInflationFactor(factor);
1099 }
1100
1102 {
1103 inflationScalingParameter_ = factor;
1104 }
1105
1107 {
1108 truncationScalingParameter_ = factor;
1109 }
1110
1112 {
1113 queuePtr_->enableCascadingRewirings(enable);
1114 }
1115
1117 {
1118 return initialInflationFactor_;
1119 }
1120
1122 {
1123 return inflationScalingParameter_;
1124 }
1125
1127 {
1128 return truncationScalingParameter_;
1129 }
1130
1132 {
1133 return queuePtr_->getInflationFactor();
1134 }
1135
1137 {
1138 return truncationFactor_;
1139 }
1140
1141 void BITstar::setRewireFactor(double rewireFactor)
1142 {
1143 graphPtr_->setRewireFactor(rewireFactor);
1144 }
1145
1147 {
1148 return graphPtr_->getRewireFactor();
1149 }
1150
1151 void BITstar::setSamplesPerBatch(unsigned int samplesPerBatch)
1152 {
1153 samplesPerBatch_ = samplesPerBatch;
1154 }
1155
1156 unsigned int BITstar::getSamplesPerBatch() const
1157 {
1158 return samplesPerBatch_;
1159 }
1160
1161 void BITstar::setUseKNearest(bool useKNearest)
1162 {
1163 // Store
1164 graphPtr_->setUseKNearest(useKNearest);
1165
1166 // If the planner is default named, we change it:
1167 if (!graphPtr_->getUseKNearest() && Planner::getName() == "kBITstar")
1168 {
1169 // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
1170 OMPL_WARN("BIT*: An r-disc version of BIT* can not be named 'kBITstar', as this name is reserved for "
1171 "the k-nearest version. Changing the name to 'BITstar'.");
1172 Planner::setName("BITstar");
1173 }
1174 else if (graphPtr_->getUseKNearest() && Planner::getName() == "BITstar")
1175 {
1176 // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
1177 OMPL_WARN("BIT*: A k-nearest version of BIT* can not be named 'BITstar', as this name is reserved for "
1178 "the r-disc version. Changing the name to 'kBITstar'.");
1179 Planner::setName("kBITstar");
1180 }
1181 else if (!graphPtr_->getUseKNearest() && Planner::getName() == "kABITstar")
1182 {
1183 // It's current the default k-nearest ABIT* name, and we're toggling, so set to the default r-disc
1184 OMPL_WARN("ABIT*: An r-disc version of ABIT* can not be named 'kABITstar', as this name is reserved "
1185 "for the k-nearest version. Changing the name to 'ABITstar'.");
1186 Planner::setName("ABITstar");
1187 }
1188 else if (graphPtr_->getUseKNearest() && Planner::getName() == "ABITstar")
1189 {
1190 // It's current the default r-disc ABIT* name, and we're toggling, so set to the default k-nearest
1191 OMPL_WARN("ABIT*: A k-nearest version of ABIT* can not be named 'ABITstar', as this name is reserved "
1192 "for the r-disc version. Changing the name to 'kABITstar'.");
1193 Planner::setName("kABITstar");
1194 }
1195 // It's not default named, don't change it
1196 }
1197
1199 {
1200 return graphPtr_->getUseKNearest();
1201 }
1202
1203 void BITstar::setStrictQueueOrdering(bool /* beStrict */)
1204 {
1205 OMPL_WARN("%s: This option no longer has any effect; The queue is always strictly ordered.",
1206 Planner::getName().c_str());
1207 }
1208
1210 {
1211 OMPL_WARN("%s: This option no longer has any effect; The queue is always strictly ordered.",
1212 Planner::getName().c_str());
1213 return true;
1214 }
1215
1216 void BITstar::setPruning(bool usePruning)
1217 {
1218 isPruningEnabled_ = usePruning;
1219 graphPtr_->setPruning(usePruning);
1220 }
1221
1223 {
1224 return isPruningEnabled_;
1225 }
1226
1227 void BITstar::setPruneThresholdFraction(double fractionalChange)
1228 {
1229 if (fractionalChange < 0.0 || fractionalChange > 1.0)
1230 {
1231 throw ompl::Exception("Prune threshold must be specified as a fraction between [0, 1].");
1232 }
1233
1234 pruneFraction_ = fractionalChange;
1235 }
1236
1238 {
1239 return pruneFraction_;
1240 }
1241
1243 {
1244 OMPL_WARN("%s: This option no longer has any effect; Rewiring is never delayed.",
1245 Planner::getName().c_str());
1246 }
1247
1249 {
1250 OMPL_WARN("%s: This option no longer has any effect; Rewiring is never delayed.",
1251 Planner::getName().c_str());
1252 return false;
1253 }
1254
1256 {
1257 graphPtr_->setJustInTimeSampling(useJit);
1258 }
1259
1261 {
1262 return graphPtr_->getJustInTimeSampling();
1263 }
1264
1265 void BITstar::setDropSamplesOnPrune(bool dropSamples)
1266 {
1267 graphPtr_->setDropSamplesOnPrune(dropSamples);
1268 }
1269
1271 {
1272 return graphPtr_->getDropSamplesOnPrune();
1273 }
1274
1276 {
1277 stopOnSolutionChange_ = stopOnChange;
1278 }
1279
1281 {
1282 return stopOnSolutionChange_;
1283 }
1284
1286 {
1287 // Store
1288 graphPtr_->setTrackApproximateSolutions(findApproximate);
1289
1290 // Mark the Planner spec:
1291 Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
1292 }
1293
1295 {
1296 return graphPtr_->getTrackApproximateSolutions();
1297 }
1298
1300 {
1301 graphPtr_->setAverageNumOfAllowedFailedAttemptsWhenSampling(number);
1302 }
1303
1305 {
1306 return graphPtr_->getAverageNumOfAllowedFailedAttemptsWhenSampling();
1307 }
1308
1309 template <template <typename T> class NN>
1311 {
1312 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1313 // change them:
1314 if (Planner::setup_)
1315 {
1316 OMPL_WARN("%s: The nearest neighbour datastructures cannot be changed once the planner is setup. "
1317 "Continuing to use the existing containers.",
1318 Planner::getName().c_str());
1319 }
1320 else
1321 {
1322 graphPtr_->setNearestNeighbors<NN>();
1323 }
1324 }
1325
1326 std::string BITstar::bestCostProgressProperty() const
1327 {
1328 return ompl::toString(bestCost_.value());
1329 }
1330
1331 std::string BITstar::bestLengthProgressProperty() const
1332 {
1333 return std::to_string(bestLength_);
1334 }
1335
1336 std::string BITstar::edgeQueueSizeProgressProperty() const
1337 {
1338 return std::to_string(queuePtr_->numEdges());
1339 }
1340
1341 std::string BITstar::iterationProgressProperty() const
1342 {
1343 return std::to_string(numIterations_);
1344 }
1345
1346 std::string BITstar::batchesProgressProperty() const
1347 {
1348 return std::to_string(numBatches_);
1349 }
1350
1351 std::string BITstar::pruningProgressProperty() const
1352 {
1353 return std::to_string(numPrunings_);
1354 }
1355
1356 std::string BITstar::totalStatesCreatedProgressProperty() const
1357 {
1358 return std::to_string(graphPtr_->numStatesGenerated());
1359 }
1360
1361 std::string BITstar::verticesConstructedProgressProperty() const
1362 {
1363 return std::to_string(graphPtr_->numVerticesConnected());
1364 }
1365
1366 std::string BITstar::statesPrunedProgressProperty() const
1367 {
1368 return std::to_string(graphPtr_->numFreeStatesPruned());
1369 }
1370
1371 std::string BITstar::verticesDisconnectedProgressProperty() const
1372 {
1373 return std::to_string(graphPtr_->numVerticesDisconnected());
1374 }
1375
1376 std::string BITstar::rewiringProgressProperty() const
1377 {
1378 return std::to_string(numRewirings_);
1379 }
1380
1381 std::string BITstar::stateCollisionCheckProgressProperty() const
1382 {
1383 return std::to_string(graphPtr_->numStateCollisionChecks());
1384 }
1385
1386 std::string BITstar::edgeCollisionCheckProgressProperty() const
1387 {
1388 return std::to_string(numEdgeCollisionChecks_);
1389 }
1390
1391 std::string BITstar::nearestNeighbourProgressProperty() const
1392 {
1393 return std::to_string(graphPtr_->numNearestLookups());
1394 }
1395
1396 std::string BITstar::edgesProcessedProgressProperty() const
1397 {
1398 return std::to_string(queuePtr_->numEdgesPopped());
1399 }
1401 } // namespace geometric
1402} // namespace ompl
The exception type for ompl.
Definition Exception.h:47
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
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition Planner.h:403
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
A shared pointer wrapper for ompl::base::SpaceInformation.
ompl::base::State * state()
The state of a vertex as a pointer.
Definition Vertex.cpp:152
unsigned int getDepth() const
Get the depth of the vertex from the root.
Definition Vertex.cpp:183
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition BITstar.cpp:1222
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition BITstar.cpp:470
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition BITstar.cpp:460
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition BITstar.cpp:1275
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition BITstar.cpp:1265
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition BITstar.cpp:1209
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition BITstar.cpp:1237
double getRewireFactor() const
Get the rewiring scale factor.
Definition BITstar.cpp:1146
double getInitialInflationFactor() const
Get the inflation factor for the initial search.
Definition BITstar.cpp:1116
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition BITstar.cpp:1141
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition BITstar.cpp:1156
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
Definition BITstar.cpp:1304
void setStrictQueueOrdering(bool beStrict)
Enable "strict sorting" of the edge queue. Rewirings can change the position in the queue of an edge....
Definition BITstar.cpp:1203
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition BITstar.cpp:1260
void setInflationScalingParameter(double parameter)
The parameter that scales the inflation factor on the second search of each RGG approximation....
Definition BITstar.cpp:1101
double getCurrentInflationFactor() const
Get the inflation factor for the current search.
Definition BITstar.cpp:1131
void getPlannerData(base::PlannerData &data) const override
Get results.
Definition BITstar.cpp:394
unsigned int numIterations() const
Get the number of iterations completed.
Definition BITstar.cpp:465
double getCurrentTruncationFactor() const
Get the truncation factor for the current search.
Definition BITstar.cpp:1136
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition BITstar.cpp:1216
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition BITstar.cpp:1280
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition BITstar.h:137
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition BITstar.h:134
double getTruncationScalingParameter() const
Get the truncation factor parameter.
Definition BITstar.cpp:1126
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition BITstar.h:119
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition BITstar.cpp:1151
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Definition BITstar.cpp:1299
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition BITstar.cpp:1310
ompl::base::Cost getNextEdgeValueInQueue()
Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if neces...
Definition BITstar.cpp:440
void setInitialInflationFactor(double factor)
Set the inflation for the initial search of RGG approximation. See ABIT*'s class description for more...
Definition BITstar.cpp:1095
void setJustInTimeSampling(bool useJit)
Delay the generation of samples until they are necessary. This only works when using an r-disc connec...
Definition BITstar.cpp:1255
void clear() override
Clear the algorithm's internal state.
Definition BITstar.cpp:263
BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name="kBITstar")
Construct with a pointer to the space information and an optional name.
Definition BITstar.cpp:65
void setup() override
Setup the algorithm.
Definition BITstar.cpp:168
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition BITstar.cpp:1248
void enableCascadingRewirings(bool enable)
Enable the cascading of rewirings.
Definition BITstar.cpp:1111
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition BITstar.cpp:1227
double getInflationScalingParameter() const
Get the inflation scaling parameter.
Definition BITstar.cpp:1121
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition BITstar.cpp:1161
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
Definition BITstar.cpp:1285
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition BITstar.h:143
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition BITstar.cpp:1294
base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override
Solve the problem given a termination condition.
Definition BITstar.cpp:300
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data.
Definition BITstar.cpp:475
void setTruncationScalingParameter(double parameter)
Sets the parameter that scales the truncation factor for the searches of each RGG approximation....
Definition BITstar.cpp:1106
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition BITstar.cpp:1242
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition BITstar.cpp:1198
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition BITstar.cpp:1270
std::pair< const ompl::base::State *, const ompl::base::State * > getNextEdgeInQueue()
Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and the...
Definition BITstar.cpp:416
#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
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
LogLevel
The set of priorities for message logging.
Definition Console.h:85
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded.
Definition Console.cpp:142
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
Representation of a solution to a planning problem.
A class to store the exit status of Planner::solve()