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