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