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 {
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 various convenience pointers:
251 // DO NOT reset the parameters:
252 // rewireFactor_
253 // useKNearest_
254 // useJustInTimeSampling_
255 // dropSamplesOnPrune_
256 // findApprox_
257 }
258
260 {
261 ASSERT_SETUP
262
263#ifdef BITSTAR_DEBUG
264 if (!static_cast<bool>(a->state()))
265 {
266 throw ompl::Exception("a->state is unallocated");
267 }
268 if (!static_cast<bool>(b->state()))
269 {
270 throw ompl::Exception("b->state is unallocated");
271 }
272#endif // BITSTAR_DEBUG
273
274 // Using RRTstar as an example, this order gives us the distance FROM the queried state TO the other
275 // neighbours in the structure.
276 // The distance function between two states
277 return spaceInformation_->distance(b->state(), a->state());
278 }
279
281 {
282 return this->distance(vertices.first, vertices.second);
283 }
284
286 {
287 ASSERT_SETUP
288
289 // Make sure sampling has happened first.
290 this->updateSamples(vertex);
291
292 // Keep track of how many times we've requested nearest neighbours.
293 ++numNearestNeighbours_;
294
295 if (useKNearest_)
296 {
297 samples_->nearestK(vertex, k_, *neighbourSamples);
298 }
299 else
300 {
301 samples_->nearestR(vertex, r_, *neighbourSamples);
302 }
303 }
304
306 {
307 ASSERT_SETUP
308
309 // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as long
310 // as the program lives.
311 static std::set<std::shared_ptr<Vertex>,
312 std::function<bool(const std::shared_ptr<Vertex> &, const std::shared_ptr<Vertex> &)>>
313 liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
314
315 // Add samples
316 if (static_cast<bool>(samples_))
317 {
318 // Get the samples as a vector.
319 VertexPtrVector samples;
320 samples_->list(samples);
321
322 // Iterate through the samples.
323 for (const auto &sample : samples)
324 {
325 // Make sure the sample is not destructed before BIT* is.
326 liveStates.insert(sample);
327
328 // Add sample.
329 if (!sample->isRoot())
330 {
331 data.addVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
332
333 // Add incoming edge.
334 if (sample->hasParent())
335 {
336 data.addEdge(ompl::base::PlannerDataVertex(sample->getParent()->state(),
337 sample->getParent()->getId()),
338 ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
339 }
340 }
341 else
342 {
343 data.addStartVertex(ompl::base::PlannerDataVertex(sample->state(), sample->getId()));
344 }
345 }
346 }
347 // No else.
348 }
349
351 {
352 ASSERT_SETUP
353
354 // We have a solution!
355 hasExactSolution_ = true;
356
357 // Store it's cost as the maximum we'd ever want to sample
358 solutionCost_ = solutionCost;
359
360 // Clear the approximate solution
361 closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
362 closestVertexToGoal_.reset();
363 }
364
366 ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
367 {
368 ASSERT_SETUP
369
370 // Variable
371 // Whether we've added a start or goal:
372 bool addedGoal = false;
373 bool addedStart = false;
374 /*
375 Add the new starts and goals to the vectors of said vertices. Do goals first, as they are only added as
376 samples. We do this as nested conditions so we always call nextGoal(ptc) at least once (regardless of
377 whether there are moreGoalStates or not) in case we have been given a non trivial PTC that wants us to wait,
378 but do *not* call it again if there are no more goals (as in the nontrivial PTC case, doing so would cause
379 us to wait out the ptc and never try to solve anything)
380 */
381 do
382 {
383 // Variable
384 // A new goal pointer, if there are none, it will be a nullptr.
385 // We will wait for the duration of PTC for a new goal to appear.
386 const ompl::base::State *newGoal = inputStates.nextGoal(terminationCondition);
387
388 // Check if it's valid
389 if (static_cast<bool>(newGoal))
390 {
391 // Allocate the vertex pointer
392 goalVertices_.push_back(
393 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_));
394
395 // Copy the value into the state
396 spaceInformation_->copyState(goalVertices_.back()->state(), newGoal);
397
398 // And add this goal to the set of samples:
399 this->addToSamples(goalVertices_.back());
400
401 // Mark that we've added:
402 addedGoal = true;
403 }
404 // No else, there was no goal.
405 } while (inputStates.haveMoreGoalStates());
406
407 /*
408 And then do the same for starts. We do this last as the starts are added to the queue, which uses a
409 cost-to-go heuristic in it's ordering, and for that we want all the goals updated. As there is no way to
410 wait for new *start* states, this loop can be cleaner There is no need to rebuild the queue when we add
411 start vertices, as the queue is ordered on current cost-to-come, and adding a start doesn't change that.
412 */
413 while (inputStates.haveMoreStartStates())
414 {
415 // Variable
416 // A new start pointer, if PlannerInputStates finds that it is invalid we will get a nullptr.
417 const ompl::base::State *newStart = inputStates.nextStart();
418
419 // Check if it's valid
420 if (static_cast<bool>(newStart))
421 {
422 // Allocate the vertex pointer:
423 startVertices_.push_back(
424 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_, true));
425
426 // Copy the value into the state.
427 spaceInformation_->copyState(startVertices_.back()->state(), newStart);
428
429 // Add the vertex to the set of vertices.
430 this->addToSamples(startVertices_.back());
431 this->registerAsVertex(startVertices_.back());
432
433 // Mark that we've added:
434 addedStart = true;
435 }
436 // No else, there was no start.
437 }
438
439 // Now, if we added a new start and have previously pruned goals, we may want to readd them.
440 if (addedStart && !prunedGoalVertices_.empty())
441 {
442 // Variable
443 // An iterator to the vector of pruned goals
444 auto prunedGoalIter = prunedGoalVertices_.begin();
445 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
446 // iterator towards the start, and then erasing once at the end.
447 auto prunedGoalEnd = prunedGoalVertices_.end();
448
449 // Consider each one
450 while (prunedGoalIter != prunedGoalEnd)
451 {
452 // Mark as unpruned
453 (*prunedGoalIter)->markUnpruned();
454
455 // Check if it should be readded (i.e., would it be pruned *now*?)
456 if (this->canVertexBeDisconnected(*prunedGoalIter))
457 {
458 // It would be pruned, so remark as pruned
459 (*prunedGoalIter)->markPruned();
460
461 // and move onto the next:
462 ++prunedGoalIter;
463 }
464 else
465 {
466 // It would not be pruned now, so readd it!
467 // Add back to the vector:
468 goalVertices_.push_back(*prunedGoalIter);
469
470 // Add as a sample
471 this->addToSamples(*prunedGoalIter);
472
473 // Mark what we've added:
474 addedGoal = true;
475
476 // Remove this goal from the vector of pruned vertices.
477 // Swap it to the element before our *new* end
478 if (prunedGoalIter != (prunedGoalEnd - 1))
479 {
480 std::swap(*prunedGoalIter, *(prunedGoalEnd - 1));
481 }
482
483 // Move the end forward by one, marking it to be deleted
484 --prunedGoalEnd;
485
486 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
487 // back
488 }
489 }
490
491 // Erase any elements moved to the "new end" of the vector
492 if (prunedGoalEnd != prunedGoalVertices_.end())
493 {
494 prunedGoalVertices_.erase(prunedGoalEnd, prunedGoalVertices_.end());
495 }
496 // No else, nothing to delete
497 }
498
499 // Similarly, if we added a goal and have previously pruned starts, we will have to do the same on those
500 if (addedGoal && !prunedStartVertices_.empty())
501 {
502 // Variable
503 // An iterator to the vector of pruned starts
504 auto prunedStartIter = prunedStartVertices_.begin();
505 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
506 // iterator towards the start, and then erasing once at the end.
507 auto prunedStartEnd = prunedStartVertices_.end();
508
509 // Consider each one
510 while (prunedStartIter != prunedStartEnd)
511 {
512 // Mark as unpruned
513 (*prunedStartIter)->markUnpruned();
514
515 // Check if it should be readded (i.e., would it be pruned *now*?)
516 if (this->canVertexBeDisconnected(*prunedStartIter))
517 {
518 // It would be pruned, so remark as pruned
519 (*prunedStartIter)->markPruned();
520
521 // and move onto the next:
522 ++prunedStartIter;
523 }
524 else
525 {
526 // It would not be pruned, readd it!
527 // Add it back to the vector
528 startVertices_.push_back(*prunedStartIter);
529
530 // Add this vertex to the queue.
531 // queuePtr_->enqueueVertex(*prunedStartIter);
532
533 // Add the vertex to the set of vertices.
534 this->registerAsVertex(*prunedStartIter);
535
536 // Mark what we've added:
537 addedStart = true;
538
539 // Remove this start from the vector of pruned vertices.
540 // Swap it to the element before our *new* end
541 if (prunedStartIter != (prunedStartEnd - 1))
542 {
543 std::swap(*prunedStartIter, *(prunedStartEnd - 1));
544 }
545
546 // Move the end forward by one, marking it to be deleted
547 --prunedStartEnd;
548
549 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
550 // back
551 }
552 }
553
554 // Erase any elements moved to the "new end" of the vector
555 if (prunedStartEnd != prunedStartVertices_.end())
556 {
557 prunedStartVertices_.erase(prunedStartEnd, prunedStartVertices_.end());
558 }
559 // No else, nothing to delete
560 }
561
562 // If we've added anything, we have some updating to do.
563 if (addedGoal || addedStart)
564 {
565 // Update the minimum cost
566 for (const auto &startVertex : startVertices_)
567 {
568 // Take the better of the min cost so far and the cost-to-go from this start
569 minCost_ = costHelpPtr_->betterCost(minCost_, costHelpPtr_->costToGoHeuristic(startVertex));
570 }
571
572 // If we have at least one start and goal, allocate a sampler
573 if (!startVertices_.empty() && !goalVertices_.empty())
574 {
575 // There is a start and goal, allocate
576 sampler_ = costHelpPtr_->getOptObj()->allocInformedStateSampler(
577 problemDefinition_, std::numeric_limits<unsigned int>::max());
578 }
579 // No else, this will get allocated when we get the updated start/goal.
580
581 // Iterate through the existing vertices and find the current best approximate solution (if enabled)
582 if (!hasExactSolution_ && findApprox_)
583 {
584 this->updateVertexClosestToGoal();
585 }
586 }
587 // No else, why were we called?
588
589 // Make sure that if we have a goal, we also have a start, since there's no way to wait for more *starts*
590 if (!goalVertices_.empty() && startVertices_.empty())
591 {
592 OMPL_WARN("%s (ImplicitGraph): The problem has a goal but not a start. BIT* cannot find a solution "
593 "since PlannerInputStates provides no method to wait for a valid _start_ state to appear.",
594 nameFunc_().c_str());
595 }
596 // No else
597 }
598
599 void BITstar::ImplicitGraph::addNewSamples(const unsigned int &numSamples)
600 {
601 ASSERT_SETUP
602
603 // Set the cost sampled to identity
604 sampledCost_ = costHelpPtr_->identityCost();
605
606 // Store the number of samples being used in this batch
607 numNewSamplesInCurrentBatch_ = numSamples;
608
609 // Update the nearest-neighbour terms for the number of samples we *will* have.
610 this->updateNearestTerms();
611
612 // Add the recycled samples to the nearest neighbours struct.
613 samples_->add(recycledSamples_);
614
615 // These recycled samples are our only new samples.
616 newSamples_ = recycledSamples_;
617
618 // And there are no longer and recycled samples.
619 recycledSamples_.clear();
620
621 // Increment the approximation id.
622 ++(*approximationId_);
623
624 // We don't add actual *new* samples until the next time "nearestSamples" is called. This is to support JIT
625 // sampling.
626 }
627
628 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::prune(double prunedMeasure)
629 {
630 ASSERT_SETUP
631
632#ifdef BITSTAR_DEBUG
633 if (!hasExactSolution_)
634 {
635 throw ompl::Exception("A graph cannot be pruned until a solution is found");
636 }
637#endif // BITSTAR_DEBUG
638
639 // Store the measure of the problem I'm approximating
640 approximationMeasure_ = prunedMeasure;
641
642 // Prune the starts/goals
643 std::pair<unsigned int, unsigned int> numPruned = this->pruneStartAndGoalVertices();
644
645 // Prune the samples.
646 numPruned = numPruned + this->pruneSamples();
647
648 return numPruned;
649 }
650
652 {
653 ASSERT_SETUP
654
655 // NO COUNTER. generated samples are counted at the sampler.
656
657 // Add to the vector of new samples
658 newSamples_.push_back(sample);
659
660 // Add to the NN structure:
661 samples_->add(sample);
662 }
663
665 {
666 ASSERT_SETUP
667
668 // NO COUNTER. generated samples are counted at the sampler.
669
670 // Add to the vector of new samples
671 newSamples_.insert(newSamples_.end(), samples.begin(), samples.end());
672
673 // Add to the NN structure:
674 samples_->add(samples);
675 }
676
678 {
679 ASSERT_SETUP
680
681 // Remove from the set of samples
682 samples_->remove(sample);
683 }
684
686 {
687 ASSERT_SETUP
688
689 // Variable:
690#ifdef BITSTAR_DEBUG
691 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
692 // own copy.
693 unsigned int initCount = sample.use_count();
694#endif // BITSTAR_DEBUG
695 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
696 // this function is given an element of the maintained set as the argument)
697 VertexPtr sampleCopy(sample);
698
699#ifdef BITSTAR_DEBUG
700 // Assert that the vertexToDelete took it's own copy
701 if (sampleCopy.use_count() <= initCount)
702 {
703 throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
704 "from taking it's own copy of the given shared pointer. See "
705 "https://github.com/ompl/ompl/issues/485");
706 }
707 if (sampleCopy->edgeQueueOutLookupSize() != 0u)
708 {
709 throw ompl::Exception("Encountered a sample with outgoing edges in the queue.");
710 }
711#endif // BITSTAR_DEBUG
712
713 // Remove all incoming edges from the search queue.
714 queuePtr_->removeInEdgesConnectedToVertexFromQueue(sampleCopy);
715
716 // Remove from the set of samples
717 samples_->remove(sampleCopy);
718
719 // Increment our counter
720 ++numFreeStatesPruned_;
721
722 // Mark the sample as pruned
723 sampleCopy->markPruned();
724 }
725
727 {
728 ASSERT_SETUP
729
730 recycledSamples_.push_back(sample);
731 }
732
734 {
735 ASSERT_SETUP
736#ifdef BITSTAR_DEBUG
737 // Make sure it's connected first, so that the queue gets updated properly.
738 // This is a day of debugging I'll never get back
739 if (!vertex->isInTree())
740 {
741 throw ompl::Exception("Vertices must be connected to the graph before adding");
742 }
743#endif // BITSTAR_DEBUG
744
745 // Increment the number of vertices added:
746 ++numVertices_;
747
748 // Update the nearest vertex to the goal (if tracking)
749 if (!hasExactSolution_ && findApprox_)
750 {
751 this->testClosestToGoal(vertex);
752 }
753 }
754
755 unsigned int BITstar::ImplicitGraph::removeFromVertices(const VertexPtr &vertex, bool moveToFree)
756 {
757 ASSERT_SETUP
758
759 // Variable:
760#ifdef BITSTAR_DEBUG
761 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
762 // own copy.
763 unsigned int initCount = vertex.use_count();
764#endif // BITSTAR_DEBUG
765 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
766 // this function is given an element of the maintained set as the argument)
767 VertexPtr vertexCopy(vertex);
768
769#ifdef BITSTAR_DEBUG
770 // Assert that the vertexToDelete took it's own copy
771 if (vertexCopy.use_count() <= initCount)
772 {
773 throw ompl::Exception("A code change has prevented ImplicitGraph::removeVertex() "
774 "from taking it's own copy of the given shared pointer. See "
775 "https://github.com/ompl/ompl/issues/485");
776 }
777#endif // BITSTAR_DEBUG
778
779 // Increment our counter
780 ++numVerticesDisconnected_;
781
782 // Remove from the nearest-neighbour structure
783 samples_->remove(vertexCopy);
784
785 // Add back as sample, if that would be beneficial
786 if (moveToFree && !this->canSampleBePruned(vertexCopy))
787 {
788 // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
789 // next batch.
790 recycledSamples_.push_back(vertexCopy);
791
792 // Return that the vertex was recycled
793 return 0u;
794 }
795 else
796 {
797 // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
798 // anything about the vertex.
799 vertexCopy->markPruned();
800
801 // Return that the vertex was completely pruned
802 return 1u;
803 }
804 }
805
806 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneVertex(const VertexPtr &vertex)
807 {
808 ASSERT_SETUP
809
810 // Variable:
811#ifdef BITSTAR_DEBUG
812 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our
813 // own copy.
814 unsigned int initCount = vertex.use_count();
815#endif // BITSTAR_DEBUG
816 // A copy of the sample pointer to be removed so we can't delete it out from under ourselves (occurs when
817 // this function is given an element of the maintained set as the argument)
818 VertexPtr vertexCopy(vertex);
819
820#ifdef BITSTAR_DEBUG
821 // Assert that the vertexToDelete took it's own copy
822 if (vertexCopy.use_count() <= initCount)
823 {
824 throw ompl::Exception("A code change has prevented ImplicitGraph::removeSample() "
825 "from taking it's own copy of the given shared pointer. See "
826 "https://github.com/ompl/ompl/issues/485");
827 }
828#endif // BITSTAR_DEBUG
829
830 // Remove from the set of inconsistent vertices if the vertex is inconsistent.
831 if (!vertexCopy->isConsistent())
832 {
833 queuePtr_->removeFromInconsistentSet(vertexCopy);
834 }
835
836 // Disconnect from parent if necessary, not cascading cost updates.
837 if (vertexCopy->hasParent())
838 {
839 this->removeEdgeBetweenVertexAndParent(vertexCopy, false);
840 }
841
842 // Remove all children and let them know that their parent is pruned.
843 VertexPtrVector children;
844 vertexCopy->getChildren(&children);
845 for (const auto &child : children)
846 {
847 // Remove this edge.
848 vertexCopy->removeChild(child);
849 child->removeParent(false);
850
851 // If the child is inconsistent, it needs to be removed from the set of inconsistent vertices.
852 if (!child->isConsistent())
853 {
854 queuePtr_->removeFromInconsistentSet(child);
855 }
856
857 // If the child has outgoing edges in the queue, they need to be removed.
858 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(child);
859 }
860
861 // Remove any edges still in the queue.
862 queuePtr_->removeAllEdgesConnectedToVertexFromQueue(vertexCopy);
863
864 // Remove this vertex from the set of samples.
865 samples_->remove(vertexCopy);
866
867 // This state is now no longer considered a vertex, but could still be useful as sample.
868 if (this->canSampleBePruned(vertexCopy))
869 {
870 // It's not even useful as sample, mark it as pruned.
871 vertexCopy->markPruned();
872 return {0, 1}; // The vertex is actually removed.
873 }
874 else
875 {
876 // It is useful as sample and should be recycled.
877 recycleSample(vertexCopy);
878 return {1, 0}; // The vertex is only disconnected and recycled as sample.
879 }
880 }
881
882 void BITstar::ImplicitGraph::removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
883 {
884#ifdef BITSTAR_DEBUG
885 if (!child->hasParent())
886 {
887 throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
888 }
889#endif // BITSTAR_DEBUG
890
891 // Check if my parent has already been pruned. This can occur if we're cascading child disconnections.
892 if (!child->getParent()->isPruned())
893 {
894 // If not, remove myself from my parent's vector of children, not updating down-stream costs
895 child->getParent()->removeChild(child);
896 }
897
898 // Remove my parent link, cascading cost updates if requested:
899 child->removeParent(cascadeCostUpdates);
900 }
901
903
905 // Private functions:
906 void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
907 {
908 // The required cost to contain the neighbourhood of this vertex.
909 ompl::base::Cost requiredCost = this->calculateNeighbourhoodCost(vertex);
910
911 // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
912 if (costHelpPtr_->isCostBetterThan(sampledCost_, requiredCost))
913 {
914 // The total number of samples we wish to have.
915 unsigned int numRequiredSamples = 0u;
916
917 // Get the measure of what we're sampling.
918 if (useJustInTimeSampling_)
919 {
920 // Calculate the sample density given the number of samples per batch and the measure of this batch
921 // by assuming that this batch will fill the same measure as the previous.
922 double sampleDensity = static_cast<double>(numNewSamplesInCurrentBatch_) / approximationMeasure_;
923
924 // Convert that into the number of samples needed for this slice.
925 double numSamplesForSlice =
926 sampleDensity * sampler_->getInformedMeasure(sampledCost_, requiredCost);
927
928 // The integer of the double are definitely sampled
929 numRequiredSamples = numSamples_ + static_cast<unsigned int>(numSamplesForSlice);
930
931 // And the fractional part represents the probability of one more sample. I like being pedantic.
932 if (rng_.uniform01() <= (numSamplesForSlice - static_cast<double>(numRequiredSamples)))
933 {
934 // One more please.
935 ++numRequiredSamples;
936 }
937 // No else.
938 }
939 else
940 {
941 // We're generating all our samples in one batch. Do it to it.
942 numRequiredSamples = numSamples_ + numNewSamplesInCurrentBatch_;
943 }
944
945 // Actually generate the new samples
946 VertexPtrVector newStates{};
947 newStates.reserve(numRequiredSamples);
948 for (std::size_t tries = 0u;
949 tries < averageNumOfAllowedFailedAttemptsWhenSampling_ * numRequiredSamples &&
950 numSamples_ < numRequiredSamples;
951 ++tries)
952 {
953 // Variable
954 // The new state:
955 auto newState =
956 std::make_shared<Vertex>(spaceInformation_, costHelpPtr_, queuePtr_, approximationId_);
957
958 // Sample in the interval [costSampled_, costReqd):
959 if (sampler_->sampleUniform(newState->state(), sampledCost_, requiredCost))
960 {
961 // If the state is collision free, add it to the set of free states
962 ++numStateCollisionChecks_;
963 if (spaceInformation_->isValid(newState->state()))
964 {
965 newStates.push_back(newState);
966
967 // Update the number of uniformly distributed states
968 ++numUniformStates_;
969
970 // Update the number of sample
971 ++numSamples_;
972 }
973 // No else
974 }
975 }
976
977 // Add the new state as a sample.
978 this->addToSamples(newStates);
979
980 // Record the sampled cost space
981 sampledCost_ = requiredCost;
982 }
983 // No else, the samples are up to date
984 }
985
986 void BITstar::ImplicitGraph::updateVertexClosestToGoal()
987 {
988 if (static_cast<bool>(samples_))
989 {
990 // Get all samples as a vector.
991 VertexPtrVector samples;
992 samples_->list(samples);
993
994 // Iterate through them testing which of the ones in the tree is the closest to the goal.
995 for (const auto &sample : samples)
996 {
997 if (sample->isInTree())
998 {
999 this->testClosestToGoal(sample);
1000 }
1001 }
1002 }
1003 // No else, there are no vertices.
1004 }
1005
1006 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneStartAndGoalVertices()
1007 {
1008 // Variable
1009 // The number of starts/goals disconnected from the tree/pruned
1010 std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
1011
1012 // Are there superfluous starts to prune?
1013 if (startVertices_.size() > 1u)
1014 {
1015 // Yes, Iterate through the vector
1016
1017 // Variable
1018 // The iterator to the start:
1019 auto startIter = startVertices_.begin();
1020 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1021 // iterator towards the start, and then erasing once at the end.
1022 auto startEnd = startVertices_.end();
1023
1024 // Run until at the end:
1025 while (startIter != startEnd)
1026 {
1027 // Check if this start has met the criteria to be pruned
1028 if (this->canVertexBeDisconnected(*startIter))
1029 {
1030 // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
1031 // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
1032 // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
1033 numPruned.second = numPruned.second + this->removeFromVertices(*startIter, false);
1034
1035 // Count as a disconnected vertex
1036 ++numPruned.first;
1037
1038 // Disconnect from parent if necessary, cascading cost updates.
1039 if ((*startIter)->hasParent())
1040 {
1041 this->removeEdgeBetweenVertexAndParent(*startIter, true);
1042 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*startIter);
1043 }
1044
1045 // // Remove it from the vertex queue.
1046 // queuePtr_->unqueueVertex(*startIter);
1047
1048 // Store the start vertex in the pruned vector, in case it later needs to be readded:
1049 prunedStartVertices_.push_back(*startIter);
1050
1051 // Remove this start from the vector.
1052 // Swap it to the element before our *new* end
1053 if (startIter != (startEnd - 1))
1054 {
1055 using std::swap; // Enable Koenig Lookup.
1056 swap(*startIter, *(startEnd - 1));
1057 }
1058
1059 // Move the end forward by one, marking it to be deleted
1060 --startEnd;
1061
1062 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1063 // back
1064 }
1065 else
1066 {
1067 // Still valid, move to the next one:
1068 ++startIter;
1069 }
1070 }
1071
1072 // Erase any elements moved to the "new end" of the vector
1073 if (startEnd != startVertices_.end())
1074 {
1075 startVertices_.erase(startEnd, startVertices_.end());
1076 }
1077 // No else, nothing to delete
1078 }
1079 // No else, can't prune 1 start.
1080
1081 // Are there superfluous goals to prune?
1082 if (goalVertices_.size() > 1u)
1083 {
1084 // Yes, Iterate through the vector
1085
1086 // Variable
1087 // The iterator to the start:
1088 auto goalIter = goalVertices_.begin();
1089 // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
1090 // iterator towards the start, and then erasing once at the end.
1091 auto goalEnd = goalVertices_.end();
1092
1093 // Run until at the end:
1094 while (goalIter != goalEnd)
1095 {
1096 // Check if this goal has met the criteria to be pruned
1097 if (this->canSampleBePruned(*goalIter))
1098 {
1099 // It has, remove the goal vertex completely
1100 // Check if this vertex is in the tree
1101 if ((*goalIter)->isInTree())
1102 {
1103 // Disconnect from parent if necessary, cascading cost updates.
1104 if ((*goalIter)->hasParent())
1105 {
1106 this->removeEdgeBetweenVertexAndParent(*goalIter, true);
1107 queuePtr_->removeOutEdgesConnectedToVertexFromQueue(*goalIter);
1108
1109 // If the goal is inconsistent, it needs to be removed from the set of inconsistent
1110 // vertices.
1111 if (!(*goalIter)->isConsistent())
1112 {
1113 queuePtr_->removeFromInconsistentSet(*goalIter);
1114 }
1115 }
1116
1117 // Remove it from the set of vertices, recycling if necessary.
1118 this->removeFromVertices(*goalIter, true);
1119
1120 // and as a vertex, allowing it to move to the set of samples.
1121 numPruned.second = numPruned.second + this->removeFromVertices(*goalIter, true);
1122
1123 // Count it as a disconnected vertex
1124 ++numPruned.first;
1125 }
1126 else
1127 {
1128 // It is not, so just it like a sample
1129 this->pruneSample(*goalIter);
1130
1131 // Count a pruned sample
1132 ++numPruned.second;
1133 }
1134
1135 // Store the start vertex in the pruned vector, in case it later needs to be readded:
1136 prunedGoalVertices_.push_back(*goalIter);
1137
1138 // Remove this goal from the vector.
1139 // Swap it to the element before our *new* end
1140 if (goalIter != (goalEnd - 1))
1141 {
1142 std::swap(*goalIter, *(goalEnd - 1));
1143 }
1144
1145 // Move the end forward by one, marking it to be deleted
1146 --goalEnd;
1147
1148 // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1149 // back
1150 }
1151 else
1152 {
1153 // The goal is still valid, get the next
1154 ++goalIter;
1155 }
1156 }
1157
1158 // Erase any elements moved to the "new end" of the vector
1159 if (goalEnd != goalVertices_.end())
1160 {
1161 goalVertices_.erase(goalEnd, goalVertices_.end());
1162 }
1163 // No else, nothing to delete
1164 }
1165 // No else, can't prune 1 goal.
1166
1167 // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1168 // solution exists.
1169
1170 // Return the amount of work done
1171 return numPruned;
1172 }
1173
1174 std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneSamples()
1175 {
1176 // The number of samples pruned in this pass:
1177 std::pair<unsigned int, unsigned int> numPruned{0u, 0u};
1178
1179 // Get the vector of samples.
1180 VertexPtrVector samples;
1181 samples_->list(samples);
1182
1183 // Are we dropping samples anytime we prune?
1184 if (dropSamplesOnPrune_)
1185 {
1186 std::size_t numFreeStatesPruned = 0u;
1187 for (const auto &sample : samples)
1188 {
1189 if (!sample->isInTree())
1190 {
1191 this->pruneSample(sample);
1192 ++numPruned.second;
1193 ++numFreeStatesPruned;
1194 }
1195 }
1196
1197 // Store the number of uniform samples.
1198 numUniformStates_ = 0u;
1199
1200 // Increase the global counter.
1201 numFreeStatesPruned_ += numFreeStatesPruned;
1202 }
1203 else
1204 {
1205 // Iterate through the vector and remove any samples that should not be in the graph.
1206 for (const auto &sample : samples)
1207 {
1208 if (sample->isInTree())
1209 {
1210 if (this->canVertexBeDisconnected(sample))
1211 {
1212 numPruned = numPruned + this->pruneVertex(sample);
1213 }
1214 }
1215 // Check if this state should be pruned.
1216 else if (this->canSampleBePruned(sample))
1217 {
1218 // It should, remove the sample from the NN structure.
1219 this->pruneSample(sample);
1220
1221 // Keep track of how many are pruned.
1222 ++numPruned.second;
1223 }
1224 // No else, keep sample.
1225 }
1226 }
1227
1228 return numPruned;
1229 }
1230
1232 {
1233 ASSERT_SETUP
1234
1235 // Threshold should always be g_t(x_g)
1236
1237 // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
1238 // case we're using an edge that is exactly optimally connected.
1239 // g_t(v) + h^(v) > g_t(x_g)?
1240 return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(vertex), solutionCost_);
1241 }
1242
1244 {
1245 ASSERT_SETUP
1246
1247#ifdef BITSTAR_DEBUG
1248 if (sample->isPruned())
1249 {
1250 throw ompl::Exception("Asking whether a pruned sample can be pruned.");
1251 }
1252#endif // BITSTAR_DEBUG
1253
1254 // Threshold should always be g_t(x_g)
1255 // Prune the unconnected sample if it could never be better of a better solution.
1256 // g^(v) + h^(v) >= g_t(x_g)?
1257 return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(sample),
1258 solutionCost_);
1259 }
1260
1261 void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &vertex)
1262 {
1263 // Find the shortest distance between the given vertex and a goal
1264 double distance = std::numeric_limits<double>::max();
1265 problemDefinition_->getGoal()->isSatisfied(vertex->state(), &distance);
1266
1267 // Compare to the current best approximate solution
1268 if (distance < closestDistanceToGoal_)
1269 {
1270 // Better, update the approximate solution.
1271 closestVertexToGoal_ = vertex;
1272 closestDistanceToGoal_ = distance;
1273 }
1274 // No else, don't update if worse.
1275 }
1276
1277 ompl::base::Cost BITstar::ImplicitGraph::calculateNeighbourhoodCost(const VertexConstPtr &vertex) const
1278 {
1279#ifdef BITSTAR_DEBUG
1280 if (vertex->isPruned())
1281 {
1282 throw ompl::Exception("Calculating the neighbourhood cost of a pruned vertex.");
1283 }
1284#endif // BITSTAR_DEBUG
1285 // Are we using JIT sampling?
1286 if (useJustInTimeSampling_)
1287 {
1288 // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1289 // vertex.
1290 // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1291 // this vertex's neighbourhood.
1292 return costHelpPtr_->betterCost(
1293 solutionCost_, costHelpPtr_->combineCosts(costHelpPtr_->lowerBoundHeuristicVertex(vertex),
1294 ompl::base::Cost(2.0 * r_)));
1295 }
1296
1297 // We are not, return the maximum cost we'd ever want to sample
1298 return solutionCost_;
1299 }
1300
1301 void BITstar::ImplicitGraph::updateNearestTerms()
1302 {
1303 // The number of uniformly distributed states.
1304 unsigned int numUniformStates = 0u;
1305
1306 // Calculate N, are we dropping samples?
1307 if (dropSamplesOnPrune_)
1308 {
1309 // We are, so we've been tracking the number of uniform states, just use that
1310 numUniformStates = numUniformStates_;
1311 }
1312 else if (isPruningEnabled_)
1313 {
1314 // We are not dropping samples but pruning is enabled, then all samples are uniform.
1315 numUniformStates = samples_->size();
1316 }
1317 else
1318 {
1319 // We don't prune, so we have to check how many samples are in the informed set.
1320 numUniformStates = computeNumberOfSamplesInInformedSet();
1321 }
1322
1323 // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1324 // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1325 // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1326 // the radius, which RRT* does with it's min(maxEdgeLength, RGG-radius).
1327 if (numUniformStates == (startVertices_.size() + goalVertices_.size()))
1328 {
1329 numUniformStates = numUniformStates + numNewSamplesInCurrentBatch_;
1330 }
1331
1332 // Now update the appropriate term
1333 if (useKNearest_)
1334 {
1335 k_ = this->calculateK(numUniformStates);
1336 }
1337 else
1338 {
1339 r_ = this->calculateR(numUniformStates);
1340 }
1341 }
1342
1343 std::size_t BITstar::ImplicitGraph::computeNumberOfSamplesInInformedSet() const
1344 {
1345 // Get the samples as a vector.
1346 std::vector<VertexPtr> samples;
1347 samples_->list(samples);
1348
1349 // Return the number of samples that can not be pruned.
1350 return std::count_if(samples.begin(), samples.end(),
1351 [this](const VertexPtr &sample) { return !canSampleBePruned(sample); });
1352 }
1353
1354 double BITstar::ImplicitGraph::calculateR(unsigned int numUniformSamples) const
1355 {
1356 // Cast to double for readability. (?)
1357 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1358 auto graphCardinality = static_cast<double>(numUniformSamples);
1359
1360 // Calculate the term and return.
1361 return rewireFactor_ * this->calculateMinimumRggR() *
1362 std::pow(std::log(graphCardinality) / graphCardinality, 1.0 / stateDimension);
1363 }
1364
1365 unsigned int BITstar::ImplicitGraph::calculateK(unsigned int numUniformSamples) const
1366 {
1367 // Calculate the term and return
1368 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numUniformSamples)));
1369 }
1370
1371 double BITstar::ImplicitGraph::calculateMinimumRggR() const
1372 {
1373 // Variables
1374 // The dimension cast as a double for readibility;
1375 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1376
1377 // Calculate the term and return
1378 // RRT*
1379 return std::pow(2.0 * (1.0 + 1.0 / stateDimension) *
1380 (approximationMeasure_ / unitNBallMeasure(spaceInformation_->getStateDimension())),
1381 1.0 / stateDimension);
1382
1383 // Relevant work on calculating the minimum radius:
1384 /*
1385 // PRM*,RRG radius (biggest for unit-volume problem)
1386 // See Theorem 34 in Karaman & Frazzoli IJRR 2011
1387 return 2.0 * std::pow((1.0 + 1.0 / dimDbl) *
1388 (approximationMeasure_ /
1389 unitNBallMeasure(si_->getStateDimension())),
1390 1.0 / dimDbl);
1391
1392 // FMT* radius (R2: smallest, R3: equiv to RRT*, Higher d: middle).
1393 // See Theorem 4.1 in Janson et al. IJRR 2015
1394 return 2.0 * std::pow((1.0 / dimDbl) *
1395 (approximationMeasure_ /
1396 unitNBallMeasure(si_->getStateDimension())),
1397 1.0 / dimDbl);
1398
1399 // RRT* radius (smallest for unit-volume problems above R3).
1400 // See Theorem 38 in Karaman & Frazzoli IJRR 2011
1401 return std::pow(2.0 * (1.0 + 1.0 / dimDbl) *
1402 (approximationMeasure_ /
1403 unitNBallMeasure(si_->getStateDimension())),
1404 1.0 / dimDbl);
1405 */
1406 }
1407
1408 double BITstar::ImplicitGraph::calculateMinimumRggK() const
1409 {
1410 // The dimension cast as a double for readibility.
1411 auto stateDimension = static_cast<double>(spaceInformation_->getStateDimension());
1412
1413 // Calculate the term and return.
1414 return boost::math::constants::e<double>() +
1415 (boost::math::constants::e<double>() / stateDimension); // RRG k-nearest
1416 }
1417
1418 void BITstar::ImplicitGraph::assertSetup() const
1419 {
1420 if (!isSetup_)
1421 {
1422 throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1423 }
1424 }
1426
1428 // Boring sets/gets (Public):
1430 {
1431 return (!startVertices_.empty());
1432 }
1433
1435 {
1436 return (!goalVertices_.empty());
1437 }
1438
1439 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1440 {
1441 return startVertices_.cbegin();
1442 }
1443
1444 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesEndConst() const
1445 {
1446 return startVertices_.cend();
1447 }
1448
1449 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1450 {
1451 return goalVertices_.cbegin();
1452 }
1453
1454 BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesEndConst() const
1455 {
1456 return goalVertices_.cend();
1457 }
1458
1460 {
1461 return minCost_;
1462 }
1463
1465 {
1466 return sampler_->hasInformedMeasure();
1467 }
1468
1470 {
1471 return sampler_->getInformedMeasure(cost);
1472 }
1473
1475 {
1476#ifdef BITSTAR_DEBUG
1477 if (!findApprox_)
1478 {
1479 throw ompl::Exception("Approximate solutions are not being tracked.");
1480 }
1481#endif // BITSTAR_DEBUG
1482 return closestVertexToGoal_;
1483 }
1484
1486 {
1487#ifdef BITSTAR_DEBUG
1488 if (!findApprox_)
1489 {
1490 throw ompl::Exception("Approximate solutions are not being tracked.");
1491 }
1492#endif // BITSTAR_DEBUG
1493 return closestDistanceToGoal_;
1494 }
1495
1497 {
1498#ifdef BITSTAR_DEBUG
1499 if (!useKNearest_)
1500 {
1501 throw ompl::Exception("Using an r-disc graph.");
1502 }
1503#endif // BITSTAR_DEBUG
1504 return k_;
1505 }
1506
1508 {
1509#ifdef BITSTAR_DEBUG
1510 if (useKNearest_)
1511 {
1512 throw ompl::Exception("Using a k-nearest graph.");
1513 }
1514#endif // BITSTAR_DEBUG
1515 return r_;
1516 }
1517
1519 {
1520 VertexPtrVector samples;
1521 samples_->list(samples);
1522 return samples;
1523 }
1524
1526 {
1527 // Store
1528 rewireFactor_ = rewireFactor;
1529
1530 // Check if there's things to update
1531 if (isSetup_)
1532 {
1533 // Reinitialize the terms:
1534 this->updateNearestTerms();
1535 }
1536 }
1537
1539 {
1540 return rewireFactor_;
1541 }
1542
1544 {
1545 // Assure that we're not trying to enable k-nearest with JIT sampling already on
1546 if (useKNearest && useJustInTimeSampling_)
1547 {
1548 OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1549 "continuing to use the r-disc variant.",
1550 nameFunc_().c_str());
1551 }
1552 else
1553 {
1554 // Store
1555 useKNearest_ = useKNearest;
1556
1557 // Check if there's things to update
1558 if (isSetup_)
1559 {
1560 // Calculate the current term:
1561 this->updateNearestTerms();
1562 }
1563 }
1564 }
1565
1567 {
1568 return useKNearest_;
1569 }
1570
1572 {
1573 // Assure that we're not trying to enable k-nearest with JIT sampling already on
1574 if (useKNearest_ && useJit)
1575 {
1576 OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1577 "BIT*, continuing to use regular sampling.",
1578 nameFunc_().c_str());
1579 }
1580 else
1581 {
1582 // Store
1583 useJustInTimeSampling_ = useJit;
1584
1585 // Announce limitation:
1586 if (useJit)
1587 {
1588 OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1589 "seeking to minimize path-length.",
1590 nameFunc_().c_str());
1591 }
1592 // No else
1593 }
1594 }
1595
1597 {
1598 return useJustInTimeSampling_;
1599 }
1600
1602 {
1603 // Make sure we're not already setup
1604 if (isSetup_)
1605 {
1606 OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1607 "Continuing to use the previous setting.",
1608 nameFunc_().c_str());
1609 }
1610 else
1611 {
1612 // Store
1613 dropSamplesOnPrune_ = dropSamples;
1614 }
1615 }
1616
1618 {
1619 isPruningEnabled_ = usePruning;
1620 }
1621
1623 {
1624 return dropSamplesOnPrune_;
1625 }
1626
1628 {
1629 // Is the flag changing?
1630 if (findApproximate != findApprox_)
1631 {
1632 // Store the flag
1633 findApprox_ = findApproximate;
1634
1635 // Check if we are enabling or disabling approximate solution support
1636 if (!findApprox_)
1637 {
1638 // We're turning it off, clear the approximate solution variables:
1639 closestDistanceToGoal_ = std::numeric_limits<double>::infinity();
1640 closestVertexToGoal_.reset();
1641 }
1642 else
1643 {
1644 // We are turning it on, do we have an exact solution?
1645 if (!hasExactSolution_)
1646 {
1647 // We don't, find our current best approximate solution:
1648 this->updateVertexClosestToGoal();
1649 }
1650 // No else, exact is better than approximate.
1651 }
1652 }
1653 // No else, no change.
1654 }
1655
1657 {
1658 return findApprox_;
1659 }
1660
1662 {
1663 averageNumOfAllowedFailedAttemptsWhenSampling_ = number;
1664 }
1665
1667 {
1668 return averageNumOfAllowedFailedAttemptsWhenSampling_;
1669 }
1670
1671 template <template <typename T> class NN>
1673 {
1674 // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1675 // change them:
1676 if (isSetup_)
1677 {
1678 OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1679 "is setup. Continuing to use the existing containers.",
1680 nameFunc_().c_str());
1681 }
1682 else
1683 {
1684 // The problem isn't setup yet, create NN structs of the specified type:
1685 samples_ = std::make_shared<NN<VertexPtr>>();
1686 }
1687 }
1688
1690 {
1691 return samples_->size();
1692 }
1693
1695 {
1696 VertexPtrVector samples;
1697 samples_->list(samples);
1698 return std::count_if(samples.begin(), samples.end(),
1699 [](const VertexPtr &sample) { return sample->isInTree(); });
1700 }
1701
1703 {
1704 return numSamples_;
1705 }
1706
1708 {
1709 return numVertices_;
1710 }
1711
1713 {
1714 return numFreeStatesPruned_;
1715 }
1716
1718 {
1719 return numVerticesDisconnected_;
1720 }
1721
1723 {
1724 return numNearestNeighbours_;
1725 }
1726
1728 {
1729 return numStateCollisionChecks_;
1730 }
1732 } // namespace geometric
1733} // 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:339
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:228
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:346
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:265
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
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:487
bool hasParent() const
Returns whether this vertex has a parent.
Definition Vertex.cpp:169
void markPruned()
Mark the vertex as pruned.
Definition Vertex.cpp:527
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:792
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:492
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
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
#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.