ompl/geometric/src/GeneticSearch.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #include "ompl/geometric/GeneticSearch.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::GeneticSearch::GeneticSearch(const base::SpaceInformationPtr &si) : hc_(si), si_(si), poolSize_(100), poolMutation_(20), poolRandom_(30), 00045 generations_(0), tryImprove_(false), maxDistance_(0.0) 00046 { 00047 hc_.setMaxImproveSteps(3); 00048 setValidityCheck(true); 00049 } 00050 00051 ompl::geometric::GeneticSearch::~GeneticSearch() 00052 { 00053 for (unsigned int i = 0 ; i < pool_.size() ; ++i) 00054 si_->freeState(pool_[i].state); 00055 } 00056 00057 bool ompl::geometric::GeneticSearch::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_, "GeneticSearch"); 00062 sc.configurePlannerRange(maxDistance_); 00063 } 00064 00065 if (poolSize_ < 1) 00066 { 00067 OMPL_ERROR("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 // add hint states 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 // add states near the hint states 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 // add random states 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 // free extra memory if needed 00143 for (std::size_t i = maxPoolSize ; i < initialSize ; ++i) 00144 si_->freeState(pool_[i].state); 00145 pool_.resize(maxPoolSize); 00146 // alloc extra memory if needed 00147 for (std::size_t i = initialSize ; i < pool_.size() ; ++i) 00148 pool_[i].state = si_->allocState(); 00149 00150 // add hint states at the bottom of the pool 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 // add random states if needed 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 // run the genetic algorithm 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 // add mutations 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 // add random states 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 // fill in solution, if found 00229 OMPL_INFORM("Ran for %u generations", generations_); 00230 00231 if (solved) 00232 { 00233 // set the solution 00234 si_->copyState(result, pool_[solution].state); 00235 00236 // try to improve the solution 00237 if (tryImprove_) 00238 tryToImprove(goal, result, pool_[solution].distance); 00239 00240 // if improving the state made it invalid, revert 00241 if (!valid(result)) 00242 si_->copyState(result, pool_[solution].state); 00243 } 00244 else 00245 if (tryImprove_) 00246 { 00247 /* one last attempt to find a solution */ 00248 std::sort(pool_.begin(), pool_.end(), gs); 00249 for (unsigned int i = 0 ; i < 5 ; ++i) 00250 { 00251 // get a valid state that is closer to the goal 00252 if (pool_[i].valid) 00253 { 00254 // set the solution 00255 si_->copyState(result, pool_[i].state); 00256 00257 // try to improve the state 00258 tryToImprove(goal, result, pool_[i].distance); 00259 00260 // if the improvement made the state no longer valid, revert to previous one 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::GeneticSearch::tryToImprove(const base::GoalRegion &goal, base::State *state, double distance) 00275 { 00276 OMPL_DEBUG("Distance to goal before improvement: %g", distance); 00277 time::point start = time::now(); 00278 double dist = si_->getMaximumExtent() / 10.0; 00279 hc_.tryToImprove(goal, state, dist, &distance); 00280 hc_.tryToImprove(goal, state, dist / 3.0, &distance); 00281 hc_.tryToImprove(goal, state, dist / 10.0, &distance); 00282 OMPL_DEBUG("Improvement took %u ms", (time::now() - start).total_milliseconds()); 00283 OMPL_DEBUG("Distance to goal after improvement: %g", distance); 00284 } 00285 00286 void ompl::geometric::GeneticSearch::clear() 00287 { 00288 generations_ = 0; 00289 pool_.clear(); 00290 sampler_.reset(); 00291 }