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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines