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:
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
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_ = si_->getSpaceMeasure();
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  // Increment our counter
728  ++numFreeStatesPruned_;
729 
730  // Remove from the set of samples
731  freeStateNN_->remove(oldSample);
732 
733  // Mark the sample as pruned
734  oldSample->markPruned();
735  }
736 
737  void BITstar::ImplicitGraph::addVertex(const VertexPtr &newVertex, bool removeFromFree)
738  {
739  this->confirmSetup();
740 
741 #ifdef BITSTAR_DEBUG
742  // Make sure it's connected first, so that the queue gets updated properly.
743  // This is a day of debugging I'll never get back
744  if (newVertex->isInTree() == false)
745  {
746  throw ompl::Exception("Vertices must be connected to the graph before adding");
747  }
748 #endif // BITSTAR_DEBUG
749 
750  // Increment the number of vertices added:
751  ++numVertices_;
752 
753  // Remove the vertex from the set of samples (if it even existed)
754  if (removeFromFree)
755  {
756  freeStateNN_->remove(newVertex);
757  }
758  // No else
759 
760  // Add to the NN structure:
761  vertexNN_->add(newVertex);
762 
763  // Update the nearest vertex to the goal (if tracking)
764  if (!hasExactSolution_ && findApprox_)
765  {
766  this->testClosestToGoal(newVertex);
767  }
768  }
769 
770  unsigned int BITstar::ImplicitGraph::removeVertex(const VertexPtr &oldSample, bool moveToFree)
771  {
772  this->confirmSetup();
773 
774  // Increment our counter
775  ++numVerticesDisconnected_;
776 
777  // Remove from the nearest-neighbour structure
778  vertexNN_->remove(oldSample);
779 
780  // Add back as sample, if that would be beneficial
781  if (moveToFree && !queuePtr_->samplePruneCondition(oldSample))
782  {
783  // Yes, the vertex is still useful as a sample. Track as recycled so they are reused as samples in the
784  // next batch.
785  recycledSamples_.push_back(oldSample);
786 
787  // Return that the vertex was recycled
788  return 0u;
789  }
790  // No, the vertex is not useful anymore. Mark as pruned. This functions as a lock to prevent accessing
791  // anything about the vertex.
792  oldSample->markPruned();
793 
794  // Return that the vertex was completely pruned
795  return 1u;
796  }
798 
800  // Private functions:
801  void BITstar::ImplicitGraph::updateSamples(const VertexConstPtr &vertex)
802  {
803  // Variable
804  // The required cost to contain the neighbourhood of this vertex:
805  ompl::base::Cost costReqd = this->neighbourhoodCost(vertex);
806 
807  // Check if we need to generate new samples inorder to completely cover the neighbourhood of the vertex
808  if (costHelpPtr_->isCostBetterThan(costSampled_, costReqd))
809  {
810  // Variable
811  // The total number of samples we wish to have.
812  unsigned int totalReqdSamples;
813 
814  // Get the measure of what we're sampling
815  if (useJustInTimeSampling_)
816  {
817  // Variables
818  // The sample density for this slice of the problem.
819  double sampleDensity;
820  // The resulting number of samples needed for this slice as a *double*
821  double dblNum;
822 
823  // Calculate the sample density given the number of samples per batch and the measure of this batch
824  // by assuming that this batch will fill the same measure as the previous
825  sampleDensity = static_cast<double>(samplesInThisBatch_) / approximationMeasure_;
826 
827  // Convert that into the number of samples needed for this slice.
828  dblNum = sampleDensity * sampler_->getInformedMeasure(costSampled_, costReqd);
829 
830  // The integer of the double are definitely sampled
831  totalReqdSamples = numSamples_ + static_cast<unsigned int>(dblNum);
832 
833  // And the fractional part represents the probability of one more sample. I like being pedantic.
834  if (rng_.uniform01() <= (dblNum - static_cast<double>(totalReqdSamples)))
835  {
836  // One more please
837  ++totalReqdSamples;
838  }
839  // No else.
840  }
841  else
842  {
843  // We're generating all our samples in one batch. Do it to it.
844  totalReqdSamples = numSamples_ + samplesInThisBatch_;
845  }
846 
847  // Actually generate the new samples
848  while (numSamples_ < totalReqdSamples)
849  {
850  // Variable
851  // The new state:
852  auto newState = std::make_shared<Vertex>(si_, costHelpPtr_->getOptObj());
853 
854  // Sample in the interval [costSampled_, costReqd):
855  sampler_->sampleUniform(newState->state(), costSampled_, costReqd);
856 
857  // If the state is collision free, add it to the set of free states
858  ++numStateCollisionChecks_;
859  if (si_->isValid(newState->stateConst()))
860  {
861  // Add the new state as a sample
862  this->addSample(newState);
863 
864  // Update the number of uniformly distributed states
865  ++numUniformStates_;
866 
867  // Update the number of sample
868  ++numSamples_;
869  }
870  // No else
871  }
872 
873  // Mark that we've sampled all cost spaces (This is in preparation for JIT sampling)
874  costSampled_ = costReqd;
875  }
876  // No else, the samples are up to date
877  }
878 
879  void BITstar::ImplicitGraph::findVertexClosestToGoal()
880  {
881  if (static_cast<bool>(vertexNN_))
882  {
883  // Variable
884  // The vertices in the graph
885  VertexPtrVector vertices;
886 
887  // Get the vector of vertices
888  vertexNN_->list(vertices);
889 
890  // Iterate through them testing which is the closest to the goal.
891  for (const auto &vertex : vertices)
892  {
893  this->testClosestToGoal(vertex);
894  }
895  }
896  // No else, I do nothing.
897  }
898 
899  std::pair<unsigned int, unsigned int> BITstar::ImplicitGraph::pruneStartsGoals()
900  {
901  // Variable
902  // The number of starts/goals disconnected from the tree/pruned
903  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
904 
905  // Are there superfluous starts to prune?
906  if (startVertices_.size() > 1u)
907  {
908  // Yes, Iterate through the vector
909 
910  // Variable
911  // The iterator to the start:
912  auto startIter = startVertices_.begin();
913  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
914  // iterator towards the start, and then erasing once at the end.
915  auto startEnd = startVertices_.end();
916 
917  // Run until at the end:
918  while (startIter != startEnd)
919  {
920  // Check if this start has met the criteria to be pruned
921  if (queuePtr_->vertexPruneCondition(*startIter))
922  {
923  // It has, remove the start vertex DO NOT consider it as a sample. It is marked as a root node,
924  // so having it as a sample would cause all kinds of problems, also it shouldn't be possible for
925  // it to ever be useful as a sample anyway, unless there is a very weird cost function in play.
926  numPruned.second = numPruned.second + this->removeVertex(*startIter, false);
927 
928  // Count as a disconnected vertex
929  ++numPruned.first;
930 
931  // Remove it from the queue
932  queuePtr_->unqueueVertex(*startIter);
933 
934  // Store the start vertex in the pruned vector, in case it later needs to be readded:
935  prunedStartVertices_.push_back(*startIter);
936 
937  // Remove this start from the vector.
938  // Swap it to the element before our *new* end
939  if (startIter != (startEnd - 1))
940  {
941  std::swap(*startIter, *(startEnd - 1));
942  }
943 
944  // Move the end forward by one, marking it to be deleted
945  --startEnd;
946 
947  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
948  // back
949  }
950  else
951  {
952  // Still valid, move to the next one:
953  ++startIter;
954  }
955  }
956 
957  // Erase any elements moved to the "new end" of the vector
958  if (startEnd != startVertices_.end())
959  {
960  startVertices_.erase(startEnd, startVertices_.end());
961  }
962  // No else, nothing to delete
963  }
964  // No else, can't prune 1 start.
965 
966  // Are there superfluous goals to prune?
967  if (goalVertices_.size() > 1u)
968  {
969  // Yes, Iterate through the vector
970 
971  // Variable
972  // The iterator to the start:
973  auto goalIter = goalVertices_.begin();
974  // The end point of the vector to consider. We will delete by swapping elements to the end, moving this
975  // iterator towards the start, and then erasing once at the end.
976  auto goalEnd = goalVertices_.end();
977 
978  // Run until at the end:
979  while (goalIter != goalEnd)
980  {
981  // Check if this start has met the criteria to be pruned
982  if (queuePtr_->vertexPruneCondition(*goalIter))
983  {
984  // It has, remove the goal vertex completely
985  // Check if this vertex is in the tree
986  if ((*goalIter)->isInTree())
987  {
988  // It is, remove it from the queue
989  queuePtr_->unqueueVertex(*goalIter);
990 
991  // and as a vertex, allowing it to move to the set of samples.
992  numPruned.second = numPruned.second + this->removeVertex(*goalIter, true);
993 
994  // Count it as a disconnected vertex
995  ++numPruned.first;
996  }
997  else
998  {
999  // It is not, so just it like a sample
1000  this->removeSample(*goalIter);
1001 
1002  // Count a pruned sample
1003  ++numPruned.second;
1004  }
1005 
1006  // Store the start vertex in the pruned vector, in case it later needs to be readded:
1007  prunedGoalVertices_.push_back(*goalIter);
1008 
1009  // Remove this goal from the vector.
1010  // Swap it to the element before our *new* end
1011  if (goalIter != (goalEnd - 1))
1012  {
1013  std::swap(*goalIter, *(goalEnd - 1));
1014  }
1015 
1016  // Move the end forward by one, marking it to be deleted
1017  --goalEnd;
1018 
1019  // Leave the iterator where it is, as we need to recheck this element that we pulled from the
1020  // back
1021  }
1022  else
1023  {
1024  // The goal is still valid, get the next
1025  ++goalIter;
1026  }
1027  }
1028 
1029  // Erase any elements moved to the "new end" of the vector
1030  if (goalEnd != goalVertices_.end())
1031  {
1032  goalVertices_.erase(goalEnd, goalVertices_.end());
1033  }
1034  // No else, nothing to delete
1035  }
1036  // No else, can't prune 1 goal.
1037 
1038  // We don't need to update our approximate solution (if we're providing one) as we will only prune once a
1039  // solution exists.
1040 
1041  // Return the amount of work done
1042  return numPruned;
1043  }
1044 
1045  unsigned int BITstar::ImplicitGraph::pruneSamples()
1046  {
1047  // Variable:
1048  // The number of samples pruned in this pass:
1049  unsigned int numPruned = 0u;
1050 
1051  // Are we dropping samples anytime we prune?
1052  if (dropSamplesOnPrune_)
1053  {
1054  // We are, store the number pruned
1055  numPruned = freeStateNN_->size();
1056 
1057  // and the number of uniform samples
1058  numUniformStates_ = 0u;
1059 
1060  // Then remove all of the samples
1061  freeStateNN_->clear();
1062 
1063  // and increasing our global counter
1064  numFreeStatesPruned_ = numFreeStatesPruned_ + numPruned;
1065  }
1066  else
1067  {
1068  // Variable:
1069  // The vector of samples:
1070  VertexPtrVector samples;
1071 
1072  // Get the vector of samples
1073  freeStateNN_->list(samples);
1074 
1075  // Iterate through the vector and remove any samples that should not be in the queue
1076  for (const auto &freeSample : samples)
1077  {
1078  // Check if this state should be pruned:
1079  if (queuePtr_->samplePruneCondition(freeSample))
1080  {
1081  // Yes, remove it
1082  this->removeSample(freeSample);
1083 
1084  // and increment the counter
1085  ++numPruned;
1086  }
1087  // No else, keep.
1088  }
1089  }
1090 
1091  return numPruned;
1092  }
1093 
1094  void BITstar::ImplicitGraph::testClosestToGoal(const VertexConstPtr &newVertex)
1095  {
1096  // Variable
1097  // The distance from this vertex to the goal:
1098  double distFromGoal;
1099 
1100  // Find the shortest distance between the given vertex and a goal
1101  pdef_->getGoal()->isSatisfied(newVertex->stateConst(), &distFromGoal);
1102 
1103  // Compare to the current best approximate solution
1104  if (distFromGoal < closestDistToGoal_)
1105  {
1106  // Better, update the approximate solution
1107  closestVertexToGoal_ = newVertex;
1108  closestDistToGoal_ = distFromGoal;
1109  }
1110  // No else, don't update if worse
1111  }
1112 
1113  ompl::base::Cost BITstar::ImplicitGraph::neighbourhoodCost(const VertexConstPtr &vertex) const
1114  {
1115  // Are we using JIT sampling?
1116  if (useJustInTimeSampling_)
1117  {
1118  // We are, return the maximum heuristic cost that represents a sample in the neighbourhood of the given
1119  // vertex.
1120  // There is no point generating samples worse the best solution (maxCost_) even if those samples are in
1121  // this vertex's neighbourhood.
1122  return costHelpPtr_->betterCost(
1123  maxCost_, costHelpPtr_->combineCosts(costHelpPtr_->lowerBoundHeuristicVertex(vertex),
1124  ompl::base::Cost(2.0 * r_)));
1125  }
1126 
1127  // We are not, return the maximum cost we'd ever want to sample
1128  return maxCost_;
1129  }
1130 
1131  void BITstar::ImplicitGraph::updateNearestTerms()
1132  {
1133  // Variables:
1134  // The number of uniformly distributed states:
1135  unsigned int N;
1136 
1137  // Calculate N, are we dropping samples?
1138  if (dropSamplesOnPrune_)
1139  {
1140  // We are, so we've been tracking the number of uniform states, just use that
1141  N = numUniformStates_;
1142  }
1143  else
1144  {
1145  // We are not, so then all vertices and samples are uniform, use that
1146  N = vertexNN_->size() + freeStateNN_->size();
1147  }
1148 
1149  // If this is the first batch, we will be calculating the connection limits from only the starts and goals
1150  // for an RGG with m samples. That will be a complex graph. In this case, let us calculate the connection
1151  // limits considering the samples about to be generated. Doing so is equivalent to setting an upper-bound on
1152  // the radius, which RRT* does with it's min(maxEdgeLength, RGG-radius).
1153  if (N == (startVertices_.size() + goalVertices_.size()))
1154  {
1155  N = N + samplesInThisBatch_;
1156  }
1157 
1158  // Now update the appropriate term
1159  if (useKNearest_)
1160  {
1161  k_ = this->calculateK(N);
1162  }
1163  else
1164  {
1165  r_ = this->calculateR(N);
1166  }
1167  }
1168 
1169  double BITstar::ImplicitGraph::calculateR(unsigned int N) const
1170  {
1171  // Variables
1172  // The dimension cast as a double for readibility;
1173  auto dimDbl = static_cast<double>(si_->getStateDimension());
1174  // The size of the graph
1175  auto cardDbl = static_cast<double>(N);
1176 
1177  // Calculate the term and return
1178  return rewireFactor_ * this->minimumRggR() * std::pow(std::log(cardDbl) / cardDbl, 1 / dimDbl);
1179  }
1180 
1181  unsigned int BITstar::ImplicitGraph::calculateK(unsigned int N) const
1182  {
1183  // Calculate the term and return
1184  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(N)));
1185  }
1186 
1187  double BITstar::ImplicitGraph::minimumRggR() const
1188  {
1189  // Variables
1190  // The dimension cast as a double for readibility;
1191  auto dimDbl = static_cast<double>(si_->getStateDimension());
1192 
1193  // Calculate the term and return
1194  return 2.0 *
1195  std::pow((1.0 + 1.0 / dimDbl) * (approximationMeasure_ / unitNBallMeasure(si_->getStateDimension())),
1196  1.0 / dimDbl); // RRG radius (biggest for unit-volume problem)
1197  /*
1198  return std::pow(2.0 * (1.0 + 1.0 / dimDbl) * (approximationMeasure_ /
1199  unitNBallMeasure(si_->getStateDimension())), 1.0 / dimDbl); //RRT* radius (smaller for unit-volume problem)
1200  return 2.0 * std::pow((1.0 / dimDbl) * (approximationMeasure_ / unitNBallMeasure(si_->getStateDimension())),
1201  1.0 / dimDbl); //FMT* radius (smallest for R2, equiv to RRT* for R3 and then middle for higher d. All
1202  unit-volume)
1203  */
1204  }
1205 
1206  double BITstar::ImplicitGraph::minimumRggK() const
1207  {
1208  // Variables
1209  // The dimension cast as a double for readibility;
1210  auto dimDbl = static_cast<double>(si_->getStateDimension());
1211 
1212  // Calculate the term and return
1213  return boost::math::constants::e<double>() +
1214  (boost::math::constants::e<double>() / dimDbl); // RRG k-nearest
1215  }
1216 
1217  void BITstar::ImplicitGraph::confirmSetup() const
1218  {
1219 #ifdef BITSTAR_DEBUG
1220  if (isSetup_ == false)
1221  {
1222  throw ompl::Exception("BITstar::ImplicitGraph was used before it was setup.");
1223  }
1224 #endif // BITSTAR_DEBUG
1225  }
1227 
1229  // Boring sets/gets (Public):
1231  {
1232  return (!goalVertices_.empty());
1233  }
1234 
1235  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesBeginConst() const
1236  {
1237  return startVertices_.cbegin();
1238  }
1239 
1240  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::startVerticesEndConst() const
1241  {
1242  return startVertices_.cend();
1243  }
1244 
1245  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesBeginConst() const
1246  {
1247  return goalVertices_.cbegin();
1248  }
1249 
1250  BITstar::VertexPtrVector::const_iterator BITstar::ImplicitGraph::goalVerticesEndConst() const
1251  {
1252  return goalVertices_.cend();
1253  }
1254 
1256  {
1257  return minCost_;
1258  }
1259 
1261  {
1262  return sampler_->hasInformedMeasure();
1263  }
1264 
1266  {
1267  return sampler_->getInformedMeasure(cost);
1268  }
1269 
1271  {
1272 #ifdef BITSTAR_DEBUG
1273  if (findApprox_ == false)
1274  {
1275  throw ompl::Exception("Approximate solutions are not being tracked.");
1276  }
1277 #endif // BITSTAR_DEBUG
1278  return closestVertexToGoal_;
1279  }
1280 
1282  {
1283 #ifdef BITSTAR_DEBUG
1284  if (findApprox_ == false)
1285  {
1286  throw ompl::Exception("Approximate solutions are not being tracked.");
1287  }
1288 #endif // BITSTAR_DEBUG
1289  return closestDistToGoal_;
1290  }
1291 
1293  {
1294 #ifdef BITSTAR_DEBUG
1295  if (useKNearest_ == false)
1296  {
1297  throw ompl::Exception("Using an r-disc graph.");
1298  }
1299 #endif // BITSTAR_DEBUG
1300  return k_;
1301  }
1302 
1304  {
1305 #ifdef BITSTAR_DEBUG
1306  if (useKNearest_ == true)
1307  {
1308  throw ompl::Exception("Using a k-nearest graph.");
1309  }
1310 #endif // BITSTAR_DEBUG
1311  return r_;
1312  }
1313 
1315  {
1316  // Store
1317  rewireFactor_ = rewireFactor;
1318 
1319  // Check if there's things to update
1320  if (isSetup_)
1321  {
1322  // Reinitialize the terms:
1323  this->updateNearestTerms();
1324  }
1325  }
1326 
1328  {
1329  return rewireFactor_;
1330  }
1331 
1333  {
1334  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1335  if (useKNearest && useJustInTimeSampling_)
1336  {
1337  OMPL_WARN("%s (ImplicitGraph): The k-nearest variant of BIT* cannot be used with JIT sampling, "
1338  "continuing to use the r-disc variant.",
1339  nameFunc_().c_str());
1340  }
1341  else
1342  {
1343  // Store
1344  useKNearest_ = useKNearest;
1345 
1346  // Check if there's things to update
1347  if (isSetup_)
1348  {
1349  // Calculate the current term:
1350  this->updateNearestTerms();
1351  }
1352  }
1353  }
1354 
1356  {
1357  return useKNearest_;
1358  }
1359 
1361  {
1362  // Assure that we're not trying to enable k-nearest with JIT sampling already on
1363  if (useKNearest_ && useJit)
1364  {
1365  OMPL_WARN("%s (ImplicitGraph): Just-in-time sampling cannot be used with the k-nearest variant of "
1366  "BIT*, continuing to use regular sampling.",
1367  nameFunc_().c_str());
1368  }
1369  else
1370  {
1371  // Store
1372  useJustInTimeSampling_ = useJit;
1373 
1374  // Announce limitation:
1375  if (useJit)
1376  {
1377  OMPL_INFORM("%s (ImplicitGraph): Just-in-time sampling is currently only implemented for problems "
1378  "seeking to minimize path-length.",
1379  nameFunc_().c_str());
1380  }
1381  // No else
1382  }
1383  }
1384 
1386  {
1387  return useJustInTimeSampling_;
1388  }
1389 
1391  {
1392  // Make sure we're not already setup
1393  if (isSetup_)
1394  {
1395  OMPL_WARN("%s (ImplicitGraph): Periodic sample removal cannot be changed once BIT* is setup. "
1396  "Continuing to use the previous setting.",
1397  nameFunc_().c_str());
1398  }
1399  else
1400  {
1401  // Store
1402  dropSamplesOnPrune_ = dropSamples;
1403  }
1404  }
1405 
1407  {
1408  return dropSamplesOnPrune_;
1409  }
1410 
1412  {
1413  // Is the flag changing?
1414  if (findApproximate != findApprox_)
1415  {
1416  // Store the flag
1417  findApprox_ = findApproximate;
1418 
1419  // Check if we are enabling or disabling approximate solution support
1420  if (!findApprox_)
1421  {
1422  // We're turning it off, clear the approximate solution variables:
1423  closestDistToGoal_ = std::numeric_limits<double>::infinity();
1424  closestVertexToGoal_.reset();
1425  }
1426  else
1427  {
1428  // We are turning it on, do we have an exact solution?
1429  if (!hasExactSolution_)
1430  {
1431  // We don't, find our current best approximate solution:
1432  this->findVertexClosestToGoal();
1433  }
1434  // No else, exact is better than approximate.
1435  }
1436  }
1437  // No else, no change.
1438  }
1439 
1441  {
1442  return findApprox_;
1443  }
1444 
1445  template <template <typename T> class NN>
1447  {
1448  // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1449  // change them:
1450  if (isSetup_)
1451  {
1452  OMPL_WARN("%s (ImplicitGraph): The nearest neighbour datastructures cannot be changed once the problem "
1453  "is setup. Continuing to use the existing containers.",
1454  nameFunc_().c_str());
1455  }
1456  else
1457  {
1458  // The problem isn't setup yet, create NN structs of the specified type:
1459  freeStateNN_ = std::make_shared<NN<VertexPtr>>();
1460  vertexNN_ = std::make_shared<NN<VertexPtr>>();
1461  }
1462  }
1463 
1465  {
1466  return freeStateNN_->size();
1467  }
1468 
1470  {
1471  return vertexNN_->size();
1472  }
1473 
1475  {
1476  return numSamples_;
1477  }
1478 
1480  {
1481  return numVertices_;
1482  }
1483 
1485  {
1486  return numFreeStatesPruned_;
1487  }
1488 
1490  {
1491  return numVerticesDisconnected_;
1492  }
1493 
1495  {
1496  return numNearestNeighbours_;
1497  }
1498 
1500  {
1501  return numStateCollisionChecks_;
1502  }
1504  } // geometric
1505 } // 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_).
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 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.
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.
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