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)
45  : hc_(si)
46  , si_(si)
47  , poolSize_(100)
48  , poolMutation_(20)
49  , poolRandom_(30)
50  , generations_(0)
51  , tryImprove_(false)
52  , maxDistance_(0.0)
53 {
54  hc_.setMaxImproveSteps(3);
55  setValidityCheck(true);
56 }
57 
58 ompl::geometric::GeneticSearch::~GeneticSearch()
59 {
60  for (auto &i : pool_)
61  si_->freeState(i.state);
62 }
63 
64 bool ompl::geometric::GeneticSearch::solve(double solveTime, const base::GoalRegion &goal, base::State *result,
65  const std::vector<base::State *> &hint)
66 {
67  if (maxDistance_ < std::numeric_limits<double>::epsilon())
68  {
69  tools::SelfConfig sc(si_, "GeneticSearch");
70  sc.configurePlannerRange(maxDistance_);
71  }
72 
73  if (poolSize_ < 1)
74  {
75  OMPL_ERROR("Pool size too small");
76  return false;
77  }
78 
79  time::point endTime = time::now() + time::seconds(solveTime);
80 
81  unsigned int maxPoolSize = poolSize_ + poolMutation_ + poolRandom_;
82  IndividualSort gs;
83  bool solved = false;
84  int solution = -1;
85 
86  if (!sampler_)
87  sampler_ = si_->allocStateSampler();
88 
89  if (pool_.empty())
90  {
91  generations_ = 1;
92  pool_.resize(maxPoolSize);
93  // add hint states
94  unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
95  for (unsigned int i = 0; i < nh; ++i)
96  {
97  pool_[i].state = si_->cloneState(hint[i]);
98  si_->enforceBounds(pool_[i].state);
99  pool_[i].valid = valid(pool_[i].state);
100  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
101  {
102  if (pool_[i].valid)
103  {
104  solved = true;
105  solution = i;
106  }
107  }
108  }
109 
110  // add states near the hint states
111  unsigned int nh2 = nh * 2;
112  if (nh2 < maxPoolSize)
113  {
114  for (unsigned int i = nh; i < nh2; ++i)
115  {
116  pool_[i].state = si_->allocState();
117  sampler_->sampleUniformNear(pool_[i].state, pool_[i % nh].state, maxDistance_);
118  pool_[i].valid = valid(pool_[i].state);
119  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
120  {
121  if (pool_[i].valid)
122  {
123  solved = true;
124  solution = i;
125  }
126  }
127  }
128  nh = nh2;
129  }
130 
131  // add random states
132  for (unsigned int i = nh; i < maxPoolSize; ++i)
133  {
134  pool_[i].state = si_->allocState();
135  sampler_->sampleUniform(pool_[i].state);
136  pool_[i].valid = valid(pool_[i].state);
137  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
138  {
139  if (pool_[i].valid)
140  {
141  solved = true;
142  solution = i;
143  }
144  }
145  }
146  }
147  else
148  {
149  std::size_t initialSize = pool_.size();
150  // free extra memory if needed
151  for (std::size_t i = maxPoolSize; i < initialSize; ++i)
152  si_->freeState(pool_[i].state);
153  pool_.resize(maxPoolSize);
154  // alloc extra memory if needed
155  for (std::size_t i = initialSize; i < pool_.size(); ++i)
156  pool_[i].state = si_->allocState();
157 
158  // add hint states at the bottom of the pool
159  unsigned int nh = std::min<unsigned int>(maxPoolSize, hint.size());
160  for (unsigned int i = 0; i < nh; ++i)
161  {
162  std::size_t pi = maxPoolSize - i - 1;
163  si_->copyState(pool_[pi].state, hint[i]);
164  si_->enforceBounds(pool_[pi].state);
165  pool_[pi].valid = valid(pool_[pi].state);
166  if (goal.isSatisfied(pool_[pi].state, &(pool_[pi].distance)))
167  {
168  if (pool_[pi].valid)
169  {
170  solved = true;
171  solution = pi;
172  }
173  }
174  }
175 
176  // add random states if needed
177  nh = maxPoolSize - nh;
178  for (std::size_t i = initialSize; i < nh; ++i)
179  {
180  sampler_->sampleUniform(pool_[i].state);
181  pool_[i].valid = valid(pool_[i].state);
182  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
183  {
184  if (pool_[i].valid)
185  {
186  solved = true;
187  solution = i;
188  }
189  }
190  }
191  }
192 
193  // run the genetic algorithm
194  unsigned int mutationsSize = poolSize_ + poolMutation_;
195 
196  while (!solved && time::now() < endTime)
197  {
198  generations_++;
199  std::sort(pool_.begin(), pool_.end(), gs);
200 
201  // add mutations
202  for (unsigned int i = poolSize_; i < mutationsSize; ++i)
203  {
204  sampler_->sampleUniformNear(pool_[i].state, pool_[i % poolSize_].state, maxDistance_);
205  pool_[i].valid = valid(pool_[i].state);
206  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
207  {
208  if (pool_[i].valid)
209  {
210  solved = true;
211  solution = i;
212  break;
213  }
214  }
215  }
216 
217  // add random states
218  if (!solved)
219  for (unsigned int i = mutationsSize; i < maxPoolSize; ++i)
220  {
221  sampler_->sampleUniform(pool_[i].state);
222  pool_[i].valid = valid(pool_[i].state);
223  if (goal.isSatisfied(pool_[i].state, &(pool_[i].distance)))
224  {
225  if (pool_[i].valid)
226  {
227  solved = true;
228  solution = i;
229  break;
230  }
231  }
232  }
233  }
234 
235  // fill in solution, if found
236  OMPL_INFORM("Ran for %u generations", generations_);
237 
238  if (solved)
239  {
240  // set the solution
241  si_->copyState(result, pool_[solution].state);
242 
243  // try to improve the solution
244  if (tryImprove_)
245  tryToImprove(goal, result, pool_[solution].distance);
246 
247  // if improving the state made it invalid, revert
248  if (!valid(result))
249  si_->copyState(result, pool_[solution].state);
250  }
251  else if (tryImprove_)
252  {
253  /* one last attempt to find a solution */
254  std::sort(pool_.begin(), pool_.end(), gs);
255  for (unsigned int i = 0; i < 5; ++i)
256  {
257  // get a valid state that is closer to the goal
258  if (pool_[i].valid)
259  {
260  // set the solution
261  si_->copyState(result, pool_[i].state);
262 
263  // try to improve the state
264  tryToImprove(goal, result, pool_[i].distance);
265 
266  // if the improvement made the state no longer valid, revert to previous one
267  if (!valid(result))
268  si_->copyState(result, pool_[i].state);
269  else
270  solved = goal.isSatisfied(result);
271  if (solved)
272  break;
273  }
274  }
275  }
276 
277  return solved;
278 }
279 
280 void ompl::geometric::GeneticSearch::tryToImprove(const base::GoalRegion &goal, base::State *state, double distance)
281 {
282  OMPL_DEBUG("Distance to goal before improvement: %g", distance);
283  time::point start = time::now();
284  double dist = si_->getMaximumExtent() / 10.0;
285  hc_.tryToImprove(goal, state, dist, &distance);
286  hc_.tryToImprove(goal, state, dist / 3.0, &distance);
287  hc_.tryToImprove(goal, state, dist / 10.0, &distance);
288  OMPL_DEBUG("Improvement took %u ms",
289  std::chrono::duration_cast<std::chrono::milliseconds>(time::now() - start).count());
290  OMPL_DEBUG("Distance to goal after improvement: %g", distance);
291 }
292 
294 {
295  generations_ = 0;
296  pool_.clear();
297  sampler_.reset();
298 }
Definition of a goal region.
Definition: GoalRegion.h:111
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.
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
point now()
Get the current time point.
Definition: Time.h:122
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
void clear()
Clear the pool of samples.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void setMaxImproveSteps(unsigned int steps)
Set the number of steps to perform.
Definition: HillClimbing.h:174
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
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128