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/geometric/ik/GAIK.h"
00038 #include "ompl/util/Time.h"
00039 #include "ompl/util/Exception.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <algorithm>
00042 #include <limits>
00043
00044 ompl::geometric::GAIK::GAIK(const base::SpaceInformationPtr &si) : hcik_(si), si_(si), poolSize_(100), poolMutation_(20), poolRandom_(30),
00045 generations_(0), tryImprove_(false), maxDistance_(0.0)
00046 {
00047 hcik_.setMaxImproveSteps(3);
00048 setValidityCheck(true);
00049 }
00050
00051 ompl::geometric::GAIK::~GAIK(void)
00052 {
00053 for (unsigned int i = 0 ; i < pool_.size() ; ++i)
00054 si_->freeState(pool_[i].state);
00055 }
00056
00057 bool ompl::geometric::GAIK::solve(double solveTime, const base::GoalRegion &goal, base::State *result, const std::vector<base::State*> &hint)
00058 {
00059 if (maxDistance_ < std::numeric_limits<double>::epsilon())
00060 {
00061 tools::SelfConfig sc(si_, "GAIK");
00062 sc.configurePlannerRange(maxDistance_);
00063 }
00064
00065 if (poolSize_ < 1)
00066 {
00067 logError("Pool size too small");
00068 return false;
00069 }
00070
00071 time::point endTime = time::now() + time::seconds(solveTime);
00072
00073 unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
00074 IndividualSort gs;
00075 bool solved = false;
00076 int solution = -1;
00077
00078 if (!sampler_)
00079 sampler_ = si_->allocStateSampler();
00080
00081 if (pool_.empty())
00082 {
00083 generations_ = 1;
00084 pool_.resize(maxPoolSize);
00085
00086 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
00087 for (unsigned int i = 0 ; i < nh ; ++i)
00088 {
00089 pool_[i].state = si_->cloneState(hint[i]);
00090 si_->enforceBounds(pool_[i].state);
00091 pool_[i].valid = valid(pool_[i].state);
00092 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00093 {
00094 if (pool_[i].valid)
00095 {
00096 solved = true;
00097 solution = i;
00098 }
00099 }
00100 }
00101
00102
00103 unsigned int nh2 = nh * 2;
00104 if (nh2 < maxPoolSize)
00105 {
00106 for (unsigned int i = nh ; i < nh2 ; ++i)
00107 {
00108 pool_[i].state = si_->allocState();
00109 sampler_->sampleUniformNear(pool_[i].state, pool_[i % nh].state, maxDistance_);
00110 pool_[i].valid = valid(pool_[i].state);
00111 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00112 {
00113 if (pool_[i].valid)
00114 {
00115 solved = true;
00116 solution = i;
00117 }
00118 }
00119 }
00120 nh = nh2;
00121 }
00122
00123
00124 for (unsigned int i = nh ; i < maxPoolSize ; ++i)
00125 {
00126 pool_[i].state = si_->allocState();
00127 sampler_->sampleUniform(pool_[i].state);
00128 pool_[i].valid = valid(pool_[i].state);
00129 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00130 {
00131 if (pool_[i].valid)
00132 {
00133 solved = true;
00134 solution = i;
00135 }
00136 }
00137 }
00138 }
00139 else
00140 {
00141 std::size_t initialSize = pool_.size();
00142
00143 for (std::size_t i = maxPoolSize ; i < initialSize ; ++i)
00144 si_->freeState(pool_[i].state);
00145 pool_.resize(maxPoolSize);
00146
00147 for (std::size_t i = initialSize ; i < pool_.size() ; ++i)
00148 pool_[i].state = si_->allocState();
00149
00150
00151 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
00152 for (unsigned int i = 0 ; i < nh ; ++i)
00153 {
00154 std::size_t pi = maxPoolSize - i - 1;
00155 si_->copyState(pool_[pi].state, hint[i]);
00156 si_->enforceBounds(pool_[pi].state);
00157 pool_[pi].valid = valid(pool_[pi].state);
00158 if (goal.isSatisfied(pool_[pi].state, &(pool_[pi].distance)))
00159 {
00160 if (pool_[pi].valid)
00161 {
00162 solved = true;
00163 solution = pi;
00164 }
00165 }
00166 }
00167
00168
00169 nh = maxPoolSize - nh;
00170 for (std::size_t i = initialSize ; i < nh ; ++i)
00171 {
00172 sampler_->sampleUniform(pool_[i].state);
00173 pool_[i].valid = valid(pool_[i].state);
00174 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00175 {
00176 if (pool_[i].valid)
00177 {
00178 solved = true;
00179 solution = i;
00180 }
00181 }
00182 }
00183 }
00184
00185
00186 unsigned int mutationsSize = poolSize_ + poolMutation_;
00187
00188 while (!solved && time::now() < endTime)
00189 {
00190 generations_++;
00191 std::sort(pool_.begin(), pool_.end(), gs);
00192
00193
00194 for (unsigned int i = poolSize_ ; i < mutationsSize ; ++i)
00195 {
00196 sampler_->sampleUniformNear(pool_[i].state, pool_[i % poolSize_].state, maxDistance_);
00197 pool_[i].valid = valid(pool_[i].state);
00198 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00199 {
00200 if (pool_[i].valid)
00201 {
00202 solved = true;
00203 solution = i;
00204 break;
00205 }
00206 }
00207 }
00208
00209
00210 if (!solved)
00211 for (unsigned int i = mutationsSize ; i < maxPoolSize ; ++i)
00212 {
00213 sampler_->sampleUniform(pool_[i].state);
00214 pool_[i].valid = valid(pool_[i].state);
00215 if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
00216 {
00217 if (pool_[i].valid)
00218 {
00219 solved = true;
00220 solution = i;
00221 break;
00222 }
00223 }
00224 }
00225 }
00226
00227
00228
00229 logInform("Ran for %u generations", generations_);
00230
00231 if (solved)
00232 {
00233
00234 si_->copyState(result, pool_[solution].state);
00235
00236
00237 if (tryImprove_)
00238 tryToImprove(goal, result, pool_[solution].distance);
00239
00240
00241 if (!valid(result))
00242 si_->copyState(result, pool_[solution].state);
00243 }
00244 else
00245 if (tryImprove_)
00246 {
00247
00248 std::sort(pool_.begin(), pool_.end(), gs);
00249 for (unsigned int i = 0 ; i < 5 ; ++i)
00250 {
00251
00252 if (pool_[i].valid)
00253 {
00254
00255 si_->copyState(result, pool_[i].state);
00256
00257
00258 tryToImprove(goal, result, pool_[i].distance);
00259
00260
00261 if (!valid(result))
00262 si_->copyState(result, pool_[i].state);
00263 else
00264 solved = goal.isSatisfied(result);
00265 if (solved)
00266 break;
00267 }
00268 }
00269 }
00270
00271 return solved;
00272 }
00273
00274 void ompl::geometric::GAIK::tryToImprove(const base::GoalRegion &goal, base::State *state, double distance)
00275 {
00276 logDebug("Distance to goal before improvement: %g", distance);
00277 time::point start = time::now();
00278 double dist = si_->getMaximumExtent() / 10.0;
00279 hcik_.tryToImprove(goal, state, dist, &distance);
00280 hcik_.tryToImprove(goal, state, dist / 3.0, &distance);
00281 hcik_.tryToImprove(goal, state, dist / 10.0, &distance);
00282 logDebug("Improvement took %u ms", (time::now() - start).total_milliseconds());
00283 logDebug("Distance to goal after improvement: %g", distance);
00284 }
00285
00286 void ompl::geometric::GAIK::clear(void)
00287 {
00288 generations_ = 0;
00289 pool_.clear();
00290 sampler_.reset();
00291 }