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