GeneticSearch.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/geometric/GeneticSearch.h"
38 #include "ompl/util/Time.h"
39 #include "ompl/util/Exception.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <algorithm>
42 #include <limits>
43 
44 ompl::geometric::GeneticSearch::GeneticSearch(const base::SpaceInformationPtr &si) : hc_(si), si_(si), poolSize_(100), poolMutation_(20), poolRandom_(30),
45  generations_(0), tryImprove_(false), maxDistance_(0.0)
46 {
47  hc_.setMaxImproveSteps(3);
48  setValidityCheck(true);
49 }
50 
51 ompl::geometric::GeneticSearch::~GeneticSearch()
52 {
53  for (unsigned int i = 0 ; i < pool_.size() ; ++i)
54  si_->freeState(pool_[i].state);
55 }
56 
57 bool ompl::geometric::GeneticSearch::solve(double solveTime, const base::GoalRegion &goal, base::State *result, const std::vector<base::State*> &hint)
58 {
59  if (maxDistance_ < std::numeric_limits<double>::epsilon())
60  {
61  tools::SelfConfig sc(si_, "GeneticSearch");
62  sc.configurePlannerRange(maxDistance_);
63  }
64 
65  if (poolSize_ < 1)
66  {
67  OMPL_ERROR("Pool size too small");
68  return false;
69  }
70 
71  time::point endTime = time::now() + time::seconds(solveTime);
72 
73  unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
74  IndividualSort gs;
75  bool solved = false;
76  int solution = -1;
77 
78  if (!sampler_)
79  sampler_ = si_->allocStateSampler();
80 
81  if (pool_.empty())
82  {
83  generations_ = 1;
84  pool_.resize(maxPoolSize);
85  // add hint states
86  unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
87  for (unsigned int i = 0 ; i < nh ; ++i)
88  {
89  pool_[i].state = si_->cloneState(hint[i]);
90  si_->enforceBounds(pool_[i].state);
91  pool_[i].valid = valid(pool_[i].state);
92  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
93  {
94  if (pool_[i].valid)
95  {
96  solved = true;
97  solution = i;
98  }
99  }
100  }
101 
102  // add states near the hint states
103  unsigned int nh2 = nh * 2;
104  if (nh2 < maxPoolSize)
105  {
106  for (unsigned int i = nh ; i < nh2 ; ++i)
107  {
108  pool_[i].state = si_->allocState();
109  sampler_->sampleUniformNear(pool_[i].state, pool_[i % nh].state, maxDistance_);
110  pool_[i].valid = valid(pool_[i].state);
111  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
112  {
113  if (pool_[i].valid)
114  {
115  solved = true;
116  solution = i;
117  }
118  }
119  }
120  nh = nh2;
121  }
122 
123  // add random states
124  for (unsigned int i = nh ; i < maxPoolSize ; ++i)
125  {
126  pool_[i].state = si_->allocState();
127  sampler_->sampleUniform(pool_[i].state);
128  pool_[i].valid = valid(pool_[i].state);
129  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
130  {
131  if (pool_[i].valid)
132  {
133  solved = true;
134  solution = i;
135  }
136  }
137  }
138  }
139  else
140  {
141  std::size_t initialSize = pool_.size();
142  // free extra memory if needed
143  for (std::size_t i = maxPoolSize ; i < initialSize ; ++i)
144  si_->freeState(pool_[i].state);
145  pool_.resize(maxPoolSize);
146  // alloc extra memory if needed
147  for (std::size_t i = initialSize ; i < pool_.size() ; ++i)
148  pool_[i].state = si_->allocState();
149 
150  // add hint states at the bottom of the pool
151  unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
152  for (unsigned int i = 0 ; i < nh ; ++i)
153  {
154  std::size_t pi = maxPoolSize - i - 1;
155  si_->copyState(pool_[pi].state, hint[i]);
156  si_->enforceBounds(pool_[pi].state);
157  pool_[pi].valid = valid(pool_[pi].state);
158  if (goal.isSatisfied(pool_[pi].state, &(pool_[pi].distance)))
159  {
160  if (pool_[pi].valid)
161  {
162  solved = true;
163  solution = pi;
164  }
165  }
166  }
167 
168  // add random states if needed
169  nh = maxPoolSize - nh;
170  for (std::size_t i = initialSize ; i < nh ; ++i)
171  {
172  sampler_->sampleUniform(pool_[i].state);
173  pool_[i].valid = valid(pool_[i].state);
174  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
175  {
176  if (pool_[i].valid)
177  {
178  solved = true;
179  solution = i;
180  }
181  }
182  }
183  }
184 
185  // run the genetic algorithm
186  unsigned int mutationsSize = poolSize_ + poolMutation_;
187 
188  while (!solved && time::now() < endTime)
189  {
190  generations_++;
191  std::sort(pool_.begin(), pool_.end(), gs);
192 
193  // add mutations
194  for (unsigned int i = poolSize_ ; i < mutationsSize ; ++i)
195  {
196  sampler_->sampleUniformNear(pool_[i].state, pool_[i % poolSize_].state, maxDistance_);
197  pool_[i].valid = valid(pool_[i].state);
198  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
199  {
200  if (pool_[i].valid)
201  {
202  solved = true;
203  solution = i;
204  break;
205  }
206  }
207  }
208 
209  // add random states
210  if (!solved)
211  for (unsigned int i = mutationsSize ; i < maxPoolSize ; ++i)
212  {
213  sampler_->sampleUniform(pool_[i].state);
214  pool_[i].valid = valid(pool_[i].state);
215  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
216  {
217  if (pool_[i].valid)
218  {
219  solved = true;
220  solution = i;
221  break;
222  }
223  }
224  }
225  }
226 
227 
228  // fill in solution, if found
229  OMPL_INFORM("Ran for %u generations", generations_);
230 
231  if (solved)
232  {
233  // set the solution
234  si_->copyState(result, pool_[solution].state);
235 
236  // try to improve the solution
237  if (tryImprove_)
238  tryToImprove(goal, result, pool_[solution].distance);
239 
240  // if improving the state made it invalid, revert
241  if (!valid(result))
242  si_->copyState(result, pool_[solution].state);
243  }
244  else
245  if (tryImprove_)
246  {
247  /* one last attempt to find a solution */
248  std::sort(pool_.begin(), pool_.end(), gs);
249  for (unsigned int i = 0 ; i < 5 ; ++i)
250  {
251  // get a valid state that is closer to the goal
252  if (pool_[i].valid)
253  {
254  // set the solution
255  si_->copyState(result, pool_[i].state);
256 
257  // try to improve the state
258  tryToImprove(goal, result, pool_[i].distance);
259 
260  // if the improvement made the state no longer valid, revert to previous one
261  if (!valid(result))
262  si_->copyState(result, pool_[i].state);
263  else
264  solved = goal.isSatisfied(result);
265  if (solved)
266  break;
267  }
268  }
269  }
270 
271  return solved;
272 }
273 
274 void ompl::geometric::GeneticSearch::tryToImprove(const base::GoalRegion &goal, base::State *state, double distance)
275 {
276  OMPL_DEBUG("Distance to goal before improvement: %g", distance);
277  time::point start = time::now();
278  double dist = si_->getMaximumExtent() / 10.0;
279  hc_.tryToImprove(goal, state, dist, &distance);
280  hc_.tryToImprove(goal, state, dist / 3.0, &distance);
281  hc_.tryToImprove(goal, state, dist / 10.0, &distance);
282  OMPL_DEBUG("Improvement took %u ms", (time::now() - start).total_milliseconds());
283  OMPL_DEBUG("Distance to goal after improvement: %g", distance);
284 }
285 
287 {
288  generations_ = 0;
289  pool_.clear();
290  sampler_.reset();
291 }
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
Definition: GeneticSearch.h:87
virtual bool isSatisfied(const State *st) const
Equivalent to calling isSatisfied(const State *, double *) with a NULL second argument.
Definition: GoalRegion.cpp:46
void setMaxImproveSteps(unsigned int steps)
Set the number of steps to perform.
Definition: HillClimbing.h:79
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
bool solve(double solveTime, const base::GoalRegion &goal, base::State *result, const std::vector< base::State *> &hint=std::vector< base::State *>())
Find a state that fits the request.
bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance=NULL) const
Try to improve a state (reduce distance to goal). The updates are performed by sampling near the stat...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
GeneticSearch(const base::SpaceInformationPtr &si)
Construct an instance of a genetic algorithm for inverse kinematics given the space information to se...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
point now()
Get the current time point.
Definition: Time.h:56
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
Definition of a goal region.
Definition: GoalRegion.h:50
void clear()
Clear the pool of samples.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68