37 #include "ompl/base/PrecomputedStateSampler.h"
38 #include "ompl/base/StateSpace.h"
39 #include "ompl/util/Exception.h"
45 throw Exception(
"Empty set of states to sample from was specified");
51 StateSampler(space), states_(states), minStateIndex_(minStateIndex), maxStateIndex_(maxStateIndex)
54 throw Exception(
"Empty set of states to sample from was specified");
55 if (minStateIndex > maxStateIndex)
56 throw Exception(
"Minimum state index cannot be larger than maximum state index");
57 if (maxStateIndex >=
states_.size())
58 throw Exception(
"Index range out of bounds");
63 space_->copyState(state, states_[rng_.uniformInt(minStateIndex_, maxStateIndex_)]);
68 int index = rng_.uniformInt(minStateIndex_, maxStateIndex_);
69 double dist = space_->distance(near, states_[index]);
71 space_->interpolate(near, states_[index], distance / dist, state);
73 space_->copyState(state, states_[index]);
78 sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
const std::vector< const State * > & states_
The states to sample from.
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
std::size_t minStateIndex_
The minimum index to start sampling at.
std::size_t maxStateIndex_
The maximum index to stop sampling at.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev) ...
The exception type for ompl.
Abstract definition of a state space sampler.
virtual void sampleUniform(State *state)
Sample a state.
PrecomputedStateSampler(const StateSpace *space, const std::vector< const State * > &states)
Constructor. Takes the state space to be sampled (space) and the set of states to draw samples from (...