All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
KPIECE1.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
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 Rice University 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_CONTROL_PLANNERS_KPIECE_KPIECE1_
38 #define OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
39 
40 #include "ompl/control/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/datastructures/GridB.h"
43 #include <vector>
44 #include <set>
45 
46 namespace ompl
47 {
48 
49  namespace control
50  {
51 
77  class KPIECE1 : public base::Planner
78  {
79  public:
80 
82  KPIECE1(const SpaceInformationPtr &si);
83 
84  virtual ~KPIECE1(void);
85 
87 
88  virtual void clear(void);
89 
97  void setGoalBias(double goalBias)
98  {
99  goalBias_ = goalBias;
100  }
101 
103  double getGoalBias(void) const
104  {
105  return goalBias_;
106  }
107 
114  void setBorderFraction(double bp)
115  {
117  }
118 
121  double getBorderFraction(void) const
122  {
123  return selectBorderFraction_;
124  }
125 
132  void setCellScoreFactor(double good, double bad)
133  {
136  }
137 
139  void setBadCellScoreFactor(double bad)
140  {
141  badScoreFactor_ = bad;
142  }
143 
145  void setGoodCellScoreFactor(double good)
146  {
147  goodScoreFactor_ = good;
148  }
149 
152  double getGoodCellScoreFactor(void) const
153  {
154  return goodScoreFactor_;
155  }
156 
159  double getBadCellScoreFactor(void) const
160  {
161  return badScoreFactor_;
162  }
163 
166  void setMaxCloseSamplesCount(unsigned int nCloseSamples)
167  {
168  nCloseSamples_ = nCloseSamples;
169  }
170 
172  unsigned int getMaxCloseSamplesCount(void) const
173  {
174  return nCloseSamples_;
175  }
176 
179  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
180  {
181  projectionEvaluator_ = projectionEvaluator;
182  }
183 
186  void setProjectionEvaluator(const std::string &name)
187  {
188  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
189  }
190 
193  {
194  return projectionEvaluator_;
195  }
196 
197  virtual void setup(void);
198  virtual void getPlannerData(base::PlannerData &data) const;
199 
200  protected:
201 
203  struct Motion
204  {
205  Motion(void) : state(NULL), control(NULL), steps(0), parent(NULL)
206  {
207  }
208 
210  Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
211  {
212  }
213 
214  ~Motion(void)
215  {
216  }
217 
220 
223 
225  unsigned int steps;
226 
229  };
230 
232  struct CellData
233  {
234  CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
235  {
236  }
237 
238  ~CellData(void)
239  {
240  }
241 
243  std::vector<Motion*> motions;
244 
248  double coverage;
249 
252  unsigned int selections;
253 
257  double score;
258 
260  unsigned int iteration;
261 
263  double importance;
264  };
265 
269  {
270  bool operator()(const CellData * const a, const CellData * const b) const
271  {
272  return a->importance > b->importance;
273  }
274  };
275 
278 
280  struct CloseSample
281  {
283  CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
284  {
285  }
286 
289 
292 
294  double distance;
295 
297  bool operator<(const CloseSample &other) const
298  {
299  return distance < other.distance;
300  }
301  };
302 
305  {
307  CloseSamples(unsigned int size) : maxSize(size)
308  {
309  }
310 
316  bool consider(Grid::Cell *cell, Motion *motion, double distance);
317 
323  bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
324 
326  bool canSample(void) const
327  {
328  return samples.size() > 0;
329  }
330 
332  unsigned int maxSize;
333 
335  std::set<CloseSample> samples;
336  };
337 
338 
340  struct TreeData
341  {
342  TreeData(void) : grid(0), size(0), iteration(1)
343  {
344  }
345 
349 
352  unsigned int size;
353 
355  unsigned int iteration;
356  };
357 
361  static void computeImportance(Grid::Cell *cell, void*)
362  {
363  CellData &cd = *(cell->data);
364  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
365  }
366 
368  void freeMemory(void);
369 
371  void freeGridMotions(Grid &grid);
372 
374  void freeCellData(CellData *cdata);
375 
377  void freeMotion(Motion *motion);
378 
384  Grid::Cell* addMotion(Motion* motion, double dist);
385 
389  bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
390 
394  unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
395 
398 
401 
404 
409 
414 
419 
423  unsigned int nCloseSamples_;
424 
428 
430  double goalBias_;
431 
434 
437  };
438 
439  }
440 }
441 
442 #endif