PathLengthDirectInfSampler.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 #include "ompl/base/samplers/informed/PathLengthDirectInfSampler.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/base/OptimizationObjective.h"
40 // For ompl::base::GoalSampleableRegion, which both GoalState and GoalStates derive from:
41 #include "ompl/base/goals/GoalSampleableRegion.h"
42 #include "ompl/base/StateSpace.h"
43 #include "ompl/base/spaces/RealVectorStateSpace.h"
44 
45 // For std::make_shared
46 #include <memory>
47 // For std::vector
48 #include <vector>
49 
50 namespace ompl
51 {
52  namespace base
53  {
55  // Public functions:
56 
57  // The direct ellipsoid sampling class for path-length:
59  unsigned int maxNumberCalls)
60  : InformedSampler(probDefn, maxNumberCalls), informedIdx_(0u), uninformedIdx_(0u)
61  {
62  // Variables
63  // The number of start states
64  unsigned int numStarts;
65  // The number of goal states
66  unsigned numGoals;
67  // The foci of the PHSs as a std::vector of states. Goals must be nonconst, as we need to allocate them
68  // (unfortunately):
69  std::vector<const State *> startStates;
70  std::vector<State *> goalStates;
71 
72  if (!probDefn_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION))
73  {
74  throw Exception("PathLengthDirectInfSampler: The direct path-length informed sampler currently only "
75  "supports goals that can be cast to a sampleable goal region (i.e., are countable "
76  "sets).");
77  }
78 
81 
82  // Store the number of starts and goals
83  numStarts = probDefn_->getStartStateCount();
84  numGoals = probDefn_->getGoal()->as<ompl::base::GoalSampleableRegion>()->maxSampleCount();
85 
86  // Sanity check that there is atleast one of each
87  if (numStarts < 1u || numGoals < 1u)
88  {
89  throw Exception("PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when "
90  "the informed sampler is created.");
91  }
92 
93  // Check that the provided statespace is compatible and extract the necessary indices.
94  // The statespace must either be R^n or SE(2) or SE(3).
95  // If it is UNKNOWN, warn and treat it as R^n
96  if (!InformedSampler::space_->isCompound())
97  {
99  {
100  // R^n, this is easy
101  informedIdx_ = 0u;
102  uninformedIdx_ = 0u;
103  }
104  else if (InformedSampler::space_->getType() == STATE_SPACE_UNKNOWN)
105  {
106  // Unknown, this is annoying. I hope the user knows what they're doing
107  OMPL_WARN("PathLengthDirectInfSampler: Treating the StateSpace of type \"STATE_SPACE_UNKNOWN\" as "
108  "type \"STATE_SPACE_REAL_VECTOR\".");
109  informedIdx_ = 0u;
110  uninformedIdx_ = 0u;
111  }
112  else
113  {
114  throw Exception("PathLengthDirectInfSampler only supports Unknown, RealVector, SE2, and SE3 "
115  "StateSpaces.");
116  }
117  }
118  else if (InformedSampler::space_->isCompound())
119  {
120  // Variable:
121  // An ease of use upcasted pointer to the space as a compound space
123 
124  // Check that the given space is SE2, SE3, Dubins, or Reeds-Shepp.
125  if (InformedSampler::space_->getType() == STATE_SPACE_SE2 ||
129  {
130  // Sanity check
131  if (compoundSpace->getSubspaceCount() != 2u)
132  {
133  // Pout
134  throw Exception("The provided compound state space does not have exactly 2 subspaces.");
135  }
136 
137  // Iterate over the state spaces, finding the real vector and SO components.
138  for (unsigned int idx = 0u;
139  idx < InformedSampler::space_->as<CompoundStateSpace>()->getSubspaceCount(); ++idx)
140  {
141  // Check if the space is real-vectored, SO2 or SO3
142  if (compoundSpace->getSubspace(idx)->getType() == STATE_SPACE_REAL_VECTOR)
143  {
144  informedIdx_ = idx;
145  }
146  else if (compoundSpace->getSubspace(idx)->getType() == STATE_SPACE_SO2)
147  {
148  uninformedIdx_ = idx;
149  }
150  else if (compoundSpace->getSubspace(idx)->getType() == STATE_SPACE_SO3)
151  {
152  uninformedIdx_ = idx;
153  }
154  else
155  {
156  // Pout
157  throw Exception("The provided compound state space contains a subspace (" +
158  std::to_string(idx) + ") that is not R^N, SO(2), or SO(3): " +
159  std::to_string(compoundSpace->getSubspace(idx)->getType()));
160  }
161  }
162  }
163  else
164  {
165  // Case where we have a compound state space with only one subspace, and said subspace being R^N
166  if (compoundSpace->getSubspaceCount() == 1u &&
167  compoundSpace->getSubspace(0)->getType() == STATE_SPACE_REAL_VECTOR)
168  {
169  informedIdx_ = 0u;
170  uninformedIdx_ = 0u;
171  }
172  else
173  {
174  throw Exception("PathLengthDirectInfSampler only supports RealVector, SE2, SE3, Dubins, and "
175  "ReedsShepp state spaces. Provided compound state space of type: " +
176  std::to_string(InformedSampler::space_->getType()) + " with " +
177  std::to_string(compoundSpace->getSubspaceCount()) + " subspaces.");
178  }
179  }
180  }
181 
182  // Create a sampler for the whole space that we can use if we have no information
183  baseSampler_ = InformedSampler::space_->allocDefaultStateSampler();
184 
185  // Check if the space is compound
186  if (!InformedSampler::space_->isCompound())
187  {
188  // It is not.
189 
190  // The informed subspace is the full space
191  informedSubSpace_ = InformedSampler::space_;
192 
193  // And the uniformed subspace and its associated sampler are null
194  uninformedSubSpace_ = StateSpacePtr();
195  uninformedSubSampler_ = StateSamplerPtr();
196  }
197  else
198  {
199  // It is
200 
201  // Get a pointer to the informed subspace...
202  informedSubSpace_ = InformedSampler::space_->as<CompoundStateSpace>()->getSubspace(informedIdx_);
203 
204  // And the uninformed subspace is the remainder.
205  uninformedSubSpace_ = InformedSampler::space_->as<CompoundStateSpace>()->getSubspace(uninformedIdx_);
206 
207  // Create a sampler for the uniformed subset:
208  uninformedSubSampler_ = uninformedSubSpace_->allocDefaultStateSampler();
209  }
210 
211  // Store the foci, first the starts:
212  for (unsigned int i = 0u; i < numStarts; ++i)
213  {
214  startStates.push_back(probDefn_->getStartState(i));
215  }
216 
217  // Extract the state of each goal one and place into the goal vector!
218  for (unsigned int i = 0u; i < numGoals; ++i)
219  {
220  // Allocate a state onto the back of the vector:
221  goalStates.push_back(InformedSampler::space_->allocState());
222 
223  // Now sample a goal into that state:
224  probDefn_->getGoal()->as<ompl::base::GoalSampleableRegion>()->sampleGoal(goalStates.back());
225  }
226 
227  // Now, iterate create a PHS for each start-goal pair
228  // Each start
229  for (unsigned int i = 0u; i < numStarts; ++i)
230  {
231  // Variable
232  // The start as a vector
233  std::vector<double> startFocusVector = getInformedSubstate(startStates.at(i));
234 
235  // Each goal
236  for (unsigned int j = 0u; j < numGoals; ++j)
237  {
238  // Variable
239  // The goal as a vector
240  std::vector<double> goalFocusVector = getInformedSubstate(goalStates.at(j));
241 
242  // Create the definition of the PHS
243  listPhsPtrs_.push_back(std::make_shared<ProlateHyperspheroid>(
244  informedSubSpace_->getDimension(), &startFocusVector[0], &goalFocusVector[0]));
245  }
246  }
247 
248  // Finally deallocate the states in the goal state vector:
249  for (unsigned int i = 0u; i < numGoals; ++i)
250  {
251  // Free the state in the vector:
252  InformedSampler::space_->freeState(goalStates.at(i));
253  }
254 
255  if (listPhsPtrs_.size() > 100u)
256  {
257  OMPL_WARN("PathLengthDirectInfSampler: Rejection sampling is used in order to maintain uniform density "
258  "in the presence of overlapping informed subsets. At some number of independent subsets, "
259  "this will become prohibitively expensive. Current number of independent subsets: %d",
260  listPhsPtrs_.size());
261  }
262  }
263 
264  PathLengthDirectInfSampler::~PathLengthDirectInfSampler() = default;
265 
266  bool PathLengthDirectInfSampler::sampleUniform(State *statePtr, const Cost &maxCost)
267  {
268  // Variable
269  // The persistent iteration counter:
270  unsigned int iter = 0u;
271 
272  // Call the sampleUniform helper function with my iteration counter:
273  return sampleUniform(statePtr, maxCost, &iter);
274  }
275 
276  bool PathLengthDirectInfSampler::sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost)
277  {
278  // Sample from the larger PHS until the sample does not lie within the smaller PHS.
279  // Since volume in a sphere/spheroid is proportionately concentrated near the surface, this isn't horribly
280  // inefficient, though a direct method would be better
281 
282  // Variable
283  // Whether we were successful in creating an informed sample. Initially not:
284  bool foundSample = false;
285 
286  // Spend numIters_ iterations trying to find an informed sample:
287  for (unsigned int i = 0u; i < InformedSampler::numIters_ && !foundSample; ++i)
288  {
289  // Call the helper function for the larger PHS. It will move our iteration counter:
290  foundSample = sampleUniform(statePtr, maxCost, &i);
291 
292  // Did we find a sample?
293  if (foundSample)
294  {
295  // We did, but it only satisfied the upper bound. Check that it meets the lower bound.
296 
297  // Variables
298  // The cost of the sample we found
299  Cost sampledCost = heuristicSolnCost(statePtr);
300 
301  // Check if the sample's cost is greater than or equal to the lower bound
302  foundSample = InformedSampler::opt_->isCostEquivalentTo(minCost, sampledCost) ||
303  InformedSampler::opt_->isCostBetterThan(minCost, sampledCost);
304  }
305  // No else, no sample was found.
306  }
307 
308  // All done, one way or the other.
309  return foundSample;
310  }
311 
313  {
314  return true;
315  }
316 
317  double PathLengthDirectInfSampler::getInformedMeasure(const Cost &currentCost) const
318  {
319  // Variable
320  // The measure of the informed set
321  double informedMeasure = 0.0;
322 
323  // The informed measure is then the sum of the measure of the individual PHSs for the given cost:
324  for (const auto &phsPtr : listPhsPtrs_)
325  {
326  // It is nonsensical for a PHS to have a transverse diameter less than the distance between its foci, so
327  // skip those that do
328  if (currentCost.value() > phsPtr->getMinTransverseDiameter())
329  {
330  informedMeasure = informedMeasure + phsPtr->getPhsMeasure(currentCost.value());
331  }
332  // No else, this value is better than this ellipse. It will get removed later.
333  }
334 
335  // And if the space is compound, further multiplied by the measure of the uniformed subspace
336  if (InformedSampler::space_->isCompound())
337  {
338  informedMeasure = informedMeasure * uninformedSubSpace_->getMeasure();
339  }
340 
341  // Return the smaller of the two measures
342  return std::min(InformedSampler::space_->getMeasure(), informedMeasure);
343  }
344 
346  {
347  // Variable
348  // The raw data in the state
349  std::vector<double> rawData = getInformedSubstate(statePtr);
350  // The Cost, infinity to start
351  Cost minCost = InformedSampler::opt_->infiniteCost();
352 
353  // Iterate over the separate subsets and return the minimum
354  for (const auto &phsPtr : listPhsPtrs_)
355  {
358  minCost = InformedSampler::opt_->betterCost(minCost, Cost(phsPtr->getPathLength(&rawData[0])));
359  }
360 
361  return minCost;
362  }
364 
366  // Private functions:
367  bool PathLengthDirectInfSampler::sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iters)
368  {
369  // Variable
370  // Whether we were successful in creating an informed sample. Initially not:
371  bool foundSample = false;
372 
373  // Whether we successfully returnes
374  // Check if a solution path has been found
375  if (!InformedSampler::opt_->isFinite(maxCost))
376  {
377  // We don't have a solution yet, we sample from our basic sampler instead...
378  baseSampler_->sampleUniform(statePtr);
379 
380  // Up our counter by one:
381  ++(*iters);
382 
383  // Mark that we sampled:
384  foundSample = true;
385  }
386  else // We have a solution
387  {
388  // Update the definitions of the PHSs
389  updatePhsDefinitions(maxCost);
390 
391  // Sample from the PHSs.
392 
393  // When the summed measure of the PHSes are suitably large, it makes more sense to just sample from the
394  // entire planning space and keep the sample if it lies in any PHS
395  // Check if the average measure is greater than half the domain's measure. Half is an arbitrary number.
396  if (informedSubSpace_->getMeasure() < summedMeasure_ / static_cast<double>(listPhsPtrs_.size()))
397  {
398  // The measure is large, sample from the entire world and keep if it's in any PHS
399  foundSample = sampleBoundsRejectPhs(statePtr, iters);
400  }
401  else
402  {
403  // The measure is sufficiently small that we will directly sample the PHSes, with the weighting
404  // given by their relative measures
405  foundSample = samplePhsRejectBounds(statePtr, iters);
406  }
407  }
408 
409  // Return:
410  return foundSample;
411  }
412 
413  bool PathLengthDirectInfSampler::sampleBoundsRejectPhs(State *statePtr, unsigned int *iters)
414  {
415  // Variable
416  // Whether we've found a sample:
417  bool foundSample = false;
418 
419  // Spend numIters_ iterations trying to find an informed sample:
420  while (!foundSample && *iters < InformedSampler::numIters_)
421  {
422  // Generate a random sample
423  baseSampler_->sampleUniform(statePtr);
424 
425  // The informed substate
426  std::vector<double> informedVector = getInformedSubstate(statePtr);
427 
428  // Check if the informed state is in any PHS.
429  foundSample = isInAnyPhs(informedVector);
430 
431  // Increment the provided counter
432  ++(*iters);
433  }
434 
435  // successful?
436  return foundSample;
437  }
438 
439  bool PathLengthDirectInfSampler::samplePhsRejectBounds(State *statePtr, unsigned int *iters)
440  {
441  // Variable
442  // Whether we were successful in creating an informed sample. Initially not:
443  bool foundSample = false;
444 
445  // Due to the possibility of overlap between multiple PHSs, we keep a sample with a probability of 1/K,
446  // where K is the number of PHSs the sample is in.
447  while (!foundSample && *iters < InformedSampler::numIters_)
448  {
449  // Variables
450  // The informed subset of the sample as a vector
451  std::vector<double> informedVector(informedSubSpace_->getDimension());
452  // The random PHS in use for this sample.
453  ProlateHyperspheroidCPtr phsCPtr = randomPhsPtr();
454 
455  // Use the PHS to get a sample in the informed subspace irrespective of boundary
456  rng_.uniformProlateHyperspheroid(phsCPtr, &informedVector[0]);
457 
458  // Keep with probability 1/K
459  foundSample = keepSample(informedVector);
460 
461  // If we're keeping it, then check if the state is in the problem domain:
462  if (foundSample)
463  {
464  // Turn into a state of our full space
465  createFullState(statePtr, informedVector);
466 
467  // Return if the resulting state is in the problem:
468  foundSample = InformedSampler::space_->satisfiesBounds(statePtr);
469  }
470  // No else
471 
472  // Increment the provided counter
473  ++(*iters);
474  }
475 
476  // Successful?
477  return foundSample;
478  }
479 
480  std::vector<double> PathLengthDirectInfSampler::getInformedSubstate(const State *statePtr) const
481  {
482  // Variable
483  // The raw data in the state
484  std::vector<double> rawData(informedSubSpace_->getDimension());
485 
486  // Get the raw data
487  if (!InformedSampler::space_->isCompound())
488  {
489  informedSubSpace_->copyToReals(rawData, statePtr);
490  }
491  else
492  {
493  informedSubSpace_->copyToReals(rawData, statePtr->as<CompoundState>()->components[informedIdx_]);
494  }
495 
496  return rawData;
497  }
498 
499  void PathLengthDirectInfSampler::createFullState(State *statePtr, const std::vector<double> &informedVector)
500  {
501  // If there is an extra "uninformed" subspace, we need to add that to the state before converting the raw
502  // vector representation into a state....
503  if (!InformedSampler::space_->isCompound())
504  {
505  // No, space_ == informedSubSpace_
506  // Copy into the state pointer
507  informedSubSpace_->copyFromReals(statePtr, informedVector);
508  }
509  else
510  {
511  // Yes, we need to also sample the uninformed subspace
512  // Variables
513  // A state for the uninformed subspace
514  State *uninformedState = uninformedSubSpace_->allocState();
515 
516  // Copy the informed subspace into the state pointer
517  informedSubSpace_->copyFromReals(statePtr->as<CompoundState>()->components[informedIdx_],
518  informedVector);
519 
520  // Sample the uniformed subspace
521  uninformedSubSampler_->sampleUniform(uninformedState);
522 
523  // Copy the informed subspace into the state pointer
524  uninformedSubSpace_->copyState(statePtr->as<CompoundState>()->components[uninformedIdx_],
525  uninformedState);
526 
527  // Free the state
528  uninformedSubSpace_->freeState(uninformedState);
529  }
530  }
531 
532  void PathLengthDirectInfSampler::updatePhsDefinitions(const Cost &maxCost)
533  {
534  // Variable
535  // The iterator for the list:
536  auto phsIter = listPhsPtrs_.begin();
537 
538  // Iterate over the list of PHSs, updating the summed measure
539  // Reset the sum
540  summedMeasure_ = 0.0;
541  while (phsIter != listPhsPtrs_.end())
542  {
543  // Check if the specific PHS can ever be better than the given maxCost, i.e., if the distance between
544  // the foci is less than the current max cost
545  if ((*phsIter)->getMinTransverseDiameter() < maxCost.value())
546  {
547  // It can improve the solution, or it's the only PHS we have, update it
548 
549  // Update the transverse diameter
550  (*phsIter)->setTransverseDiameter(maxCost.value());
551 
552  // Increment the summed measure of the ellipses.
553  summedMeasure_ = summedMeasure_ + (*phsIter)->getPhsMeasure();
554 
555  // Increment the iterator
556  ++phsIter;
557  }
558  else if (listPhsPtrs_.size() > 1u)
559  {
560  // It can't, and it is not the last PHS, remove it
561 
562  // Remove the iterator to delete from the list, this returns the next:
564  phsIter = listPhsPtrs_.erase(phsIter);
565  }
566  else
567  {
568  // It can't, but it's the last PHS, so we can't remove it.
569 
570  // Make sure it's transverse diameter is set to something:
571  (*phsIter)->setTransverseDiameter((*phsIter)->getMinTransverseDiameter());
572 
573  // Set the summed measure to 0.0 (as a degenerate PHS is a line):
574  summedMeasure_ = 0.0;
575 
576  // Increment the iterator so we move past this to the end.
577  ++phsIter;
578  }
579  }
580  }
581 
582  ompl::ProlateHyperspheroidPtr PathLengthDirectInfSampler::randomPhsPtr()
583  {
584  // Variable
585  // The return value
586  ompl::ProlateHyperspheroidPtr rval;
587 
588  // If we only have one PHS, this can be simplified:
589  if (listPhsPtrs_.size() == 1u)
590  {
591  // One PHS, keep this simple.
592 
593  // Return it
594  rval = listPhsPtrs_.front();
595  }
596  else
597  {
598  // We have more than one PHS to consider
599 
600  // Variables
601  // A randomly generated number in the interval [0,1]
602  double randDbl = rng_.uniform01();
603  // The running measure
604  double runningRelativeMeasure = 0.0;
605 
606  // The probability of using each PHS is weighted by it's measure. Therefore, if we iterate up the list
607  // of PHSs, the first one who's relative measure is greater than the PHS randomly selected
608  for (std::list<ompl::ProlateHyperspheroidPtr>::const_iterator phsIter = listPhsPtrs_.begin();
609  phsIter != listPhsPtrs_.end() && !static_cast<bool>(rval); ++phsIter)
610  {
611  // Update the running measure
612  runningRelativeMeasure = runningRelativeMeasure + (*phsIter)->getPhsMeasure() / summedMeasure_;
613 
614  // Check if it's now greater than the proportion of the summed measure
615  if (runningRelativeMeasure > randDbl)
616  {
617  // It is, return this PHS:
618  rval = *phsIter;
619  }
620  // No else, continue
621  }
622  }
623 
624  // Return
625  return rval;
626  }
627 
628  bool PathLengthDirectInfSampler::keepSample(const std::vector<double> &informedVector)
629  {
630  // Variable
631  // The return value, do we keep this sample? Start true.
632  bool keep = true;
633 
634  // Is there more than 1 goal?
635  if (listPhsPtrs_.size() > 1u)
636  {
637  // There is, do work
638 
639  // Variable
640  // The number of PHSs the sample is in
641  unsigned int numIn = numberOfPhsInclusions(informedVector);
642  // The random number between [0,1]
643  double randDbl = rng_.uniform01();
644 
645  // Keep the sample if the random number is less than 1/K
646  keep = (randDbl <= 1.0 / static_cast<double>(numIn));
647  }
648  // No else, keep is true by default.
649 
650  return keep;
651  }
652 
653  bool PathLengthDirectInfSampler::isInAnyPhs(const std::vector<double> &informedVector) const
654  {
655  // Variable
656  // The return value, whether the given state is in any PHS
657  bool inPhs = false;
658 
659  // Iterate over the list, stopping as soon as we get our first true
660  for (auto phsIter = listPhsPtrs_.begin(); phsIter != listPhsPtrs_.end() && !inPhs; ++phsIter)
661  {
662  inPhs = isInPhs(*phsIter, informedVector);
663  }
664 
665  return inPhs;
666  }
667 
668  bool PathLengthDirectInfSampler::isInPhs(const ProlateHyperspheroidCPtr &phsCPtr,
669  const std::vector<double> &informedVector) const
670  {
671  return phsCPtr->isInPhs(&informedVector[0]);
672  }
673 
674  unsigned int PathLengthDirectInfSampler::numberOfPhsInclusions(const std::vector<double> &informedVector) const
675  {
676  // Variable
677  // The return value, the number of PHSs the vector is in
678  unsigned int numInclusions = 0u;
679 
680  // Iterate over the list counting
681  for (const auto &phsPtr : listPhsPtrs_)
682  {
683  // Conditionally increment
684  if (phsPtr->isInPhs(&informedVector[0]))
685  {
686  ++numInclusions;
687  }
688  // No else
689  }
690 
691  return numInclusions;
692  }
694  }; // namespace base
695 }; // namespace ompl
A space to allow the composition of state spaces.
Definition: StateSpace.h:637
ProblemDefinitionPtr probDefn_
A copy of the problem definition.
Definition of an abstract state.
Definition: State.h:113
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:909
@ STATE_SPACE_REEDS_SHEPP
ompl::base::ReedsSheppStateSpace
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_UNKNOWN
Unset type; this is the default type.
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:904
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
Construct a sampler that only generates states with a heuristic solution estimate that is less than t...
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_DUBINS
ompl::base::DubinsStateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
double getInformedMeasure(const Cost &currentCost) const override
The measure of the subset of the state space defined by the current solution cost that is being searc...
Cost heuristicSolnCost(const State *statePtr) const override
A helper function to calculate the heuristic estimate of the solution cost for the informed subset of...
A shared pointer wrapper for ompl::base::ProblemDefinition.
OptimizationObjectivePtr opt_
A copy of the optimization objective.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
void uniformProlateHyperspheroid(const std::shared_ptr< const ProlateHyperspheroid > &phsPtr, double value[])
Uniform random sampling of a prolate hyperspheroid, a special symmetric type of n-dimensional ellipse...
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
unsigned int numIters_
The number of iterations I'm allowed to attempt.
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
StateSpacePtr space_
A copy of the state space.
Abstract definition of a goal region that can be sampled.
double uniform01()
Generate a random real between 0 and 1.
bool sampleUniform(State *statePtr, const Cost &maxCost) override
Sample uniformly in the subset of the state space whose heuristic solution estimates are less than th...
The exception type for ompl.
Definition: Exception.h:78
Main namespace. Contains everything in this library.
Definition: AppBase.h:21