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 }
00046
00047 bool ompl::base::GaussianValidStateSampler::sample(State *state)
00048 {
00049 bool result = false;
00050 unsigned int attempts = 0;
00051 State *temp = si_->allocState();
00052 do
00053 {
00054 sampler_->sampleUniform(state);
00055 bool v1 = si_->isValid(state);
00056 sampler_->sampleGaussian(temp, state, stddev_);
00057 bool v2 = si_->isValid(temp);
00058 if (v1 != v2)
00059 {
00060 if (v2)
00061 si_->copyState(state, temp);
00062 result = true;
00063 }
00064 ++attempts;
00065 } while (!result && attempts < attempts_);
00066 si_->freeState(temp);
00067 return result;
00068 }
00069
00070 bool ompl::base::GaussianValidStateSampler::sampleNear(State *state, const State *near, const double distance)
00071 {
00072 bool result = false;
00073 unsigned int attempts = 0;
00074 State *temp = si_->allocState();
00075 do
00076 {
00077 sampler_->sampleUniformNear(state, near, distance);
00078 bool v1 = si_->isValid(state);
00079 sampler_->sampleGaussian(temp, state, distance);
00080 bool v2 = si_->isValid(temp);
00081 if (v1 != v2)
00082 {
00083 if (v2)
00084 si_->copyState(state, temp);
00085 result = true;
00086 }
00087 ++attempts;
00088 } while (!result && attempts < attempts_);
00089 si_->freeState(temp);
00090 return result;
00091 }