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
50namespace 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
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
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
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
The exception type for ompl.
Definition: Exception.h:47
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 uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:67
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:909
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:904
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Abstract definition of a goal region that can be sampled.
An abstract class for the concept of using information about the state space and the current solution...
OptimizationObjectivePtr opt_
A copy of the optimization objective.
ProblemDefinitionPtr probDefn_
A copy of the problem definition.
unsigned int numIters_
The number of iterations I'm allowed to attempt.
StateSpacePtr space_
A copy of the state space.
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...
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.
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...
PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)
Construct a sampler that only generates states with a heuristic solution estimate that is less than t...
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ STATE_SPACE_SE2
ompl::base::SE2StateSpace
@ STATE_SPACE_UNKNOWN
Unset type; this is the default type.
@ STATE_SPACE_REAL_VECTOR
ompl::base::RealVectorStateSpace
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
@ STATE_SPACE_SE3
ompl::base::SE3StateSpace
@ STATE_SPACE_SO3
ompl::base::SO3StateSpace
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
Main namespace. Contains everything in this library.