Loading...
Searching...
No Matches
ImplicitGraph.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// My definition:
38#include "ompl/geometric/planners/informedtrees/bitstar/ImplicitGraph.h"
39
40// STL/Boost:
41// For std::move
42#include <utility>
43// For smart pointers
44#include <memory>
45// For, you know, math
46#include <cmath>
47// For boost math constants
48#include <boost/math/constants/constants.hpp>
49
50// OMPL:
51// For OMPL_INFORM et al.
52#include "ompl/util/Console.h"
53// For exceptions:
54#include "ompl/util/Exception.h"
55// For SelfConfig
56#include "ompl/tools/config/SelfConfig.h"
57// For RNG
58#include "ompl/util/RandomNumbers.h"
59// For geometric equations like unitNBallMeasure
60#include "ompl/util/GeometricEquations.h"
61
62// BIT*:
63// A collection of common helper functions
64#include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
65// The vertex class:
66#include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
67// The cost-helper class:
68#include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
69// The search queue class
70#include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
71
72// Debug macros
73#ifdef BITSTAR_DEBUG
75#define ASSERT_SETUP this->assertSetup();
76#else
77#define ASSERT_SETUP
78#endif // BITSTAR_DEBUG
79
80namespace ompl
81{
82 namespace geometric
83 {
85 // Public functions:
87 : nameFunc_(std::move(nameFunc)), approximationId_(std::make_shared<unsigned int>(1u))
88 {
89 }
90
92 const ompl::base::ProblemDefinitionPtr &problemDefinition,
93 CostHelper *costHelper, SearchQueue *searchQueue,
94 const ompl::base::Planner *plannerPtr,
96 {
97 // Store that I am setup so that any debug-level tests will pass. This requires assuring that this function
98 // is ordered properly.
99 isSetup_ = true;
100
101 // Store arguments
102 spaceInformation_ = spaceInformation;
103 problemDefinition_ = problemDefinition;
104 costHelpPtr_ = costHelper;
105 queuePtr_ = searchQueue;
106
107 // Configure the nearest-neighbour constructs.
108 // Only allocate if they are empty (as they can be set to a specific version by a call to
109 // setNearestNeighbors)
110 if (!static_cast<bool>(samples_))
111 {
112 samples_.reset(ompl::tools::SelfConfig::getDefaultNearestNeighbors<VertexPtr>(plannerPtr));
113 }
114 // No else, already allocated (by a call to setNearestNeighbors())
115
116 // Configure:
118 [this](const VertexConstPtr &a, const VertexConstPtr &b) { return distance(a, b); });
119 samples_->setDistanceFunction(distanceFunction);
120
121 // Set the min, max and sampled cost to the proper objective-based values:
122 minCost_ = costHelpPtr_->infiniteCost();
123 solutionCost_ = costHelpPtr_->infiniteCost();
124 sampledCost_ = costHelpPtr_->infiniteCost();
125
126 // Add any start and goals vertices that exist to the queue, but do NOT wait for any more goals:
127 this->updateStartAndGoalStates(inputStates, ompl::base::plannerAlwaysTerminatingCondition());
128
129 // Get the measure of the problem
130 approximationMeasure_ = spaceInformation_->getSpaceMeasure();
131
132 // Does the problem have finite boundaries?
133 if (!std::isfinite(approximationMeasure_))
134 {
135 // It does not, so let's estimate a measure of the planning problem.
136 // A not horrible place to start would be hypercube proportional to the distance between the start and
137 // goal. It's not *great*, but at least it sort of captures the order-of-magnitude of the problem.
138
139 // First, some asserts.
140 // Check that JIT sampling is on, which is required for infinite problems
141 if (!useJustInTimeSampling_)
142 {
143 throw ompl::Exception("For unbounded planning problems, just-in-time sampling must be enabled "
144 "before calling setup.");
145 }
146 // No else
147
148 // Check that we have a start and goal
149 if (startVertices_.empty() || goalVertices_.empty())
150 {
151 throw ompl::Exception("For unbounded planning problems, at least one start and one goal must exist "
152 "before calling setup.");
153 }
154 // No else
155
156 // Variables
157 // The maximum distance between start and goal:
158 double maxDist = 0.0;
159 // The scale on the maximum distance, i.e. the width of the hypercube is equal to this value times the
160 // distance between start and goal.
161 // This number is completely made up.
162 double distScale = 2.0;
163
164 // Find the max distance
165 for (const auto &startVertex : startVertices_)
166 {
167 for (const auto &goalVertex : goalVertices_)
168 {
169 maxDist =
170 std::max(maxDist, spaceInformation_->distance(startVertex->state(), goalVertex->state()));
171 }
172 }
173
174 // Calculate an estimate of the problem measure by (hyper)cubing the max distance
175 approximationMeasure_ = std::pow(distScale * maxDist, spaceInformation_->getStateDimension());
176 }
177 // No else, finite problem dimension
178
179 // Finally initialize the nearestNeighbour terms:
180 // Calculate the k-nearest constant
181 k_rgg_ = this->calculateMinimumRggK();
182
183 // Make the initial k all vertices:
184 k_ = startVertices_.size() + goalVertices_.size();
185
186 // Make the initial r infinity
187 r_ = std::numeric_limits<double>::infinity();
188 }
189
191 {
192 // Reset everything to the state of construction OTHER than planner name and settings/parameters
193 // Keep this in the order of the constructors for easy verification:
194
195 // Mark as cleared
196 isSetup_ = false;
197
198 // Pointers given at setup
199 spaceInformation_.reset();
200 problemDefinition_.reset();
201 costHelpPtr_ = nullptr;
202 queuePtr_ = nullptr;
203
204 // Sampling
205 rng_ = ompl::RNG();
206 sampler_.reset();
207
208 // Containers
209 startVertices_.clear();
210 goalVertices_.clear();
211 prunedStartVertices_.clear();
212 prunedGoalVertices_.clear();
213 newSamples_.clear();
214 recycledSamples_.clear();
215
216 // The set of samples
217 if (static_cast<bool>(samples_))
218 {
219 samples_->clear();
220 samples_.reset();
221 }
222 // No else, not allocated
223
224 // The various calculations and tracked values, same as in the header
225 numNewSamplesInCurrentBatch_ = 0u;
226 numUniformStates_ = 0u;
227 r_ = 0.0;
228 k_rgg_ = 0.0; // This is a double for better rounding later
229 k_ = 0u;
230
231 approximationMeasure_ = 0.0;
232 minCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
233 solutionCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
234 sampledCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
235 hasExactSolution_ = false;
236 closestVertexToGoal_.reset();
237 closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
238
239 // The planner property trackers:
240 numSamples_ = 0u;
241 numVertices_ = 0u;
242 numFreeStatesPruned_ = 0u;
243 numVerticesDisconnected_ = 0u;
244 numNearestNeighbours_ = 0u;
245 numStateCollisionChecks_ = 0u;
246
247 // The approximation id.
248 *approximationId_ = 1u;
249
250 // The lookups and white-/blacklists of the start vertices.
251 for (const auto &vertex : startVertices_)
252 {
253 vertex->clearEdgeQueueInLookup();
254 vertex->clearEdgeQueueOutLookup();
255 vertex->clearBlacklist();
256 vertex->clearWhitelist();
257 }
258
259 // The lookups and white-/blacklists of the goal vertices.
260 for (const auto &vertex : goalVertices_)
261 {
262 vertex->clearEdgeQueueInLookup();
263 vertex->clearEdgeQueueOutLookup();
264 vertex->clearBlacklist();
265 vertex->clearWhitelist();
266 }
267
268 // The various convenience pointers:
269 // DO NOT reset the parameters:
270 // rewireFactor_
271 // useKNearest_
272 // useJustInTimeSampling_
273 // dropSamplesOnPrune_
274 // findApprox_
275 }
276
278 {
279 ASSERT_SETUP
280
281#ifdef BITSTAR_DEBUG
282 if (!static_cast<bool>(a->state()))
283 {
284 throw ompl::Exception("a->state is unallocated");
285 }
286 if (!static_cast<bool>(b->state()))
287 {
288 throw ompl::Exception("b->state is unallocated");
289 }
290#endif // BITSTAR_DEBUG
291
292 // Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other
293 // neighbours in the structure.
294 // The distance function between two states
295 return spaceInformation_->distance(b->state(), a->state());
296 }
297
299 {
300 return this->distance(vertices.first, vertices.second);
301 }
302
304 {
305 ASSERT_SETUP
306
307 // Make sure sampling has happened first.
308 this->updateSamples(vertex);
309
310 // Keep track of how many times we've requested nearest neighbours.
311 ++numNearestNeighbours_;
312
313 if (useKNearest_)
314 {
315 samples_->nearestK(vertex, k_, *neighbourSamples);
316 }
317 else
318 {
319 samples_->nearestR(vertex, r_, *neighbourSamples);
320 }
321 }
322
324 {
325 ASSERT_SETUP
326
327 // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as long
328 // as the program lives.
329 static std::set<std::shared_ptr<Vertex>,
330 std::function<bool(const std::shared_ptr<Vertex> &, const std::shared_ptr<Vertex> &)>>
331 liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
332
333 // Add samples
334 if (static_cast<bool>(samples_))
335 {
336 // Get the samples as a vector.
337 VertexPtrVector samples;
338 samples_->list(samples);
339
340 // Iterate through the samples.
341 for (const auto &sample : samples)
342 {
343 // Make sure the sample is not destructed before BIT* is.
344 liveStates.insert(sample);
345
346 // Add sample.
347 if (!sample->isRoot())
348 {
349 data.addVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
350
351 // Add incoming edge.
352 if (sample->hasParent())
353 {
354 data.addEdge(ompl::base::PlannerDataVertex(sample->getParent()->state(),
355 sample->getParent()->getId()),
356 ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
357 }
358 }
359 else
360 {
361 data.addStartVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
362 }
363 }
364 }
365 // No else.
366 }
367
369 {
370 ASSERT_SETUP
371
372 // We have a solution!
373 hasExactSolution_ = true;
374
375 // Store it's cost as the maximum we'd ever want to sample
376 solutionCost_ = solutionCost;
377
378 // Clear the approximate solution
379 closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
380 closestVertexToGoal_.reset();
381 }
382
384 ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
385 {
386 ASSERT_SETUP
387
388 // Variable
389 // Whether we've added a start or goal:
390 bool addedGoal = false;
391 bool addedStart = false;
392 /*
393 Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
394 samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
395 whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
396 but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
397 us to wait out the ptc and never try to solve anything)
398 */
399 do
400 {
401 // Variable
402 // A new goal pointer, if there are none, it will be a nullptr.
403 // We will wait for the duration of PTC for a new goal to appear.
404 const ompl::base::State *newGoal = inputStates.nextGoal(terminationCondition);
405
406 // Check if it's valid
407 if (static_cast<bool>(newGoal))
408 {
409 // Allocate the vertex pointer
410 goalVertices_.push_back(
411 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_));
412
413 // Copy the value into the state
414 spaceInformation_->copyState(goalVertices_.back()->state(), newGoal);
415
416 // And add this goal to the set of samples:
417 this->addToSamples(goalVertices_.back());
418
419 // Mark that we've added:
420 addedGoal = true;
421 }
422 // No else, there was no goal.
423 } while (inputStates.haveMoreGoalStates());
424
425 /*
426 And then do the same for starts. We do this last as the starts are added to the queue, which uses a
427 cost-to-go heuristic in it's ordering, and for that we want all the goals updated. As there is no way to
428 wait for new *start* states, this loop can be cleaner There is no need to rebuild the queue when we add
429 start vertices, as the queue is ordered on current cost-to-come, and adding a start doesn't change that.
430 */
431 while (inputStates.haveMoreStartStates())
432 {
433 // Variable
434 // A new start pointer, if PlannerInputStates finds that it is invalid we will get a nullptr.
435 const ompl::base::State *newStart = inputStates.nextStart();
436
437 // Check if it's valid
438 if (static_cast<bool>(newStart))
439 {
440 // Allocate the vertex pointer:
441 startVertices_.push_back(
442 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_, true));
443
444 // Copy the value into the state.
445 spaceInformation_->copyState(startVertices_.back()->state(), newStart);
446
447 // Add the vertex to the set of vertices.
448 this->addToSamples(startVertices_.back());
449 this->registerAsVertex(startVertices_.back());
450
451 // Mark that we've added:
452 addedStart = true;
453 }
454 // No else, there was no start.
455 }
456
457 // Now, if we added a new start and have previously pruned goals, we may want to readd them.
458 if (addedStart && !prunedGoalVertices_.empty())
459 {
460 // Variable
461 // An iterator to the vector of pruned goals
462 auto prunedGoalIter = prunedGoalVertices_.begin();
463 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
464 // iterator towards the start, and then erasing once at the end.
465 auto prunedGoalEnd = prunedGoalVertices_.end();
466
467 // Consider each one
468 while (prunedGoalIter != prunedGoalEnd)
469 {
470 // Mark as unpruned
471 (*prunedGoalIter)->markUnpruned();
472
473 // Check if it should be readded (i.e., would it be pruned *now*?)
474 if (this->canVertexBeDisconnected(*prunedGoalIter))
475 {
476 // It would be pruned, so remark as pruned
477 (*prunedGoalIter)->markPruned();
478
479 // and move onto the next:
480 ++prunedGoalIter;
481 }
482 else
483 {
484 // It would not be pruned now, so readd it!
485 // Add back to the vector:
486 goalVertices_.push_back(*prunedGoalIter);
487
488 // Add as a sample
489 this->addToSamples(*prunedGoalIter);
490
491 // Mark what we've added:
492 addedGoal = true;
493
494 // Remove this goal from the vector of pruned vertices.
495 // Swap it to the element before our *new* end
496 if (prunedGoalIter != (prunedGoalEnd - 1))
497 {
498 std::swap(*prunedGoalIter, *(prunedGoalEnd - 1));
499 }
500
501 // Move the end forward by one, marking it to be deleted
502 --prunedGoalEnd;
503
504 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
505 // back
506 }
507 }
508
509 // Erase any elements moved to the "new end" of the vector
510 if (prunedGoalEnd != prunedGoalVertices_.end())
511 {
512 prunedGoalVertices_.erase(prunedGoalEnd, prunedGoalVertices_.end());
513 }
514 // No else, nothing to delete
515 }
516
517 // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
518 if (addedGoal && !prunedStartVertices_.empty())
519 {
520 // Variable
521 // An iterator to the vector of pruned starts
522 auto prunedStartIter = prunedStartVertices_.begin();
523 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
524 // iterator towards the start, and then erasing once at the end.
525 auto prunedStartEnd = prunedStartVertices_.end();
526
527 // Consider each one
528 while (prunedStartIter != prunedStartEnd)
529 {
530 // Mark as unpruned
531 (*prunedStartIter)->markUnpruned();
532
533 // Check if it should be readded (i.e., would it be pruned *now*?)
534 if (this->canVertexBeDisconnected(*prunedStartIter))
535 {
536 // It would be pruned, so remark as pruned
537 (*prunedStartIter)->markPruned();
538
539 // and move onto the next:
540 ++prunedStartIter;
541 }
542 else
543 {
544 // It would not be pruned, readd it!
545 // Add it back to the vector
546 startVertices_.push_back(*prunedStartIter);
547
548 // Add this vertex to the queue.
549 // queuePtr_->enqueueVertex(*prunedStartIter);
550
551 // Add the vertex to the set of vertices.
552 this->registerAsVertex(*prunedStartIter);
553
554 // Mark what we've added:
555 addedStart = true;
556
557 // Remove this start from the vector of pruned vertices.
558 // Swap it to the element before our *new* end
559 if (prunedStartIter != (prunedStartEnd - 1))
560 {
561 std::swap(*prunedStartIter, *(prunedStartEnd - 1));
562 }
563
564 // Move the end forward by one, marking it to be deleted
565 --prunedStartEnd;
566
567 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
568 // back
569 }
570 }
571
572 // Erase any elements moved to the "new end" of the vector
573 if (prunedStartEnd != prunedStartVertices_.end())
574 {
575 prunedStartVertices_.erase(prunedStartEnd, prunedStartVertices_.end());
576 }
577 // No else, nothing to delete
578 }
579
580 // If we've added anything, we have some updating to do.
581 if (addedGoal || addedStart)
582 {
583 // Update the minimum cost
584 for (const auto &startVertex : startVertices_)
585 {
586 // Take the better of the min cost so far and the cost-to-go from this start
587 minCost_ = costHelpPtr_->betterCost(minCost_, costHelpPtr_->costToGoHeuristic(startVertex));
588 }
589
590 // If we have at least one start and goal, allocate a sampler
591 if (!startVertices_.empty() && !goalVertices_.empty())
592 {
593 // There is a start and goal, allocate
594 sampler_ = costHelpPtr_->getOptObj()->allocInformedStateSampler(
595 problemDefinition_, std::numeric_limits<unsigned int>::max());
596 }
597 // No else, this will get allocated when we get the updated start/goal.
598
599 // Iterate through the existing vertices and find the current best approximate solution (if enabled)
600 if (!hasExactSolution_ && findApprox_)
601 {
602 this->updateVertexClosestToGoal();
603 }
604 }
605 // No else, why were we called?
606
607 // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
608 if (!goalVertices_.empty() && startVertices_.empty())
609 {
610 OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
611 "since PlannerInputStates provides no method to wait for a valid _start_ state to appear.",
612 nameFunc_().c_str());
613 }
614 // No else
615 }
616
617 void BITstar::ImplicitGraph::addNewSamples(const unsigned int &numSamples)
618 {
619 ASSERT_SETUP
620
621 // Set the cost sampled to the minimum
622 sampledCost_ = minCost_;
623
624 // Store the number of samples being used in this batch
625 numNewSamplesInCurrentBatch_ = numSamples;
626
627 // Update the nearest-neighbour terms for the number of samples we *will* have.
628 this->updateNearestTerms();
629
630 // Add the recycled samples to the nearest neighbours struct.
631 samples_->add(recycledSamples_);
632
633 // These recycled samples are our only new samples.
634 newSamples_ = recycledSamples_;
635
636 // And there are no longer and recycled samples.
637 recycledSamples_.clear();
638
639 // Increment the approximation id.
640 ++(*approximationId_);
641
642 // We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT
643 // sampling.
644 }
645
646 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::prune(double prunedMeasure)
647 {
648 ASSERT_SETUP
649
650#ifdef BITSTAR_DEBUG
651 if (!hasExactSolution_)
652 {
653 throw ompl::Exception("A graph cannot be pruned until a solution is found");
654 }
655#endif // BITSTAR_DEBUG
656
657 // Store the measure of the problem I'm approximating
658 approximationMeasure_ = prunedMeasure;
659
660 // Prune the starts/goals
661 std::pair<unsigned int, unsigned int> numPruned = this->pruneStartAndGoalVertices();
662
663 // Prune the samples.
664 numPruned = numPruned + this->pruneSamples();
665
666 return numPruned;
667 }
668
670 {
671 ASSERT_SETUP
672
673 // NO COUNTER. generated samples are counted at the sampler.
674
675 // Add to the vector of new samples
676 newSamples_.push_back(sample);
677
678 // Add to the NN structure:
679 samples_->add(sample);
680 }
681
683 {
684 ASSERT_SETUP
685
686 // NO COUNTER. generated samples are counted at the sampler.
687
688 // Add to the vector of new samples
689 newSamples_.insert(newSamples_.end(), samples.begin(), samples.end());
690
691 // Add to the NN structure:
692 samples_->add(samples);
693 }
694
696 {
697 ASSERT_SETUP
698
699 // Remove from the set of samples
700 samples_->remove(sample);
701 }
702
704 {
705 ASSERT_SETUP
706
707 // Variable:
708#ifdef BITSTAR_DEBUG
709 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
710 // own copy.
711 unsigned int initCount = sample.use_count();
712#endif // BITSTAR_DEBUG
713 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
714 // this function is given an element of the maintained set as the argument)
715 VertexPtr sampleCopy(sample);
716
717#ifdef BITSTAR_DEBUG
718 // Assert that the vertexToDelete took it's own copy
719 if (sampleCopy.use_count() <= initCount)
720 {
721 throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
722 "from taking it's own copy of the given shared pointer. See "
723 "https://bitbucket.org/ompl/ompl/issues/364/code-cleanup-breaking-bit");
724 }
725 if (sampleCopy->edgeQueueOutLookupSize() != 0u)
726 {
727 throw ompl::Exception("Encountered a sample with outgoing edges in the queue.");
728 }
729#endif // BITSTAR_DEBUG
730
731 // Remove all incoming edges from the search queue.
732 queuePtr_->removeInEdgesConnectedToVertexFromQueue(sampleCopy);
733
734 // Remove from the set of samples
735 samples_->remove(sampleCopy);
736
737 // Increment our counter
738 ++numFreeStatesPruned_;
739
740 // Mark the sample as pruned
741 sampleCopy->markPruned();
742 }
743
745 {
746 ASSERT_SETUP
747
748 recycledSamples_.push_back(sample);
749 }
750
752 {
753 ASSERT_SETUP
754#ifdef BITSTAR_DEBUG
755 // Make sure it's connected first, so that the queue gets updated properly.
756 // This is a day of debugging I'll never get back
757 if (!vertex->isInTree())
758 {
759 throw ompl::Exception("Vertices must be connected to the graph before adding");
760 }
761#endif // BITSTAR_DEBUG
762
763 // Increment the number of vertices added:
764 ++numVertices_;
765
766 // Update the nearest vertex to the goal (if tracking)
767 if (!hasExactSolution_ && findApprox_)
768 {
769 this->testClosestToGoal(vertex);
770 }
771 }
772
773 unsigned int BITstar::ImplicitGraph::removeFromVertices(const VertexPtr &vertex, bool moveToFree)
774 {
775 ASSERT_SETUP
776
777 // Variable:
778#ifdef BITSTAR_DEBUG
779 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
780 // own copy.
781 unsigned int initCount = vertex.use_count();
782#endif // BITSTAR_DEBUG
783 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
784 // this function is given an element of the maintained set as the argument)
785 VertexPtr vertexCopy(vertex);
786
787#ifdef BITSTAR_DEBUG
788 // Assert that the vertexToDelete took it's own copy
789 if (vertexCopy.use_count() <= initCount)
790 {
791 throw ompl::Exception("A code change has prevented ImplicitGraph::removeVertex() "
792 "from taking it's own copy of the given shared pointer. See "
793 "https://bitbucket.org/ompl/ompl/issues/364/code-cleanup-breaking-bit");
794 }
795#endif // BITSTAR_DEBUG
796
797 // Increment our counter
798 ++numVerticesDisconnected_;
799
800 // Remove from the nearest-neighbour structure
801 samples_->remove(vertexCopy);
802
803 // Add back as sample, if that would be beneficial
804 if (moveToFree && !this->canSampleBePruned(vertexCopy))
805 {
806 // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
807 // next batch.
808 recycledSamples_.push_back(vertexCopy);
809
810 // Return that the vertex was recycled
811 return 0u;
812 }
813 else
814 {
815 // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
816 // anything about the vertex.
817 vertexCopy->markPruned();
818
819 // Return that the vertex was completely pruned
820 return 1u;
821 }
822 }
823
824 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
825 {
826 ASSERT_SETUP
827
828 // Variable:
829#ifdef BITSTAR_DEBUG
830 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
831 // own copy.
832 unsigned int initCount = vertex.use_count();
833#endif // BITSTAR_DEBUG
834 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
835 // this function is given an element of the maintained set as the argument)
836 VertexPtr vertexCopy(vertex);
837
838#ifdef BITSTAR_DEBUG
839 // Assert that the vertexToDelete took it's own copy
840 if (vertexCopy.use_count() <= initCount)
841 {
842 throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
843 "from taking it's own copy of the given shared pointer. See "
844 "https://bitbucket.org/ompl/ompl/issues/364/code-cleanup-breaking-bit");
845 }
846#endif // BITSTAR_DEBUG
847
848 // Remove from the set of inconsistent vertices if the vertex is inconsistent.
849 if (!vertexCopy->isConsistent())
850 {
851 queuePtr_->removeFromInconsistentSet(vertexCopy);
852 }
853
854 // Disconnect from parent if necessary, not cascading cost updates.
855 if (vertexCopy->hasParent())
856 {
857 this->removeEdgeBetweenVertexAndParent(vertexCopy, false);
858 }
859
860 // Remove all children and let them know that their parent is pruned.
861 VertexPtrVector children;
862 vertexCopy->getChildren(&children);
863 for (const auto &child : children)
864 {
865 // Remove this edge.
866 vertexCopy->removeChild(child);
867 child->removeParent(false);
868
869 // If the child is inconsistent, it needs to be removed from the set of inconsistent vertices.
870 if (!child->isConsistent())
871 {
872 queuePtr_->removeFromInconsistentSet(child);
873 }
874
875 // If the child has outgoing edges in the queue, they need to be removed.
876 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(child);
877 }
878
879 // Remove any edges still in the queue.
880 queuePtr_->removeAllEdgesConnectedToVertexFromQueue(vertexCopy);
881
882 // Remove this vertex from the set of samples.
883 samples_->remove(vertexCopy);
884
885 // This state is now no longer considered a vertex, but could still be useful as sample.
886 if (this->canSampleBePruned(vertexCopy))
887 {
888 // It's not even useful as sample, mark it as pruned.
889 vertexCopy->markPruned();
890 return {0, 1}; // The vertex is actually removed.
891 }
892 else
893 {
894 // It is useful as sample and should be recycled.
895 recycleSample(vertexCopy);
896 return {1, 0}; // The vertex is only disconnected and recycled as sample.
897 }
898 }
899
900 void BITstar::ImplicitGraph::removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
901 {
902#ifdef BITSTAR_DEBUG
903 if (!child->hasParent())
904 {
905 throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
906 }
907#endif // BITSTAR_DEBUG
908
909 // Check if my parent has already been pruned. This can occur if we're cascading child disconnections.
910 if (!child->getParent()->isPruned())
911 {
912 // If not, remove myself from my parent's vector of children, not updating down-stream costs
913 child->getParent()->removeChild(child);
914 }
915
916 // Remove my parent link, cascading cost updates if requested:
917 child->removeParent(cascadeCostUpdates);
918 }
919
921
923 // Private functions:
924 void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
925 {
926 // The required cost to contain the neighbourhood of this vertex.
927 ompl::base::Cost requiredCost = this->calculateNeighbourhoodCost(vertex);
928
929 // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
930 if (costHelpPtr_->isCostBetterThan(sampledCost_, requiredCost))
931 {
932 // The total number of samples we wish to have.
933 unsigned int numRequiredSamples = 0u;
934
935 // Get the measure of what we're sampling.
936 if (useJustInTimeSampling_)
937 {
938 // Calculate the sample density given the number of samples per batch and the measure of this batch
939 // by assuming that this batch will fill the same measure as the previous.
940 double sampleDensity = static_cast<double>(numNewSamplesInCurrentBatch_) / approximationMeasure_;
941
942 // Convert that into the number of samples needed for this slice.
943 double numSamplesForSlice =
944 sampleDensity * sampler_->getInformedMeasure(sampledCost_, requiredCost);
945
946 // The integer of the double are definitely sampled
947 numRequiredSamples = numSamples_ + static_cast<unsigned int>(numSamplesForSlice);
948
949 // And the fractional part represents the probability of one more sample. I like being pedantic.
950 if (rng_.uniform01() <= (numSamplesForSlice - static_cast<double>(numRequiredSamples)))
951 {
952 // One more please.
953 ++numRequiredSamples;
954 }
955 // No else.
956 }
957 else
958 {
959 // We're generating all our samples in one batch. Do it to it.
960 numRequiredSamples = numSamples_ + numNewSamplesInCurrentBatch_;
961 }
962
963 // Actually generate the new samples
964 VertexPtrVector newStates{};
965 newStates.reserve(numRequiredSamples);
966 for (std::size_t tries = 0u;
967 tries < averageNumOfAllowedFailedAttemptsWhenSampling_ * numRequiredSamples &&
968 numSamples_ < numRequiredSamples;
969 ++tries)
970 {
971 // Variable
972 // The new state:
973 auto newState =
974 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_);
975
976 // Sample in the interval [costSampled_, costReqd):
977 if (sampler_->sampleUniform(newState->state(), sampledCost_, requiredCost))
978 {
979 // If the state is collision free, add it to the set of free states
980 ++numStateCollisionChecks_;
981 if (spaceInformation_->isValid(newState->state()))
982 {
983 newStates.push_back(newState);
984
985 // Update the number of uniformly distributed states
986 ++numUniformStates_;
987
988 // Update the number of sample
989 ++numSamples_;
990 }
991 // No else
992 }
993 }
994
995 // Add the new state as a sample.
996 this->addToSamples(newStates);
997
998 // Record the sampled cost space
999 sampledCost_ = requiredCost;
1000 }
1001 // No else, the samples are up to date
1002 }
1003
1004 void BITstar::ImplicitGraph::updateVertexClosestToGoal()
1005 {
1006 if (static_cast<bool>(samples_))
1007 {
1008 // Get all samples as a vector.
1009 VertexPtrVector samples;
1010 samples_->list(samples);
1011
1012 // Iterate through them testing which of the ones in the tree is the closest to the goal.
1013 for (const auto &sample : samples)
1014 {
1015 if (sample->isInTree())
1016 {
1017 this->testClosestToGoal(sample);
1018 }
1019 }
1020 }
1021 // No else, there are no vertices.
1022 }
1023
1024 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneStartAndGoalVertices()
1025 {
1026 // Variable
1027 // The number of starts/goals disconnected from the tree/pruned
1028 std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
1029
1030 // Are there superfluous starts to prune?
1031 if (startVertices_.size() > 1u)
1032 {
1033 // Yes, Iterate through the vector
1034
1035 // Variable
1036 // The iterator to the start:
1037 auto startIter = startVertices_.begin();
1038 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1039 // iterator towards the start, and then erasing once at the end.
1040 auto startEnd = startVertices_.end();
1041
1042 // Run until at the end:
1043 while (startIter != startEnd)
1044 {
1045 // Check if this start has met the criteria to be pruned
1046 if (this->canVertexBeDisconnected(*startIter))
1047 {
1048 // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1049 // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1050 // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1051 numPruned.second = numPruned.second + this->removeFromVertices(*startIter, false);
1052
1053 // Count as a disconnected vertex
1054 ++numPruned.first;
1055
1056 // Disconnect from parent if necessary, cascading cost updates.
1057 if ((*startIter)->hasParent())
1058 {
1059 this->removeEdgeBetweenVertexAndParent(*startIter, true);
1060 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*startIter);
1061 }
1062
1063 // // Remove it from the vertex queue.
1064 // queuePtr_->unqueueVertex(*startIter);
1065
1066 // Store the start vertex in the pruned vector, in case it later needs to be readded:
1067 prunedStartVertices_.push_back(*startIter);
1068
1069 // Remove this start from the vector.
1070 // Swap it to the element before our *new* end
1071 if (startIter != (startEnd - 1))
1072 {
1073 using std::swap; // Enable Koenig Lookup.
1074 swap(*startIter, *(startEnd - 1));
1075 }
1076
1077 // Move the end forward by one, marking it to be deleted
1078 --startEnd;
1079
1080 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1081 // back
1082 }
1083 else
1084 {
1085 // Still valid, move to the next one:
1086 ++startIter;
1087 }
1088 }
1089
1090 // Erase any elements moved to the "new end" of the vector
1091 if (startEnd != startVertices_.end())
1092 {
1093 startVertices_.erase(startEnd, startVertices_.end());
1094 }
1095 // No else, nothing to delete
1096 }
1097 // No else, can't prune 1 start.
1098
1099 // Are there superfluous goals to prune?
1100 if (goalVertices_.size() > 1u)
1101 {
1102 // Yes, Iterate through the vector
1103
1104 // Variable
1105 // The iterator to the start:
1106 auto goalIter = goalVertices_.begin();
1107 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1108 // iterator towards the start, and then erasing once at the end.
1109 auto goalEnd = goalVertices_.end();
1110
1111 // Run until at the end:
1112 while (goalIter != goalEnd)
1113 {
1114 // Check if this goal has met the criteria to be pruned
1115 if (this->canSampleBePruned(*goalIter))
1116 {
1117 // It has, remove the goal vertex completely
1118 // Check if this vertex is in the tree
1119 if ((*goalIter)->isInTree())
1120 {
1121 // Disconnect from parent if necessary, cascading cost updates.
1122 if ((*goalIter)->hasParent())
1123 {
1124 this->removeEdgeBetweenVertexAndParent(*goalIter, true);
1125 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*goalIter);
1126
1127 // If the goal is inconsistent, it needs to be removed from the set of inconsistent
1128 // vertices.
1129 if (!(*goalIter)->isConsistent())
1130 {
1131 queuePtr_->removeFromInconsistentSet(*goalIter);
1132 }
1133 }
1134
1135 // Remove it from the set of vertices, recycling if necessary.
1136 this->removeFromVertices(*goalIter, true);
1137
1138 // and as a vertex, allowing it to move to the set of samples.
1139 numPruned.second = numPruned.second + this->removeFromVertices(*goalIter, true);
1140
1141 // Count it as a disconnected vertex
1142 ++numPruned.first;
1143 }
1144 else
1145 {
1146 // It is not, so just it like a sample
1147 this->pruneSample(*goalIter);
1148
1149 // Count a pruned sample
1150 ++numPruned.second;
1151 }
1152
1153 // Store the start vertex in the pruned vector, in case it later needs to be readded:
1154 prunedGoalVertices_.push_back(*goalIter);
1155
1156 // Remove this goal from the vector.
1157 // Swap it to the element before our *new* end
1158 if (goalIter != (goalEnd - 1))
1159 {
1160 std::swap(*goalIter, *(goalEnd - 1));
1161 }
1162
1163 // Move the end forward by one, marking it to be deleted
1164 --goalEnd;
1165
1166 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1167 // back
1168 }
1169 else
1170 {
1171 // The goal is still valid, get the next
1172 ++goalIter;
1173 }
1174 }
1175
1176 // Erase any elements moved to the "new end" of the vector
1177 if (goalEnd != goalVertices_.end())
1178 {
1179 goalVertices_.erase(goalEnd, goalVertices_.end());
1180 }
1181 // No else, nothing to delete
1182 }
1183 // No else, can't prune 1 goal.
1184
1185 // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1186 // solution exists.
1187
1188 // Return the amount of work done
1189 return numPruned;
1190 }
1191
1192 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneSamples()
1193 {
1194 // The number of samples pruned in this pass:
1195 std::pair<unsigned int, unsigned int> numPruned{0u, 0u};
1196
1197 // Get the vector of samples.
1198 VertexPtrVector samples;
1199 samples_->list(samples);
1200
1201 // Are we dropping samples anytime we prune?
1202 if (dropSamplesOnPrune_)
1203 {
1204 std::size_t numFreeStatesPruned = 0u;
1205 for (const auto &sample : samples)
1206 {
1207 if (!sample->isInTree())
1208 {
1209 this->pruneSample(sample);
1210 ++numPruned.second;
1211 ++numFreeStatesPruned;
1212 }
1213 }
1214
1215 // Store the number of uniform samples.
1216 numUniformStates_ = 0u;
1217
1218 // Increase the global counter.
1219 numFreeStatesPruned_ += numFreeStatesPruned;
1220 }
1221 else
1222 {
1223 // Iterate through the vector and remove any samples that should not be in the graph.
1224 for (const auto &sample : samples)
1225 {
1226 if (sample->isInTree())
1227 {
1228 if (this->canVertexBeDisconnected(sample))
1229 {
1230 numPruned = numPruned + this->pruneVertex(sample);
1231 }
1232 }
1233 // Check if this state should be pruned.
1234 else if (this->canSampleBePruned(sample))
1235 {
1236 // It should, remove the sample from the NN structure.
1237 this->pruneSample(sample);
1238
1239 // Keep track of how many are pruned.
1240 ++numPruned.second;
1241 }
1242 // No else, keep sample.
1243 }
1244 }
1245
1246 return numPruned;
1247 }
1248
1250 {
1251 ASSERT_SETUP
1252
1253 // Threshold should always be g_t(x_g)
1254
1255 // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
1256 // case we're using an edge that is exactly optimally connected.
1257 // g_t(v) + h^(v) > g_t(x_g)?
1258 return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(vertex), solutionCost_);
1259 }
1260
1262 {
1263 ASSERT_SETUP
1264
1265#ifdef BITSTAR_DEBUG
1266 if (sample->isPruned())
1267 {
1268 throw ompl::Exception("Asking whether a pruned sample can be pruned.");
1269 }
1270#endif // BITSTAR_DEBUG
1271
1272 // Threshold should always be g_t(x_g)
1273 // Prune the unconnected sample if it could never be better of a better solution.
1274 // g^(v) + h^(v) >= g_t(x_g)?
1275 return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(sample),
1276 solutionCost_);
1277 }
1278
1279 void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &vertex)
1280 {
1281 // Find the shortest distance between the given vertex and a goal
1282 double distance = std::numeric_limits<double>::max();
1283 problemDefinition_->getGoal()->isSatisfied(vertex->state(), &distance);
1284
1285 // Compare to the current best approximate solution
1286 if (distance < closestDistanceToGoal_)
1287 {
1288 // Better, update the approximate solution.
1289 closestVertexToGoal_ = vertex;
1290 closestDistanceToGoal_ = distance;
1291 }
1292 // No else, don't update if worse.
1293 }
1294
1295 ompl::base::Cost BITstar::ImplicitGraph::calculateNeighbourhoodCost(const VertexConstPtr &vertex) const
1296 {
1297#ifdef BITSTAR_DEBUG
1298 if (vertex->isPruned())
1299 {
1300 throw ompl::Exception("Calculating the neighbourhood cost of a pruned vertex.");
1301 }
1302#endif // BITSTAR_DEBUG
1303 // Are we using JIT sampling?
1304 if (useJustInTimeSampling_)
1305 {
1306 // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1307 // vertex.
1308 // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1309 // this vertex's neighbourhood.
1310 return costHelpPtr_->betterCost(
1311 solutionCost_, costHelpPtr_->combineCosts(costHelpPtr_->lowerBoundHeuristicVertex(vertex),
1312 ompl::base::Cost(2.0 * r_)));
1313 }
1314
1315 // We are not, return the maximum cost we'd ever want to sample
1316 return solutionCost_;
1317 }
1318
1319 void BITstar::ImplicitGraph::updateNearestTerms()
1320 {
1321 // The number of uniformly distributed states.
1322 unsigned int numUniformStates = 0u;
1323
1324 // Calculate N, are we dropping samples?
1325 if (dropSamplesOnPrune_)
1326 {
1327 // We are, so we've been tracking the number of uniform states, just use that
1328 numUniformStates = numUniformStates_;
1329 }
1330 else if (isPruningEnabled_)
1331 {
1332 // We are not dropping samples but pruning is enabled, then all samples are uniform.
1333 numUniformStates = samples_->size();
1334 }
1335 else
1336 {
1337 // We don't prune, so we have to check how many samples are in the informed set.
1338 numUniformStates = computeNumberOfSamplesInInformedSet();
1339 }
1340
1341 // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1342 // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1343 // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1344 // the radius, which RRT* does with it's min(maxEdgeLength, RGG-radius).
1345 if (numUniformStates == (startVertices_.size() + goalVertices_.size()))
1346 {
1347 numUniformStates = numUniformStates + numNewSamplesInCurrentBatch_;
1348 }
1349
1350 // Now update the appropriate term
1351 if (useKNearest_)
1352 {
1353 k_ = this->calculateK(numUniformStates);
1354 }
1355 else
1356 {
1357 r_ = this->calculateR(numUniformStates);
1358 }
1359 }
1360
1361 std::size_t BITstar::ImplicitGraph::computeNumberOfSamplesInInformedSet() const
1362 {
1363 // Get the samples as a vector.
1364 std::vector<VertexPtr> samples;
1365 samples_->list(samples);
1366
1367 // Return the number of samples that can not be pruned.
1368 return std::count_if(samples.begin(), samples.end(),
1369 [this](const VertexPtr &sample) { return !canSampleBePruned(sample); });
1370 }
1371
1372 double BITstar::ImplicitGraph::calculateR(unsigned int numUniformSamples) const
1373 {
1374 // Cast to double for readability. (?)
1375 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1376 auto graphCardinality = static_cast<double>(numUniformSamples);
1377
1378 // Calculate the term and return.
1379 return rewireFactor_ * this->calculateMinimumRggR() *
1380 std::pow(std::log(graphCardinality) / graphCardinality, 1.0 / stateDimension);
1381 }
1382
1383 unsigned int BITstar::ImplicitGraph::calculateK(unsigned int numUniformSamples) const
1384 {
1385 // Calculate the term and return
1386 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numUniformSamples)));
1387 }
1388
1389 double BITstar::ImplicitGraph::calculateMinimumRggR() const
1390 {
1391 // Variables
1392 // The dimension cast as a double for readibility;
1393 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1394
1395 // Calculate the term and return
1396 // RRT*
1397 return std::pow(2.0 * (1.0 + 1.0 / stateDimension) *
1398 (approximationMeasure_ / unitNBallMeasure(spaceInformation_->getStateDimension())),
1399 1.0 / stateDimension);
1400
1401 // Relevant work on calculating the minimum radius:
1402 /*
1403 // PRM*,RRG radius (biggest for unit-volume problem)
1404 // See Theorem 34 in Karaman & Frazzoli IJRR 2011
1405 return 2.0 * std::pow((1.0 + 1.0 / dimDbl) *
1406 (approximationMeasure_ /
1407 unitNBallMeasure(si_->getStateDimension())),
1408 1.0 / dimDbl);
1409
1410 // FMT* radius (R2: smallest, R3: equiv to RRT*, Higher d: middle).
1411 // See Theorem 4.1 in Janson et al. IJRR 2015
1412 return 2.0 * std::pow((1.0 / dimDbl) *
1413 (approximationMeasure_ /
1414 unitNBallMeasure(si_->getStateDimension())),
1415 1.0 / dimDbl);
1416
1417 // RRT* radius (smallest for unit-volume problems above R3).
1418 // See Theorem 38 in Karaman & Frazzoli IJRR 2011
1419 return std::pow(2.0 * (1.0 + 1.0 / dimDbl) *
1420 (approximationMeasure_ /
1421 unitNBallMeasure(si_->getStateDimension())),
1422 1.0 / dimDbl);
1423 */
1424 }
1425
1426 double BITstar::ImplicitGraph::calculateMinimumRggK() const
1427 {
1428 // The dimension cast as a double for readibility.
1429 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1430
1431 // Calculate the term and return.
1432 return boost::math::constants::e<double>() +
1433 (boost::math::constants::e<double>() / stateDimension); // RRG k-nearest
1434 }
1435
1436 void BITstar::ImplicitGraph::assertSetup() const
1437 {
1438 if (!isSetup_)
1439 {
1440 throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1441 }
1442 }
1444
1446 // Boring sets/gets (Public):
1448 {
1449 return (!startVertices_.empty());
1450 }
1451
1453 {
1454 return (!goalVertices_.empty());
1455 }
1456
1457 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1458 {
1459 return startVertices_.cbegin();
1460 }
1461
1462 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesEndConst() const
1463 {
1464 return startVertices_.cend();
1465 }
1466
1467 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1468 {
1469 return goalVertices_.cbegin();
1470 }
1471
1472 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesEndConst() const
1473 {
1474 return goalVertices_.cend();
1475 }
1476
1478 {
1479 return minCost_;
1480 }
1481
1483 {
1484 return sampler_->hasInformedMeasure();
1485 }
1486
1488 {
1489 return sampler_->getInformedMeasure(cost);
1490 }
1491
1493 {
1494#ifdef BITSTAR_DEBUG
1495 if (!findApprox_)
1496 {
1497 throw ompl::Exception("Approximate solutions are not being tracked.");
1498 }
1499#endif // BITSTAR_DEBUG
1500 return closestVertexToGoal_;
1501 }
1502
1504 {
1505#ifdef BITSTAR_DEBUG
1506 if (!findApprox_)
1507 {
1508 throw ompl::Exception("Approximate solutions are not being tracked.");
1509 }
1510#endif // BITSTAR_DEBUG
1511 return closestDistanceToGoal_;
1512 }
1513
1515 {
1516#ifdef BITSTAR_DEBUG
1517 if (!useKNearest_)
1518 {
1519 throw ompl::Exception("Using an r-disc graph.");
1520 }
1521#endif // BITSTAR_DEBUG
1522 return k_;
1523 }
1524
1526 {
1527#ifdef BITSTAR_DEBUG
1528 if (useKNearest_)
1529 {
1530 throw ompl::Exception("Using a k-nearest graph.");
1531 }
1532#endif // BITSTAR_DEBUG
1533 return r_;
1534 }
1535
1537 {
1538 VertexPtrVector samples;
1539 samples_->list(samples);
1540 return samples;
1541 }
1542
1544 {
1545 // Store
1546 rewireFactor_ = rewireFactor;
1547
1548 // Check if there's things to update
1549 if (isSetup_)
1550 {
1551 // Reinitialize the terms:
1552 this->updateNearestTerms();
1553 }
1554 }
1555
1557 {
1558 return rewireFactor_;
1559 }
1560
1562 {
1563 // Assure that we're not trying to enable k-nearest with JIT sampling already on
1564 if (useKNearest && useJustInTimeSampling_)
1565 {
1566 OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1567 "continuing to use the r-disc variant.",
1568 nameFunc_().c_str());
1569 }
1570 else
1571 {
1572 // Store
1573 useKNearest_ = useKNearest;
1574
1575 // Check if there's things to update
1576 if (isSetup_)
1577 {
1578 // Calculate the current term:
1579 this->updateNearestTerms();
1580 }
1581 }
1582 }
1583
1585 {
1586 return useKNearest_;
1587 }
1588
1590 {
1591 // Assure that we're not trying to enable k-nearest with JIT sampling already on
1592 if (useKNearest_ && useJit)
1593 {
1594 OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1595 "BIT*, continuing to use regular sampling.",
1596 nameFunc_().c_str());
1597 }
1598 else
1599 {
1600 // Store
1601 useJustInTimeSampling_ = useJit;
1602
1603 // Announce limitation:
1604 if (useJit)
1605 {
1606 OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1607 "seeking to minimize path-length.",
1608 nameFunc_().c_str());
1609 }
1610 // No else
1611 }
1612 }
1613
1615 {
1616 return useJustInTimeSampling_;
1617 }
1618
1620 {
1621 // Make sure we're not already setup
1622 if (isSetup_)
1623 {
1624 OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1625 "Continuing to use the previous setting.",
1626 nameFunc_().c_str());
1627 }
1628 else
1629 {
1630 // Store
1631 dropSamplesOnPrune_ = dropSamples;
1632 }
1633 }
1634
1636 {
1637 isPruningEnabled_ = usePruning;
1638 }
1639
1641 {
1642 return dropSamplesOnPrune_;
1643 }
1644
1646 {
1647 // Is the flag changing?
1648 if (findApproximate != findApprox_)
1649 {
1650 // Store the flag
1651 findApprox_ = findApproximate;
1652
1653 // Check if we are enabling or disabling approximate solution support
1654 if (!findApprox_)
1655 {
1656 // We're turning it off, clear the approximate solution variables:
1657 closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
1658 closestVertexToGoal_.reset();
1659 }
1660 else
1661 {
1662 // We are turning it on, do we have an exact solution?
1663 if (!hasExactSolution_)
1664 {
1665 // We don't, find our current best approximate solution:
1666 this->updateVertexClosestToGoal();
1667 }
1668 // No else, exact is better than approximate.
1669 }
1670 }
1671 // No else, no change.
1672 }
1673
1675 {
1676 return findApprox_;
1677 }
1678
1680 {
1681 averageNumOfAllowedFailedAttemptsWhenSampling_ = number;
1682 }
1683
1685 {
1686 return averageNumOfAllowedFailedAttemptsWhenSampling_;
1687 }
1688
1689 template <template <typename T> class NN>
1691 {
1692 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1693 // change them:
1694 if (isSetup_)
1695 {
1696 OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1697 "is setup. Continuing to use the existing containers.",
1698 nameFunc_().c_str());
1699 }
1700 else
1701 {
1702 // The problem isn't setup yet, create NN structs of the specified type:
1703 samples_ = std::make_shared<NN<VertexPtr>>();
1704 }
1705 }
1706
1708 {
1709 return samples_->size();
1710 }
1711
1713 {
1714 VertexPtrVector samples;
1715 samples_->list(samples);
1716 return std::count_if(samples.begin(), samples.end(),
1717 [](const VertexPtr &sample) { return sample->isInTree(); });
1718 }
1719
1721 {
1722 return numSamples_;
1723 }
1724
1726 {
1727 return numVertices_;
1728 }
1729
1731 {
1732 return numFreeStatesPruned_;
1733 }
1734
1736 {
1737 return numVerticesDisconnected_;
1738 }
1739
1741 {
1742 return numNearestNeighbours_;
1743 }
1744
1746 {
1747 return numStateCollisionChecks_;
1748 }
1750 } // namespace geometric
1751} // namespace ompl
The exception type for ompl.
Definition Exception.h:47
std::function< double(const _T &, const _T &)> DistanceFunction
The definition of a distance function.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition Planner.h:78
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition Planner.cpp:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:237
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:355
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition Planner.cpp:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:223
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...
void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
unsigned int numStatesGenerated() const
The total number of states generated.
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
void reset()
Reset the graph to the state of construction.
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
bool hasAGoal() const
Gets whether the graph contains a goal or not.
unsigned int numVertices() const
The number of vertices in the search tree.
unsigned int numStateCollisionChecks() const
The number of state collision checks.
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
bool hasAStart() const
Gets whether the graph contains a start or not.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
unsigned int numSamples() const
The number of samples.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found and that the graph should be limited to the given heuristic value...
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected...
unsigned int numFreeStatesPruned() const
The number of states pruned.
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
double getRewireFactor() const
Get the rewiring scale factor.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &inputStates)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates,...
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
ImplicitGraph(NameFunc nameFunc)
Construct an implicit graph.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
unsigned int removeFromVertices(const VertexPtr &sample, bool moveToFree)
Remove a vertex from the tree, can optionally be allowed to move it to the set of unconnected samples...
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
A queue of edges, sorted according to a sort key.
Definition SearchQueue.h:65
void reset()
Reset the queue to the state of construction.
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex's cost or those of its descendants....
Definition Vertex.cpp:370
bool isConsistent() const
Whether the vertex is consistent.
Definition Vertex.cpp:484
bool hasParent() const
Returns whether this vertex has a parent.
Definition Vertex.cpp:169
void markPruned()
Mark the vertex as pruned.
Definition Vertex.cpp:524
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition Vertex.cpp:299
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition Vertex.cpp:176
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex's cost, and can update the descenden...
Definition Vertex.cpp:267
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition Vertex.cpp:783
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition Vertex.cpp:198
bool isPruned() const
Whether the vertex has been pruned.
Definition Vertex.cpp:489
ompl::base::State * state()
The state of a vertex as a pointer.
Definition Vertex.cpp:152
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition BITstar.h:137
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition BITstar.h:125
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition BITstar.h:119
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition BITstar.h:116
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition BITstar.h:149
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#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.
Main namespace. Contains everything in this library.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
STL namespace.