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