00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/StateSampler.h"
00038 #include "ompl/base/StateSpace.h"
00039
00040 void ompl::base::CompoundStateSampler::addSampler(const StateSamplerPtr &sampler, double weightImportance)
00041 {
00042 samplers_.push_back(sampler);
00043 weightImportance_.push_back(weightImportance);
00044 samplerCount_ = samplers_.size();
00045 }
00046
00047 void ompl::base::CompoundStateSampler::sampleUniform(State *state)
00048 {
00049 State **comps = state->as<CompoundState>()->components;
00050 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00051 samplers_[i]->sampleUniform(comps[i]);
00052 }
00053
00054 void ompl::base::CompoundStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00055 {
00056 State **comps = state->as<CompoundState>()->components;
00057 State **nearComps = near->as<CompoundState>()->components;
00058 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00059 if (weightImportance_[i] > std::numeric_limits<double>::epsilon())
00060 samplers_[i]->sampleUniformNear(comps[i], nearComps[i], distance * weightImportance_[i]);
00061 else
00062 samplers_[i]->sampleUniform(comps[i]);
00063
00064 }
00065
00066 void ompl::base::CompoundStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00067 {
00068 State **comps = state->as<CompoundState>()->components;
00069 State **meanComps = mean->as<CompoundState>()->components;
00070 for (unsigned int i = 0 ; i < samplerCount_ ; ++i)
00071 samplers_[i]->sampleGaussian(comps[i], meanComps[i], stdDev * weightImportance_[i]);
00072 }
00073
00074 ompl::base::SubspaceStateSampler::SubspaceStateSampler(const StateSpace *space, const StateSpace *subspace, double weight) : StateSampler(space), subspace_(subspace), weight_(weight)
00075 {
00076 work_ = subspace_->allocState();
00077 work2_ = subspace_->allocState();
00078 subspaceSampler_ = subspace_->allocStateSampler();
00079 space_->getCommonSubspaces(subspace_, subspaces_);
00080 if (subspaces_.empty())
00081 logWarn("Subspace state sampler did not find any common subspaces. Sampling will have no effect.");
00082 }
00083
00084 ompl::base::SubspaceStateSampler::~SubspaceStateSampler(void)
00085 {
00086 subspace_->freeState(work_);
00087 subspace_->freeState(work2_);
00088 }
00089
00090 void ompl::base::SubspaceStateSampler::sampleUniform(State *state)
00091 {
00092 subspaceSampler_->sampleUniform(work_);
00093 copyStateData(space_, state, subspace_, work_, subspaces_);
00094 }
00095
00096 void ompl::base::SubspaceStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00097 {
00098 copyStateData(subspace_, work2_, space_, near);
00099 subspaceSampler_->sampleUniformNear(work_, work2_, distance * weight_);
00100 copyStateData(space_, state, subspace_, work_, subspaces_);
00101 }
00102
00103 void ompl::base::SubspaceStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00104 {
00105 copyStateData(subspace_, work2_, space_, mean);
00106 subspaceSampler_->sampleGaussian(work_, work2_, stdDev * weight_);
00107 copyStateData(space_, state, subspace_, work_, subspaces_);
00108 }