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 <algorithm>
00041 #include <limits>
00042
00043 bool ompl::geometric::GAIK::solve(double solveTime, const base::GoalRegion &goal, base::State *result, const std::vector<base::State*> &hint)
00044 {
00045 if (maxDistance_ < std::numeric_limits<double>::epsilon())
00046 {
00047 maxDistance_ = si_->getMaximumExtent() / 5.0;
00048 msg_.inform("The maximum distance for sampling around states is assumed to be %f", maxDistance_);
00049 }
00050
00051 if (poolSize_ < 1)
00052 {
00053 msg_.error("Pool size too small");
00054 return false;
00055 }
00056
00057 time::point endTime = time::now() + time::seconds(solveTime);
00058
00059 unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
00060 std::vector<Individual> pool(maxPoolSize);
00061 IndividualSort gs;
00062 bool solved = false;
00063 int solution = -1;
00064
00065 base::StateSamplerPtr sampler = si_->allocStateSampler();
00066
00067
00068 unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
00069 for (unsigned int i = 0 ; i < nh ; ++i)
00070 {
00071 pool[i].state = si_->cloneState(hint[i]);
00072 si_->enforceBounds(pool[i].state);
00073 pool[i].valid = valid(pool[i].state);
00074 if (goal.isSatisfied(pool[i].state, &(pool[i].distance)))
00075 {
00076 if (pool[i].valid)
00077 {
00078 solved = true;
00079 solution = i;
00080 }
00081 }
00082 }
00083
00084
00085 unsigned int nh2 = nh * 2;
00086 if (nh2 < maxPoolSize)
00087 {
00088 for (unsigned int i = nh ; i < nh2 ; ++i)
00089 {
00090 pool[i].state = si_->allocState();
00091 sampler->sampleUniformNear(pool[i].state, pool[i % nh].state, maxDistance_);
00092 pool[i].valid = valid(pool[i].state);
00093 if (goal.isSatisfied(pool[i].state, &(pool[i].distance)))
00094 {
00095 if (pool[i].valid)
00096 {
00097 solved = true;
00098 solution = i;
00099 }
00100 }
00101 }
00102 nh = nh2;
00103 }
00104
00105
00106 for (unsigned int i = nh ; i < maxPoolSize ; ++i)
00107 {
00108 pool[i].state = si_->allocState();
00109 sampler->sampleUniform(pool[i].state);
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
00121
00122 unsigned int generations = 1;
00123 unsigned int mutationsSize = poolSize_ + poolMutation_;
00124
00125 while (!solved && time::now() < endTime)
00126 {
00127 generations++;
00128 std::sort(pool.begin(), pool.end(), gs);
00129
00130
00131 for (unsigned int i = poolSize_ ; i < mutationsSize ; ++i)
00132 {
00133 sampler->sampleUniformNear(pool[i].state, pool[i % poolSize_].state, maxDistance_);
00134 pool[i].valid = valid(pool[i].state);
00135 if (goal.isSatisfied(pool[i].state, &(pool[i].distance)))
00136 {
00137 if (pool[i].valid)
00138 {
00139 solved = true;
00140 solution = i;
00141 break;
00142 }
00143 }
00144 }
00145
00146
00147 if (!solved)
00148 for (unsigned int i = mutationsSize ; i < maxPoolSize ; ++i)
00149 {
00150 sampler->sampleUniform(pool[i].state);
00151 pool[i].valid = valid(pool[i].state);
00152 if (goal.isSatisfied(pool[i].state, &(pool[i].distance)))
00153 {
00154 if (pool[i].valid)
00155 {
00156 solved = true;
00157 solution = i;
00158 break;
00159 }
00160 }
00161 }
00162 }
00163
00164
00165
00166 msg_.inform("Ran for %u generations", generations);
00167
00168 if (solved)
00169 {
00170
00171 si_->copyState(result, pool[solution].state);
00172
00173
00174 tryToImprove(goal, result, pool[solution].distance);
00175
00176
00177 if (!valid(result))
00178 si_->copyState(result, pool[solution].state);
00179 }
00180 else
00181 {
00182
00183 std::sort(pool.begin(), pool.end(), gs);
00184 for (unsigned int i = 0 ; i < 5 ; ++i)
00185 {
00186
00187 if (pool[i].valid)
00188 {
00189
00190 si_->copyState(result, pool[i].state);
00191
00192
00193 tryToImprove(goal, result, pool[i].distance);
00194
00195
00196 if (!valid(result))
00197 si_->copyState(result, pool[i].state);
00198 else
00199 solved = goal.isSatisfied(result);
00200 if (solved)
00201 break;
00202 }
00203 }
00204 }
00205
00206 for (unsigned int i = 0 ; i < maxPoolSize ; ++i)
00207 si_->freeState(pool[i].state);
00208
00209 return solved;
00210 }
00211
00212 bool ompl::geometric::GAIK::tryToImprove(const base::GoalRegion &goal, base::State *state, double distance)
00213 {
00214 msg_.debug("Distance to goal before improvement: %g", distance);
00215 time::point start = time::now();
00216 double dist = si_->getMaximumExtent() / 10.0;
00217 hcik_.tryToImprove(goal, state, dist, &distance);
00218 hcik_.tryToImprove(goal, state, dist / 3.0, &distance);
00219 hcik_.tryToImprove(goal, state, dist / 10.0, &distance);
00220 msg_.debug("Improvement took %u ms", (time::now() - start).total_milliseconds());
00221 msg_.debug("Distance to goal after improvement: %g", distance);
00222 return true;
00223 }