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"
58 ompl::geometric::GeneticSearch::~GeneticSearch()
61 si_->freeState(i.state);
65 const std::vector<base::State *> &hint)
67 if (maxDistance_ < std::numeric_limits<double>::epsilon())
81 unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
87 sampler_ = si_->allocStateSampler();
92 pool_.resize(maxPoolSize);
94 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
95 for (
unsigned int i = 0; i < nh; ++i)
97 pool_[i].state = si_->cloneState(hint[i]);
98 si_->enforceBounds(pool_[i].state);
99 pool_[i].valid = valid(pool_[i].state);
100 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
111 unsigned int nh2 = nh * 2;
112 if (nh2 < maxPoolSize)
114 for (
unsigned int i = nh; i < nh2; ++i)
116 pool_[i].state = si_->allocState();
117 sampler_->sampleUniformNear(pool_[i].state, pool_[i % nh].state, maxDistance_);
118 pool_[i].valid = valid(pool_[i].state);
119 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
132 for (
unsigned int i = nh; i < maxPoolSize; ++i)
134 pool_[i].state = si_->allocState();
135 sampler_->sampleUniform(pool_[i].state);
136 pool_[i].valid = valid(pool_[i].state);
137 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
149 std::size_t initialSize = pool_.size();
151 for (std::size_t i = maxPoolSize; i < initialSize; ++i)
152 si_->freeState(pool_[i].state);
153 pool_.resize(maxPoolSize);
155 for (std::size_t i = initialSize; i < pool_.size(); ++i)
156 pool_[i].state = si_->allocState();
159 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
160 for (
unsigned int i = 0; i < nh; ++i)
162 std::size_t pi = maxPoolSize - i - 1;
163 si_->copyState(pool_[pi].state, hint[i]);
164 si_->enforceBounds(pool_[pi].state);
165 pool_[pi].valid = valid(pool_[pi].state);
166 if (goal.isSatisfied(pool_[pi].state, &(pool_[pi].distance)))
177 nh = maxPoolSize - nh;
178 for (std::size_t i = initialSize; i < nh; ++i)
180 sampler_->sampleUniform(pool_[i].state);
181 pool_[i].valid = valid(pool_[i].state);
182 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
194 unsigned int mutationsSize = poolSize_ + poolMutation_;
199 std::sort(pool_.begin(), pool_.end(), gs);
202 for (
unsigned int i = poolSize_; i < mutationsSize; ++i)
204 sampler_->sampleUniformNear(pool_[i].state, pool_[i % poolSize_].state, maxDistance_);
205 pool_[i].valid = valid(pool_[i].state);
206 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
219 for (
unsigned int i = mutationsSize; i < maxPoolSize; ++i)
221 sampler_->sampleUniform(pool_[i].state);
222 pool_[i].valid = valid(pool_[i].state);
223 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
236 OMPL_INFORM(
"Ran for %u generations", generations_);
241 si_->copyState(result, pool_[solution].state);
245 tryToImprove(goal, result, pool_[solution].distance);
249 si_->copyState(result, pool_[solution].state);
251 else if (tryImprove_)
254 std::sort(pool_.begin(), pool_.end(), gs);
255 for (
unsigned int i = 0; i < 5; ++i)
261 si_->copyState(result, pool_[i].state);
264 tryToImprove(goal, result, pool_[i].distance);
268 si_->copyState(result, pool_[i].state);
270 solved = goal.isSatisfied(result);
282 OMPL_DEBUG(
"Distance to goal before improvement: %g", distance);
284 double dist = si_->getMaximumExtent() / 10.0;
285 hc_.tryToImprove(goal, state, dist, &distance);
286 hc_.tryToImprove(goal, state, dist / 3.0, &distance);
287 hc_.tryToImprove(goal, state, dist / 10.0, &distance);
289 std::chrono::duration_cast<std::chrono::milliseconds>(
time::now() - start).count());
290 OMPL_DEBUG(
"Distance to goal after improvement: %g", distance);