37 #include "ompl/geometric/ik/GAIK.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::GAIK::~GAIK(
void)
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;
279 hcik_.tryToImprove(goal, state, dist, &distance);
280 hcik_.tryToImprove(goal, state, dist / 3.0, &distance);
281 hcik_.tryToImprove(goal, state, dist / 10.0, &distance);
283 OMPL_DEBUG(
"Distance to goal after improvement: %g", distance);