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