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