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
74 
75 #define ASSERT_SETUP this->assertSetup();
76 #else
77 #define ASSERT_SETUP
78 #endif // BITSTAR_DEBUG
79 
80 namespace 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,
95  ompl::base::PlannerInputStates &inputStates)
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 
303  void BITstar::ImplicitGraph::nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
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
void nearestSamples(const VertexPtr &vertex, VertexPtrVector *neighbourSamples)
Get the nearest unconnected samples using the appropriate "near" definition (i.e.,...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:90
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
Base class for a planner.
Definition: Planner.h:287
void addToSamples(const VertexPtr &sample)
Add an unconnected sample.
void setDropSamplesOnPrune(bool dropSamples)
Set whether unconnected samples are dropped on pruning.
unsigned int numStatesGenerated() const
The total number of states generated.
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.
void setTrackApproximateSolutions(bool findApproximate)
Set whether to track approximate solutions during the search.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:142
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
unsigned int numVertices() const
The number of vertices in the search tree.
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:129
Definition of an abstract state.
Definition: State.h:114
bool hasAGoal() const
Gets whether the graph contains a goal or not.
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
void getGraphAsPlannerData(ompl::base::PlannerData &data) const
Adds the graph to the given PlannerData struct.
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:245
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:233
void registerAsVertex(const VertexPtr &vertex)
Add a vertex to the tree, optionally moving it from the set of unconnected samples.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
VertexPtrVector getCopyOfSamples() const
Get a copy of all samples.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void setPruning(bool usePruning)
Set whether samples that are provably not beneficial should be kept around.
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,...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
unsigned int numVerticesDisconnected() const
The number of tree vertices disconnected.
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:134
bool canSampleBePruned(const VertexPtr &sample) const
Returns whether the sample can be pruned, i.e., whether it could ever provide a better solution....
std::pair< unsigned int, unsigned int > pruneVertex(const VertexPtr &vertex)
Remove a vertex and mark as pruned.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
ImplicitGraph(NameFunc nameFunc)
Construct an implicit graph.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
double getInformedMeasure(const ompl::base::Cost &cost) const
Query the underlying state sampler for the informed measure of the problem.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
void removeFromSamples(const VertexPtr &sample)
Remove a sample from the sample set.
double distance(const VertexConstPtr &a, const VertexConstPtr &b) const
Computes the distance between two states.
unsigned int numVerticesConnected() const
The total number of vertices added to the graph.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
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
Abstract representation of a container that can perform nearest neighbors queries.
VertexConstPtr closestVertexToGoal() const
IF BEING TRACKED, returns the closest vertex in the tree to the goal.
void removeEdgeBetweenVertexAndParent(const VertexPtr &child, bool cascadeCostUpdates)
Disconnect a vertex from its parent by removing the edges stored in itself, and its parents....
A shared pointer wrapper for ompl::base::ProblemDefinition.
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...
unsigned int numStateCollisionChecks() const
The number of state collision checks.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
double getConnectivityR() const
Get the radius of this r-disc RGG.
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
unsigned int numFreeStatesPruned() const
The number of states pruned.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
unsigned int numNearestLookups() const
The number of nearest neighbour calls.
bool hasInformedMeasure() const
Query whether the underlying state sampler can provide an informed measure.
void reset()
Reset the queue to the state of construction.
void recycleSample(const VertexPtr &sample)
Insert a sample into the set for recycled samples.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
void pruneSample(const VertexPtr &sample)
Remove an unconnected sample.
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
double smallestDistanceToGoal() const
IF BEING TRACKED, returns the how close vertices in the tree are to the goal.
void reset()
Reset the graph to the state of construction.
bool getTrackApproximateSolutions() const
Get whether approximate solutions are tracked during the search.
ompl::base::Cost minCost() const
Get the minimum cost solution possible for this problem.
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
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...
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
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 setNearestNeighbors()
Set a different nearest neighbours datastructure.
unsigned int getConnectivityK() const
Get the k of this k-nearest RGG.
unsigned int numSamples() const
The number of samples.
bool canVertexBeDisconnected(const VertexPtr &vertex) const
Returns whether the vertex can be pruned, i.e., whether it could provide a better solution given....
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...
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...
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
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...
double getRewireFactor() const
Get the rewiring scale factor.
The exception type for ompl.
Definition: Exception.h:79
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:123
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
virtual void setDistanceFunction(const DistanceFunction &distFun)
Set the distance function to use.
void addNewSamples(const unsigned int &numSamples)
Increase the resolution of the graph-based approximation of the continuous search domain by adding a ...