37 #include "ompl/base/StateSampler.h"
38 #include "ompl/base/StateSpace.h"
50 for (
unsigned int i = 0 ; i < samplerCount_ ; ++i)
51 samplers_[i]->sampleUniform(comps[i]);
58 for (
unsigned int i = 0 ; i < samplerCount_ ; ++i)
59 if (weightImportance_[i] > std::numeric_limits<double>::epsilon())
60 samplers_[i]->sampleUniformNear(comps[i], nearComps[i], distance * weightImportance_[i]);
62 samplers_[i]->sampleUniform(comps[i]);
70 for (
unsigned int i = 0 ; i < samplerCount_ ; ++i)
71 samplers_[i]->sampleGaussian(comps[i], meanComps[i], stdDev * weightImportance_[i]);
81 OMPL_WARN(
"Subspace state sampler did not find any common subspaces. Sampling will have no effect.");
84 ompl::base::SubspaceStateSampler::~SubspaceStateSampler(
void)
86 subspace_->freeState(work_);
87 subspace_->freeState(work2_);
92 subspaceSampler_->sampleUniform(work_);
99 subspaceSampler_->sampleUniformNear(work_, work2_, distance * weight_);
106 subspaceSampler_->sampleGaussian(work_, work2_, stdDev * weight_);
const StateSpace * space_
The state space this sampler samples.
Definition of a compound state.
std::vector< std::string > subspaces_
The names of common subspaces between space_ and subspace_; these are the ones copied after sampling ...
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) ...
virtual void sampleUniform(State *state)
Sample a state.
A boost shared pointer wrapper for ompl::base::StateSampler.
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
std::vector< StateSamplerPtr > samplers_
The samplers that are composed.
StateSamplerPtr subspaceSampler_
The sampler for the subspace.
SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight)
Construct a sampler for space but only sample components common to subspace. Use weight as a multipli...
const T * as(void) const
Cast this instance to a desired type.
virtual void addSampler(const StateSamplerPtr &sampler, double weightImportance)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Call sampleGaussian for each of the subspace states with stdDev scaled by the corresponding subspace ...
const StateSpace * subspace_
The subspace to sample.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Call sampleUniformNear for each of the subspace states with distance scaled by the corresponding subs...
virtual State * allocState(void) const =0
Allocate a state that can store a point in the described space.
virtual StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
void getCommonSubspaces(const StateSpacePtr &other, std::vector< std::string > &subspaces) const
Get the set of subspaces that this space and other have in common. The computed list of subspaces doe...
virtual void sampleUniform(State *state)
Sample a state.
Abstract definition of a state space sampler.
std::vector< double > weightImportance_
The weight of each sampler (used when sampling near a state)