GeneticSearch.h
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 #ifndef OMPL_GEOMETRIC_GENETIC_SEARCH_
38 #define OMPL_GEOMETRIC_GENETIC_SEARCH_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/goals/GoalRegion.h"
42 #include "ompl/geometric/HillClimbing.h"
43 #include "ompl/util/Console.h"
44 
45 namespace ompl
46 {
47 
48  namespace geometric
49  {
50 
63  {
64  public:
65 
68  ~GeneticSearch();
69 
71  bool solve(double solveTime, const base::GoalRegion &goal, base::State *result,
72  const std::vector<base::State*> &hint = std::vector<base::State*>());
73 
75  void setMaxImproveSteps(unsigned int maxSteps)
76  {
77  hc_.setMaxImproveSteps(maxSteps);
78  }
79 
81  unsigned int getMaxImproveSteps() const
82  {
83  return hc_.getMaxImproveSteps();
84  }
85 
87  void setValidityCheck(bool valid)
88  {
89  checkValidity_ = valid;
90  hc_.setValidityCheck(valid);
91  }
92 
94  bool getValidityCheck() const
95  {
96  return checkValidity_;
97  }
98 
100  void setTryImprove(bool flag)
101  {
102  tryImprove_ = flag;
103  }
104 
106  bool getTryImprove() const
107  {
108  return tryImprove_;
109  }
110 
112  void setPoolSize(unsigned int size)
113  {
114  poolSize_ = size;
115  }
116 
118  unsigned int getPoolSize() const
119  {
120  return poolSize_;
121  }
122 
124  void setPoolMutationSize(unsigned int size)
125  {
126  poolMutation_ = size;
127  }
128 
130  unsigned int getPoolMutationSize() const
131  {
132  return poolMutation_;
133  }
134 
136  void setPoolRandomSize(unsigned int size)
137  {
138  poolRandom_ = size;
139  }
140 
142  unsigned int getPoolRandomSize() const
143  {
144  return poolRandom_;
145  }
146 
148  void setRange(double distance)
149  {
150  maxDistance_ = distance;
151  }
152 
154  double getRange() const
155  {
156  return maxDistance_;
157  }
158 
160  void clear();
161 
162  private:
163 
165  void tryToImprove(const base::GoalRegion &goal, base::State *state, double distance);
166 
168  bool valid(const base::State *state) const
169  {
170  return checkValidity_ ? si_->isValid(state) : true;
171  }
172 
173  struct Individual
174  {
175  base::State *state;
176  double distance;
177  bool valid;
178  };
179 
180  struct IndividualSort
181  {
182  bool operator()(const Individual& a, const Individual& b)
183  {
184  if (a.valid == b.valid)
185  return a.distance < b.distance;
186  return a.valid;
187  }
188  };
189 
190  HillClimbing hc_;
191  base::SpaceInformationPtr si_;
192  base::StateSamplerPtr sampler_;
193  std::vector<Individual> pool_;
194  unsigned int poolSize_;
195  unsigned int poolMutation_;
196  unsigned int poolRandom_;
197  unsigned int generations_;
198  bool checkValidity_;
199  bool tryImprove_;
200 
201  double maxDistance_;
202  };
203 
204  }
205 }
206 
207 #endif
unsigned int getPoolSize() const
Get the number number of individuals in the population.
void setTryImprove(bool flag)
Set the flag that determines whether improvements using hill climbing should be attempted for solutio...
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
Definition: GeneticSearch.h:87
void setPoolRandomSize(unsigned int size)
Set the number of individuals to randomly sample at each generation.
void setMaxImproveSteps(unsigned int steps)
Set the number of steps to perform.
Definition: HillClimbing.h:79
void setRange(double distance)
Set the range (distance) to be used when sampling around a state.
bool getTryImprove() const
Returns true if improvements using hill climbing should be attempted for solutions generated by the g...
unsigned int getPoolMutationSize() const
Get the number of individuals that are mutated at each generation.
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.
unsigned int getMaxImproveSteps() const
Get the number of steps to perform.
Definition: HillClimbing.h:85
Main namespace. Contains everything in this library.
Definition: Cost.h:42
unsigned int getPoolRandomSize() const
Get the number of individuals to randomly sample at each generation.
bool getValidityCheck() const
Get the state validity flag; if this is false, states are not checked for validity.
Definition: GeneticSearch.h:94
void setPoolMutationSize(unsigned int size)
Set the number of individuals to mutate at each generation.
void setMaxImproveSteps(unsigned int maxSteps)
Set the number of steps to perform when using hill climbing to improve an individual in the populatio...
Definition: GeneticSearch.h:75
A boost shared pointer wrapper for ompl::base::SpaceInformation.
double getRange() const
Get the range GeneticSearch is using.
Definition of an abstract state.
Definition: State.h:50
void setPoolSize(unsigned int size)
Set the number of individuals in the population.
GeneticSearch(const base::SpaceInformationPtr &si)
Construct an instance of a genetic algorithm for inverse kinematics given the space information to se...
Definition of a goal region.
Definition: GoalRegion.h:50
Genetic Algorithm for searching valid states.
Definition: GeneticSearch.h:62
void clear()
Clear the pool of samples.
void setValidityCheck(bool valid)
Set the state validity flag; if this is false, states are not checked for validity.
Definition: HillClimbing.h:91
unsigned int getMaxImproveSteps() const
Get the number of steps to perform when using hill climbing to improve an individual in the populatio...
Definition: GeneticSearch.h:81