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,
460 std::vector<double> PathLengthDirectInfSampler::getInformedSubstate(const State *statePtr) const
473 informedSubSpace_->copyToReals(rawData, statePtr->as<CompoundState>()->components[informedIdx_]);
479 void PathLengthDirectInfSampler::createFullState(State *statePtr, const std::vector<double> &informedVector)
481 // If there is an extra "uninformed" subspace, we need to add that to the state before converting the raw
523 // Check if the specific PHS can ever be better than the given maxCost, i.e., if the distance between
586 // The probability of using each PHS is weighted by it's measure. Therefore, if we iterate up the list
654 unsigned int PathLengthDirectInfSampler::numberOfPhsInclusions(const std::vector<double> &informedVector) const
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
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.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
OptimizationObjectivePtr opt_
A copy of the optimization objective.
Definition: InformedStateSampler.h:115
An abstract class for the concept of using information about the state space and the current solution...
Definition: InformedStateSampler.h:60
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
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:47
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.
Definition: PathLengthDirectInfSampler.cpp:295
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound 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...
Definition: PathLengthDirectInfSampler.cpp:249
ompl::base::RealVectorStateSpace
Definition: StateSpaceTypes.h:52
ProblemDefinitionPtr probDefn_
A copy of the problem definition.
Definition: InformedStateSampler.h:113
unsigned int numIters_
The number of iterations I'm allowed to attempt.
Definition: InformedStateSampler.h:119
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
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.
Definition: StateSpaceTypes.h:49
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56