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 
456  // Successful?
457  return foundSample;
458  }
459 
460  std::vector<double> PathLengthDirectInfSampler::getInformedSubstate(const State *statePtr) const
461  {
462  // Variable
463  // The raw data in the state
464  std::vector<double> rawData(informedSubSpace_->getDimension());
465 
466  // Get the raw data
467  if (!InformedSampler::space_->isCompound())
468  {
469  informedSubSpace_->copyToReals(rawData, statePtr);
470  }
471  else
472  {
473  informedSubSpace_->copyToReals(rawData, statePtr->as<CompoundState>()->components[informedIdx_]);
474  }
475 
476  return rawData;
477  }
478 
479  void PathLengthDirectInfSampler::createFullState(State *statePtr, const std::vector<double> &informedVector)
480  {
481  // If there is an extra "uninformed" subspace, we need to add that to the state before converting the raw
482  // vector representation into a state....
483  if (!InformedSampler::space_->isCompound())
484  {
485  // No, space_ == informedSubSpace_
486  // Copy into the state pointer
487  informedSubSpace_->copyFromReals(statePtr, informedVector);
488  }
489  else
490  {
491  // Yes, we need to also sample the uninformed subspace
492  // Variables
493  // A state for the uninformed subspace
494  State *uninformedState = uninformedSubSpace_->allocState();
495 
496  // Copy the informed subspace into the state pointer
497  informedSubSpace_->copyFromReals(statePtr->as<CompoundState>()->components[informedIdx_],
498  informedVector);
499 
500  // Sample the uniformed subspace
501  uninformedSubSampler_->sampleUniform(uninformedState);
502 
503  // Copy the informed subspace into the state pointer
504  uninformedSubSpace_->copyState(statePtr->as<CompoundState>()->components[uninformedIdx_],
505  uninformedState);
506 
507  // Free the state
508  uninformedSubSpace_->freeState(uninformedState);
509  }
510  }
511 
512  void PathLengthDirectInfSampler::updatePhsDefinitions(const Cost &maxCost)
513  {
514  // Variable
515  // The iterator for the list:
516  auto phsIter = listPhsPtrs_.begin();
517 
518  // Iterate over the list of PHSs, updating the summed measure
519  // Reset the sum
520  summedMeasure_ = 0.0;
521  while (phsIter != listPhsPtrs_.end())
522  {
523  // Check if the specific PHS can ever be better than the given maxCost, i.e., if the distance between
524  // the foci is less than the current max cost
525  if ((*phsIter)->getMinTransverseDiameter() < maxCost.value())
526  {
527  // It can improve the solution, or it's the only PHS we have, update it
528 
529  // Update the transverse diameter
530  (*phsIter)->setTransverseDiameter(maxCost.value());
531 
532  // Increment the summed measure of the ellipses.
533  summedMeasure_ = summedMeasure_ + (*phsIter)->getPhsMeasure();
534 
535  // Increment the iterator
536  ++phsIter;
537  }
538  else if (listPhsPtrs_.size() > 1u)
539  {
540  // It can't, and it is not the last PHS, remove it
541 
542  // Remove the iterator to delete from the list, this returns the next:
544  phsIter = listPhsPtrs_.erase(phsIter);
545  }
546  else
547  {
548  // It can't, but it's the last PHS, so we can't remove it.
549 
550  // Make sure it's transverse diameter is set to something:
551  (*phsIter)->setTransverseDiameter((*phsIter)->getMinTransverseDiameter());
552 
553  // Set the summed measure to 0.0 (as a degenerate PHS is a line):
554  summedMeasure_ = 0.0;
555 
556  // Increment the iterator so we move past this to the end.
557  ++phsIter;
558  }
559  }
560  }
561 
562  ompl::ProlateHyperspheroidPtr PathLengthDirectInfSampler::randomPhsPtr()
563  {
564  // Variable
565  // The return value
566  ompl::ProlateHyperspheroidPtr rval;
567 
568  // If we only have one PHS, this can be simplified:
569  if (listPhsPtrs_.size() == 1u)
570  {
571  // One PHS, keep this simple.
572 
573  // Return it
574  rval = listPhsPtrs_.front();
575  }
576  else
577  {
578  // We have more than one PHS to consider
579 
580  // Variables
581  // A randomly generated number in the interval [0,1]
582  double randDbl = rng_.uniform01();
583  // The running measure
584  double runningRelativeMeasure = 0.0;
585 
586  // The probability of using each PHS is weighted by it's measure. Therefore, if we iterate up the list
587  // of PHSs, the first one who's relative measure is greater than the PHS randomly selected
588  for (std::list<ompl::ProlateHyperspheroidPtr>::const_iterator phsIter = listPhsPtrs_.begin();
589  phsIter != listPhsPtrs_.end() && !static_cast<bool>(rval); ++phsIter)
590  {
591  // Update the running measure
592  runningRelativeMeasure = runningRelativeMeasure + (*phsIter)->getPhsMeasure() / summedMeasure_;
593 
594  // Check if it's now greater than the proportion of the summed measure
595  if (runningRelativeMeasure > randDbl)
596  {
597  // It is, return this PHS:
598  rval = *phsIter;
599  }
600  // No else, continue
601  }
602  }
603 
604  // Return
605  return rval;
606  }
607 
608  bool PathLengthDirectInfSampler::keepSample(const std::vector<double> &informedVector)
609  {
610  // Variable
611  // The return value, do we keep this sample? Start true.
612  bool keep = true;
613 
614  // Is there more than 1 goal?
615  if (listPhsPtrs_.size() > 1u)
616  {
617  // There is, do work
618 
619  // Variable
620  // The number of PHSs the sample is in
621  unsigned int numIn = numberOfPhsInclusions(informedVector);
622  // The random number between [0,1]
623  double randDbl = rng_.uniform01();
624 
625  // Keep the sample if the random number is less than 1/K
626  keep = (randDbl <= 1.0 / static_cast<double>(numIn));
627  }
628  // No else, keep is true by default.
629 
630  return keep;
631  }
632 
633  bool PathLengthDirectInfSampler::isInAnyPhs(const std::vector<double> &informedVector) const
634  {
635  // Variable
636  // The return value, whether the given state is in any PHS
637  bool inPhs = false;
638 
639  // Iterate over the list, stopping as soon as we get our first true
640  for (auto phsIter = listPhsPtrs_.begin(); phsIter != listPhsPtrs_.end() && !inPhs; ++phsIter)
641  {
642  inPhs = isInPhs(*phsIter, informedVector);
643  }
644 
645  return inPhs;
646  }
647 
648  bool PathLengthDirectInfSampler::isInPhs(const ProlateHyperspheroidCPtr &phsCPtr,
649  const std::vector<double> &informedVector) const
650  {
651  return phsCPtr->isInPhs(&informedVector[0]);
652  }
653 
654  unsigned int PathLengthDirectInfSampler::numberOfPhsInclusions(const std::vector<double> &informedVector) const
655  {
656  // Variable
657  // The return value, the number of PHSs the vector is in
658  unsigned int numInclusions = 0u;
659 
660  // Iterate over the list counting
661  for (const auto &phsPtr : listPhsPtrs_)
662  {
663  // Conditionally increment
664  if (phsPtr->isInPhs(&informedVector[0]))
665  {
666  ++numInclusions;
667  }
668  // No else
669  }
670 
671  return numInclusions;
672  }
674  }; // base
675 }; // 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.
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.
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...
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:66
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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.
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
Unset type; this is the default type.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56