Loading...
Searching...
No Matches
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 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.
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.
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.
#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