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