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/MaximizeClearanceValidStateSampler.h"
00038 #include "ompl/base/SpaceInformation.h"
00039
00040 ompl::base::MaximizeClearanceValidStateSampler::MaximizeClearanceValidStateSampler(const SpaceInformation *si) :
00041 ValidStateSampler(si), sampler_(si->allocStateSampler()), improveAttempts_(3), work_(si->allocState())
00042 {
00043 name_ = "max_clear_uniform";
00044 }
00045
00046 ompl::base::MaximizeClearanceValidStateSampler::~MaximizeClearanceValidStateSampler(void)
00047 {
00048 si_->freeState(work_);
00049 }
00050
00051 bool ompl::base::MaximizeClearanceValidStateSampler::sample(State *state)
00052 {
00053 unsigned int attempts = 0;
00054 bool valid = false;
00055 double dist = 0.0;
00056 do
00057 {
00058 sampler_->sampleUniform(state);
00059 valid = si_->getStateValidityChecker()->isValid(state, dist);
00060 ++attempts;
00061 } while (!valid && attempts < attempts_);
00062
00063 if (valid)
00064 {
00065 bool validW = false;
00066 double distW = 0.0;
00067 attempts = 0;
00068 while (attempts < improveAttempts_)
00069 {
00070 sampler_->sampleUniform(work_);
00071 validW = si_->getStateValidityChecker()->isValid(work_, distW);
00072 ++attempts;
00073 if (validW && distW > dist)
00074 {
00075 dist = distW;
00076 si_->copyState(state, work_);
00077 }
00078 }
00079 return true;
00080 }
00081 else
00082 return false;
00083 }
00084
00085 bool ompl::base::MaximizeClearanceValidStateSampler::sampleNear(State *state, const State *near, const double distance)
00086 {
00087 unsigned int attempts = 0;
00088 bool valid = false;
00089 double dist = 0.0;
00090 do
00091 {
00092 sampler_->sampleUniformNear(state, near, distance);
00093 valid = si_->getStateValidityChecker()->isValid(state, dist);
00094 ++attempts;
00095 } while (!valid && attempts < attempts_);
00096
00097 if (valid)
00098 {
00099 bool validW = false;
00100 double distW = 0.0;
00101 attempts = 0;
00102 while (attempts < improveAttempts_)
00103 {
00104 sampler_->sampleUniformNear(work_, near, distance);
00105 validW = si_->getStateValidityChecker()->isValid(work_, distW);
00106 ++attempts;
00107 if (validW && distW > dist)
00108 {
00109 dist = distW;
00110 si_->copyState(state, work_);
00111 }
00112 }
00113 return true;
00114 }
00115 else
00116 return false;
00117 }