All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
GAIK.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/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     // add hint states
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     // add states near the hint states
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     // add random states
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     // run the genetic algorithm
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         // add mutations
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         // add random states
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     // fill in solution, if found
00166     msg_.inform("Ran for %u generations", generations);
00167 
00168     if (solved)
00169     {
00170         // set the solution
00171         si_->copyState(result, pool[solution].state);
00172 
00173         // try to improve the solution
00174         tryToImprove(goal, result, pool[solution].distance);
00175 
00176         // if improving the state made it invalid, revert
00177         if (!valid(result))
00178             si_->copyState(result, pool[solution].state);
00179     }
00180     else
00181     {
00182         /* one last attempt to find a solution */
00183         std::sort(pool.begin(), pool.end(), gs);
00184         for (unsigned int i = 0 ; i < 5 ; ++i)
00185         {
00186             // get a valid state that is closer to the goal
00187             if (pool[i].valid)
00188             {
00189                 // set the solution
00190                 si_->copyState(result, pool[i].state);
00191 
00192                 // try to improve the state
00193                 tryToImprove(goal, result, pool[i].distance);
00194 
00195                 // if the improvement made the state no longer valid, revert to previous one
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 }