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/samplers/GaussianValidStateSampler.h"
00038 #include "ompl/base/SpaceInformation.h"
00039 #include "ompl/tools/config/MagicConstants.h"
00040
00041 ompl::base::GaussianValidStateSampler::GaussianValidStateSampler(const SpaceInformation *si) :
00042 ValidStateSampler(si), sampler_(si->allocStateSampler()), stddev_(si->getMaximumExtent() * magic::STD_DEV_AS_SPACE_EXTENT_FRACTION)
00043 {
00044 name_ = "gaussian";
00045 params_.declareParam<double>("standard_deviation",
00046 boost::bind(&GaussianValidStateSampler::setStdDev, this, _1),
00047 boost::bind(&GaussianValidStateSampler::getStdDev, this));
00048 }
00049
00050 bool ompl::base::GaussianValidStateSampler::sample(State *state)
00051 {
00052 bool result = false;
00053 unsigned int attempts = 0;
00054 State *temp = si_->allocState();
00055 do
00056 {
00057 sampler_->sampleUniform(state);
00058 bool v1 = si_->isValid(state);
00059 sampler_->sampleGaussian(temp, state, stddev_);
00060 bool v2 = si_->isValid(temp);
00061 if (v1 != v2)
00062 {
00063 if (v2)
00064 si_->copyState(state, temp);
00065 result = true;
00066 }
00067 ++attempts;
00068 } while (!result && attempts < attempts_);
00069 si_->freeState(temp);
00070 return result;
00071 }
00072
00073 bool ompl::base::GaussianValidStateSampler::sampleNear(State *state, const State *near, const double distance)
00074 {
00075 bool result = false;
00076 unsigned int attempts = 0;
00077 State *temp = si_->allocState();
00078 do
00079 {
00080 sampler_->sampleUniformNear(state, near, distance);
00081 bool v1 = si_->isValid(state);
00082 sampler_->sampleGaussian(temp, state, distance);
00083 bool v2 = si_->isValid(temp);
00084 if (v1 != v2)
00085 {
00086 if (v2)
00087 si_->copyState(state, temp);
00088 result = true;
00089 }
00090 ++attempts;
00091 } while (!result && attempts < attempts_);
00092 si_->freeState(temp);
00093 return result;
00094 }