37 #include "ompl/geometric/GeneticSearch.h" 38 #include "ompl/util/Time.h" 39 #include "ompl/util/Exception.h" 40 #include "ompl/tools/config/SelfConfig.h" 45 generations_(0), tryImprove_(false), maxDistance_(0.0)
51 ompl::geometric::GeneticSearch::~GeneticSearch()
53 for (
unsigned int i = 0 ; i < pool_.size() ; ++i)
54 si_->freeState(pool_[i].state);
59 if (maxDistance_ < std::numeric_limits<double>::epsilon())
73 unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
79 sampler_ = si_->allocStateSampler();
84 pool_.resize(maxPoolSize);
86 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
87 for (
unsigned int i = 0 ; i < nh ; ++i)
89 pool_[i].state = si_->cloneState(hint[i]);
90 si_->enforceBounds(pool_[i].state);
91 pool_[i].valid = valid(pool_[i].state);
92 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
103 unsigned int nh2 = nh * 2;
104 if (nh2 < maxPoolSize)
106 for (
unsigned int i = nh ; i < nh2 ; ++i)
108 pool_[i].state = si_->allocState();
109 sampler_->sampleUniformNear(pool_[i].state, pool_[i % nh].state, maxDistance_);
110 pool_[i].valid = valid(pool_[i].state);
111 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
124 for (
unsigned int i = nh ; i < maxPoolSize ; ++i)
126 pool_[i].state = si_->allocState();
127 sampler_->sampleUniform(pool_[i].state);
128 pool_[i].valid = valid(pool_[i].state);
129 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
141 std::size_t initialSize = pool_.size();
143 for (std::size_t i = maxPoolSize ; i < initialSize ; ++i)
144 si_->freeState(pool_[i].state);
145 pool_.resize(maxPoolSize);
147 for (std::size_t i = initialSize ; i < pool_.size() ; ++i)
148 pool_[i].state = si_->allocState();
151 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
152 for (
unsigned int i = 0 ; i < nh ; ++i)
154 std::size_t pi = maxPoolSize - i - 1;
155 si_->copyState(pool_[pi].state, hint[i]);
156 si_->enforceBounds(pool_[pi].state);
157 pool_[pi].valid = valid(pool_[pi].state);
158 if (goal.
isSatisfied(pool_[pi].state, &(pool_[pi].distance)))
169 nh = maxPoolSize - nh;
170 for (std::size_t i = initialSize ; i < nh ; ++i)
172 sampler_->sampleUniform(pool_[i].state);
173 pool_[i].valid = valid(pool_[i].state);
174 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
186 unsigned int mutationsSize = poolSize_ + poolMutation_;
191 std::sort(pool_.begin(), pool_.end(), gs);
194 for (
unsigned int i = poolSize_ ; i < mutationsSize ; ++i)
196 sampler_->sampleUniformNear(pool_[i].state, pool_[i % poolSize_].state, maxDistance_);
197 pool_[i].valid = valid(pool_[i].state);
198 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
211 for (
unsigned int i = mutationsSize ; i < maxPoolSize ; ++i)
213 sampler_->sampleUniform(pool_[i].state);
214 pool_[i].valid = valid(pool_[i].state);
215 if (goal.
isSatisfied(pool_[i].state, &(pool_[i].distance)))
229 OMPL_INFORM(
"Ran for %u generations", generations_);
234 si_->copyState(result, pool_[solution].state);
238 tryToImprove(goal, result, pool_[solution].distance);
242 si_->copyState(result, pool_[solution].state);
248 std::sort(pool_.begin(), pool_.end(), gs);
249 for (
unsigned int i = 0 ; i < 5 ; ++i)
255 si_->copyState(result, pool_[i].state);
258 tryToImprove(goal, result, pool_[i].distance);
262 si_->copyState(result, pool_[i].state);
276 OMPL_DEBUG(
"Distance to goal before improvement: %g", distance);
278 double dist = si_->getMaximumExtent() / 10.0;
283 OMPL_DEBUG(
"Distance to goal after improvement: %g", distance);
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
virtual bool isSatisfied(const State *st) const
Equivalent to calling isSatisfied(const State *, double *) with a NULL second argument.
void setMaxImproveSteps(unsigned int steps)
Set the number of steps to perform.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
boost::posix_time::ptime point
Representation of a point in time.
bool solve(double solveTime, const base::GoalRegion &goal, base::State *result, const std::vector< base::State *> &hint=std::vector< base::State *>())
Find a state that fits the request.
bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance=NULL) const
Try to improve a state (reduce distance to goal). The updates are performed by sampling near the stat...
Definition of an abstract state.
GeneticSearch(const base::SpaceInformationPtr &si)
Construct an instance of a genetic algorithm for inverse kinematics given the space information to se...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
point now()
Get the current time point.
Definition of a goal region.
void clear()
Clear the pool of samples.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.