Discretization.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
38 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
39 
40 #include "ompl/base/Planner.h"
41 #include "ompl/datastructures/GridB.h"
42 #include "ompl/util/Exception.h"
43 #include <boost/function.hpp>
44 #include <vector>
45 #include <limits>
46 #include <cassert>
47 #include <utility>
48 #include <cstdlib>
49 #include <cmath>
50 
51 namespace ompl
52 {
53 
54  namespace geometric
55  {
56 
58  template<typename Motion>
60  {
61  public:
62 
64  struct CellData
65  {
66  CellData() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
67  {
68  }
69 
70  ~CellData()
71  {
72  }
73 
75  std::vector<Motion*> motions;
76 
80  double coverage;
81 
84  unsigned int selections;
85 
89  double score;
90 
92  unsigned int iteration;
93 
95  double importance;
96  };
97 
101  {
102 
104  bool operator()(const CellData * const a, const CellData * const b) const
105  {
106  return a->importance > b->importance;
107  }
108  };
109 
112 
114  typedef typename Grid::Cell Cell;
115 
117  typedef typename Grid::Coord Coord;
118 
120  typedef typename boost::function<void(Motion*)> FreeMotionFn;
121 
122  Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
123  freeMotion_(freeMotion)
124  {
125  grid_.onCellUpdate(computeImportance, NULL);
126  selectBorderFraction_ = 0.9;
127  }
128 
129  ~Discretization()
130  {
131  freeMemory();
132  }
133 
140  void setBorderFraction(double bp)
141  {
142  if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
143  throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
144  selectBorderFraction_ = bp;
145  }
146 
149  double getBorderFraction() const
150  {
151  return selectBorderFraction_;
152  }
153 
155  void setDimension(unsigned int dim)
156  {
157  grid_.setDimension(dim);
158  }
159 
161  void clear()
162  {
163  freeMemory();
164  size_ = 0;
165  iteration_ = 1;
166  recentCell_ = NULL;
167  }
168 
169  void countIteration()
170  {
171  ++iteration_;
172  }
173 
174  std::size_t getMotionCount() const
175  {
176  return size_;
177  }
178 
179  std::size_t getCellCount() const
180  {
181  return grid_.size();
182  }
183 
185  void freeMemory()
186  {
187  for (typename Grid::iterator it = grid_.begin(); it != grid_.end() ; ++it)
188  freeCellData(it->second->data);
189  grid_.clear();
190  }
191 
200  unsigned int addMotion(Motion *motion, const Coord &coord, double dist = 0.0)
201  {
202  Cell *cell = grid_.getCell(coord);
203 
204  unsigned int created = 0;
205  if (cell)
206  {
207  cell->data->motions.push_back(motion);
208  cell->data->coverage += 1.0;
209  grid_.update(cell);
210  }
211  else
212  {
213  cell = grid_.createCell(coord);
214  cell->data = new CellData();
215  cell->data->motions.push_back(motion);
216  cell->data->coverage = 1.0;
217  cell->data->iteration = iteration_;
218  cell->data->selections = 1;
219  cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
220  grid_.add(cell);
221  recentCell_ = cell;
222  created = 1;
223  }
224  ++size_;
225  return created;
226  }
227 
231  void selectMotion(Motion* &smotion, Cell* &scell)
232  {
233  scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ?
234  grid_.topExternal() : grid_.topInternal();
235 
236  // We are running on finite precision, so our update scheme will end up
237  // with 0 values for the score. This is where we fix the problem
238  if (scell->data->score < std::numeric_limits<double>::epsilon())
239  {
240  std::vector<CellData*> content;
241  content.reserve(grid_.size());
242  grid_.getContent(content);
243  for (typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
244  (*it)->score += 1.0 + log((double)((*it)->iteration));
245  grid_.updateAll();
246  }
247 
248  assert(scell && !scell->data->motions.empty());
249 
250  ++scell->data->selections;
251  smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
252  }
253 
254  bool removeMotion(Motion *motion, const Coord &coord)
255  {
256  Cell* cell = grid_.getCell(coord);
257  if (cell)
258  {
259  bool found = false;
260  for (unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
261  if (cell->data->motions[i] == motion)
262  {
263  cell->data->motions.erase(cell->data->motions.begin() + i);
264  found = true;
265  --size_;
266  break;
267  }
268  if (cell->data->motions.empty())
269  {
270  grid_.remove(cell);
271  freeCellData(cell->data);
272  grid_.destroyCell(cell);
273  }
274  return found;
275  }
276  return false;
277  }
278 
279  void updateCell(Cell *cell)
280  {
281  grid_.update(cell);
282  }
283 
284  const Grid& getGrid() const
285  {
286  return grid_;
287  }
288 
289  void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
290  {
291  std::vector<CellData*> cdata;
292  grid_.getContent(cdata);
293 
294  if (lastGoalMotion)
295  data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
296 
297  for (unsigned int i = 0 ; i < cdata.size() ; ++i)
298  for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
299  {
300  if (cdata[i]->motions[j]->parent == NULL)
301  {
302  if (start)
303  data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
304  else
305  data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
306  }
307  else
308  {
309  if (start)
310  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
311  base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
312  else
313  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
314  base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
315  }
316  }
317  }
318 
319  private:
320 
322  void freeCellData(CellData *cdata)
323  {
324  for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
325  freeMotion_(cdata->motions[i]);
326  delete cdata;
327  }
328 
332  static void computeImportance(Cell *cell, void*)
333  {
334  CellData &cd = *(cell->data);
335  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
336  }
337 
340  Grid grid_;
341 
344  std::size_t size_;
345 
347  unsigned int iteration_;
348 
350  Cell *recentCell_;
351 
353  FreeMotionFn freeMotion_;
354 
357  double selectBorderFraction_;
358 
360  RNG rng_;
361 
362  };
363  }
364 }
365 
366 #endif
Grid::Coord Coord
The datatype for the maintained grid coordinates.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
boost::function< void(Motion *)> FreeMotionFn
The signature of a function that frees the memory for a motion.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:104
unsigned int neighbors
The number of neighbors.
Definition: GridN.h:64
GridN< CellData * >::Coord Coord
Datatype for cell coordinates.
Definition: GridB.h:62
void freeMemory()
Free the memory for the motions contained in a grid.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
The data held by a cell in the grid of motions.
unsigned int selections
The number of times this cell has been selected for expansion.
_T data
The data we store in the cell.
Definition: Grid.h:62
unsigned int iteration
The iteration at which this cell was created.
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion lengths...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
One-level discretization used for KPIECE.
unsigned int addMotion(Motion *motion, const Coord &coord, double dist=0.0)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
double getBorderFraction() const
Set the fraction of time for focusing on the border (between 0 and 1).
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
std::vector< Motion * > motions
The set of motions contained in this grid cell.
The exception type for ompl.
Definition: Exception.h:47
Grid::Cell Cell
The datatype for the maintained grid cells.
void selectMotion(Motion *&smotion, Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
bool operator()(const CellData *const a, const CellData *const b) const
Order function.
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
Definition of a cell in this grid.
Definition: GridN.h:61
void clear()
Restore the discretization to its original form.
void setDimension(unsigned int dim)
Set the dimension of the grid to be maintained.
double importance
The computed importance (based on other class members)
Definintion of an operator passed to the Grid structure, to order cells by importance.
CoordHash::const_iterator iterator
We only allow const iterators.
Definition: Grid.h:374