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
44ompl::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
58ompl::geometric::GeneticSearch::~GeneticSearch()
59{
60 for (auto &i : pool_)
61 si_->freeState(i.state);
62}
63
64bool 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
280void 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:48
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
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...
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
Definition: GeneticSearch.h:87
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 clear()
Clear the pool of samples.
void setMaxImproveSteps(unsigned int steps)
Set the number of steps to perform.
Definition: HillClimbing.h:78
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64