All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
GAIK.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_IK_GAIK_
38 #define OMPL_GEOMETRIC_IK_GAIK_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/goals/GoalRegion.h"
42 #include "ompl/geometric/ik/HCIK.h"
43 #include "ompl/util/Console.h"
44 
45 namespace ompl
46 {
47 
48  namespace geometric
49  {
50 
63  class GAIK
64  {
65  public:
66 
69  ~GAIK(void);
70 
72  bool solve(double solveTime, const base::GoalRegion &goal, base::State *result,
73  const std::vector<base::State*> &hint = std::vector<base::State*>());
74 
76  void setMaxImproveSteps(unsigned int maxSteps)
77  {
78  hcik_.setMaxImproveSteps(maxSteps);
79  }
80 
82  unsigned int getMaxImproveSteps(void) const
83  {
84  return hcik_.getMaxImproveSteps();
85  }
86 
88  void setValidityCheck(bool valid)
89  {
90  checkValidity_ = valid;
91  hcik_.setValidityCheck(valid);
92  }
93 
95  bool getValidityCheck(void) const
96  {
97  return checkValidity_;
98  }
99 
101  void setTryImprove(bool flag)
102  {
103  tryImprove_ = flag;
104  }
105 
107  bool getTryImprove(void) const
108  {
109  return tryImprove_;
110  }
111 
113  void setPoolSize(unsigned int size)
114  {
115  poolSize_ = size;
116  }
117 
119  unsigned int getPoolSize(void) const
120  {
121  return poolSize_;
122  }
123 
125  void setPoolMutationSize(unsigned int size)
126  {
127  poolMutation_ = size;
128  }
129 
131  unsigned int getPoolMutationSize(void) const
132  {
133  return poolMutation_;
134  }
135 
137  void setPoolRandomSize(unsigned int size)
138  {
139  poolRandom_ = size;
140  }
141 
143  unsigned int getPoolRandomSize(void) const
144  {
145  return poolRandom_;
146  }
147 
149  void setRange(double distance)
150  {
151  maxDistance_ = distance;
152  }
153 
155  double getRange(void) const
156  {
157  return maxDistance_;
158  }
159 
161  void clear(void);
162 
163  private:
164 
166  void tryToImprove(const base::GoalRegion &goal, base::State *state, double distance);
167 
169  bool valid(const base::State *state) const
170  {
171  return checkValidity_ ? si_->isValid(state) : true;
172  }
173 
174  struct Individual
175  {
176  base::State *state;
177  double distance;
178  bool valid;
179  };
180 
181  struct IndividualSort
182  {
183  bool operator()(const Individual& a, const Individual& b)
184  {
185  if (a.valid == b.valid)
186  return a.distance < b.distance;
187  return a.valid;
188  }
189  };
190 
191  HCIK hcik_;
192  base::SpaceInformationPtr si_;
193  base::StateSamplerPtr sampler_;
194  std::vector<Individual> pool_;
195  unsigned int poolSize_;
196  unsigned int poolMutation_;
197  unsigned int poolRandom_;
198  unsigned int generations_;
199  bool checkValidity_;
200  bool tryImprove_;
201 
202  double maxDistance_;
203  };
204 
205  }
206 }
207 
208 #endif