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;
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.
virtual unsigned int maxSampleCount() const =0
Return the maximum number of samples that can be asked for before repeating.
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.