PathLengthDirectInfSampler.cpp
67 // The foci of the PHSs as a std::vector of states. Goals must be nonconst, as we need to allocate them
74 throw Exception("PathLengthDirectInfSampler: The direct path-length informed sampler currently only "
89 throw Exception("PathLengthDirectInfSampler: There must be at least 1 start and and 1 goal state when "
107 OMPL_WARN("PathLengthDirectInfSampler: Treating the StateSpace of type \"STATE_SPACE_UNKNOWN\" as type \"STATE_SPACE_REAL_VECTOR\".");
113 throw Exception("PathLengthDirectInfSampler only supports Unknown, RealVector, SE2, and SE3 StateSpaces.");
161 throw Exception("PathLengthDirectInfSampler only supports RealVector, SE2 and SE3 statespaces.");
185 informedSubSpace_ = InformedSampler::space_->as<CompoundStateSpace>()->getSubspace(informedIdx_);
188 uninformedSubSpace_ = InformedSampler::space_->as<CompoundStateSpace>()->getSubspace(uninformedIdx_);
240 OMPL_WARN("PathLengthDirectInfSampler: Rejection sampling is used in order to maintain uniform density "
259 bool PathLengthDirectInfSampler::sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost)
262 // Since volume in a sphere/spheroid is proportionately concentrated near the surface, this isn't horribly
306 // The informed measure is then the sum of the measure of the individual PHSs for the given cost:
309 // It is nonsensical for a PHS to have a transverse diameter less than the distance between its foci, so
350 bool PathLengthDirectInfSampler::sampleUniform(State *statePtr, const Cost &maxCost, unsigned int *iters)
376 // When the summed measure of the PHSes are suitably large, it makes more sense to just sample from the
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()))
428 // Due to the possibility of overlap between multiple PHSs, we keep a sample with a probability of 1/K,
463 std::vector<double> PathLengthDirectInfSampler::getInformedSubstate(const State *statePtr) const
476 informedSubSpace_->copyToReals(rawData, statePtr->as<CompoundState>()->components[informedIdx_]);
482 void PathLengthDirectInfSampler::createFullState(State *statePtr, const std::vector<double> &informedVector)
484 // If there is an extra "uninformed" subspace, we need to add that to the state before converting the raw
526 // Check if the specific PHS can ever be better than the given maxCost, i.e., if the distance between
589 // The probability of using each PHS is weighted by it's measure. Therefore, if we iterate up the list
A space to allow the composition of state spaces.
Definition: StateSpace.h:574
ProblemDefinitionPtr probDefn_
A copy of the problem definition.
Definition: InformedStateSampler.h:113
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
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: PathLengthDirectInfSampler.cpp:58
double getInformedMeasure(const Cost ¤tCost) const override
The measure of the subset of the state space defined by the current solution cost that is being searc...
Definition: PathLengthDirectInfSampler.cpp:300
Cost heuristicSolnCost(const State *statePtr) const override
A helper function to calculate the heuristic estimate of the solution cost for the informed subset of...
Definition: PathLengthDirectInfSampler.cpp:328
A shared pointer wrapper for ompl::base::ProblemDefinition.
OptimizationObjectivePtr opt_
A copy of the optimization objective.
Definition: InformedStateSampler.h:115
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
void uniformProlateHyperspheroid(const std::shared_ptr< const ProlateHyperspheroid > &phsPtr, double value[])
Uniform random sampling of a prolate hyperspheroid, a special symmetric type of n-dimensional ellipse...
Definition: RandomNumbers.cpp:321
An abstract class for the concept of using information about the state space and the current solution...
Definition: InformedStateSampler.h:61
unsigned int numIters_
The number of iterations I'm allowed to attempt.
Definition: InformedStateSampler.h:119
bool hasInformedMeasure() const override
Whether the sampler can provide a measure of the informed subset.
Definition: PathLengthDirectInfSampler.cpp:295
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:48
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...
Definition: PathLengthDirectInfSampler.cpp:249