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