Class PathLengthDirectInfSampler

Inheritance Relationships

Base Type

Class Documentation

class PathLengthDirectInfSampler : public ompl::base::InformedSampler

An informed sampler for problems seeking to minimize path length.

PathLengthDirectInfSampler is a method to generate uniform samples in the subset of a problem that could provide a shorter path from start to goal. This subset is a prolate hyperspheroid (PHS), a special type of an hyperellipsoid) and can be sampled directly.

Informed sampling is a method to focus search which continuing to consider all homotopy classes that can provide a better solution. Directly sampling the informed subset guarantees a non-zero probability of improving a solution regardless of the size of the planning domain, the number of state dimensions, and how close the current solution is to the theoretical minimum.

Currently only implemented for problems in R^n (i.e., RealVectorStateSpace), SE(2) (i.e., SE2StateSpace), and SE(3) (i.e., SE3StateSpace). Until an initial solution is found, this sampler simply passes-through to a uniform distribution over the entire state space.

J. D. Gammell, T. D. Barfoot, S. S. Srinivasa, “Informed sampling for asymptotically optimal path planning.” IEEE Transactions on Robotics (T-RO), 34(4): 966-984, Aug. 2018. DOI: TRO.2018.2830331. arXiv: 1706.06454 [cs.RO]. Illustration video. Short description video.

Associated publication:

Public Functions

PathLengthDirectInfSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls)

Construct a sampler that only generates states with a heuristic solution estimate that is less than the cost of the current solution using a direct ellipsoidal method.

~PathLengthDirectInfSampler() override
virtual bool sampleUniform(State *statePtr, const Cost &maxCost) override

Sample uniformly in the subset of the state space whose heuristic solution estimates are less than the provided cost, i.e. in the interval [0, maxCost). Returns false if such a state was not found in the specified number of iterations.

virtual bool sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost) override

Sample uniformly in the subset of the state space whose heuristic solution estimates are between the provided costs, [minCost, maxCost). Returns false if such a state was not found in the specified number of iterations.

virtual bool hasInformedMeasure() const override

Whether the sampler can provide a measure of the informed subset.

virtual 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 searched. Does not consider problem boundaries but returns the measure of the entire space if no solution has been found. In the case of multiple goals, this measure assume each individual subset is independent, therefore the resulting measure will be an overestimate if any of the subsets overlap.

virtual Cost heuristicSolnCost(const State *statePtr) const override

A helper function to calculate the heuristic estimate of the solution cost for the informed subset of a given state.