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 <functional>
44 #include <utility>
45 #include <vector>
46 #include <limits>
47 #include <cassert>
48 #include <utility>
49 #include <cstdlib>
50 #include <cmath>
51 
52 namespace ompl
53 {
54  namespace geometric
55  {
57  template <typename Motion>
59  {
60  public:
62  struct CellData
63  {
64  CellData() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
65  {
66  }
67 
68  ~CellData() = default;
69 
71  std::vector<Motion *> motions;
72 
76  double coverage;
77 
80  unsigned int selections;
81 
85  double score;
86 
88  unsigned int iteration;
89 
91  double importance;
92  };
93 
97  {
99  bool operator()(const CellData *const a, const CellData *const b) const
100  {
101  return a->importance > b->importance;
102  }
103  };
104 
107 
109  typedef typename Grid::Cell Cell;
110 
112  typedef typename Grid::Coord Coord;
113 
115  typedef typename std::function<void(Motion *)> FreeMotionFn;
116 
117  Discretization(FreeMotionFn freeMotion)
118  : grid_(0), size_(0), iteration_(1), recentCell_(nullptr), freeMotion_(std::move(freeMotion))
119  {
120  grid_.onCellUpdate(computeImportance, nullptr);
121  selectBorderFraction_ = 0.9;
122  }
123 
124  ~Discretization()
125  {
126  freeMemory();
127  }
128 
135  void setBorderFraction(double bp)
136  {
137  if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
138  throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
139  selectBorderFraction_ = bp;
140  }
141 
144  double getBorderFraction() const
145  {
146  return selectBorderFraction_;
147  }
148 
150  void setDimension(unsigned int dim)
151  {
152  grid_.setDimension(dim);
153  }
154 
156  void clear()
157  {
158  freeMemory();
159  size_ = 0;
160  iteration_ = 1;
161  recentCell_ = nullptr;
162  }
163 
164  void countIteration()
165  {
166  ++iteration_;
167  }
168 
169  std::size_t getMotionCount() const
170  {
171  return size_;
172  }
173 
174  std::size_t getCellCount() const
175  {
176  return grid_.size();
177  }
178 
180  void freeMemory()
181  {
182  for (auto it = grid_.begin(); it != grid_.end(); ++it)
183  freeCellData(it->second->data);
184  grid_.clear();
185  }
186 
195  unsigned int addMotion(Motion *motion, const Coord &coord, double dist = 0.0)
196  {
197  Cell *cell = grid_.getCell(coord);
198 
199  unsigned int created = 0;
200  if (cell)
201  {
202  cell->data->motions.push_back(motion);
203  cell->data->coverage += 1.0;
204  grid_.update(cell);
205  }
206  else
207  {
208  cell = grid_.createCell(coord);
209  cell->data = new CellData();
210  cell->data->motions.push_back(motion);
211  cell->data->coverage = 1.0;
212  cell->data->iteration = iteration_;
213  cell->data->selections = 1;
214  cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
215  grid_.add(cell);
216  recentCell_ = cell;
217  created = 1;
218  }
219  ++size_;
220  return created;
221  }
222 
226  void selectMotion(Motion *&smotion, Cell *&scell)
227  {
228  scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ? grid_.topExternal() :
229  grid_.topInternal();
230 
231  // We are running on finite precision, so our update scheme will end up
232  // with 0 values for the score. This is where we fix the problem
233  if (scell->data->score < std::numeric_limits<double>::epsilon())
234  {
235  std::vector<CellData *> content;
236  content.reserve(grid_.size());
237  grid_.getContent(content);
238  for (auto it = content.begin(); it != content.end(); ++it)
239  (*it)->score += 1.0 + log((double)((*it)->iteration));
240  grid_.updateAll();
241  }
242 
243  assert(scell && !scell->data->motions.empty());
244 
245  ++scell->data->selections;
246  smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
247  }
248 
249  bool removeMotion(Motion *motion, const Coord &coord)
250  {
251  Cell *cell = grid_.getCell(coord);
252  if (cell)
253  {
254  bool found = false;
255  for (unsigned int i = 0; i < cell->data->motions.size(); ++i)
256  if (cell->data->motions[i] == motion)
257  {
258  cell->data->motions.erase(cell->data->motions.begin() + i);
259  found = true;
260  --size_;
261  break;
262  }
263  if (cell->data->motions.empty())
264  {
265  grid_.remove(cell);
266  freeCellData(cell->data);
267  grid_.destroyCell(cell);
268  }
269  return found;
270  }
271  return false;
272  }
273 
274  void updateCell(Cell *cell)
275  {
276  grid_.update(cell);
277  }
278 
279  const Grid &getGrid() const
280  {
281  return grid_;
282  }
283 
284  void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
285  {
286  std::vector<CellData *> cdata;
287  grid_.getContent(cdata);
288 
289  if (lastGoalMotion)
290  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion->state, tag));
291 
292  for (unsigned int i = 0; i < cdata.size(); ++i)
293  for (unsigned int j = 0; j < cdata[i]->motions.size(); ++j)
294  {
295  if (cdata[i]->motions[j]->parent == nullptr)
296  {
297  if (start)
298  data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
299  else
300  data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
301  }
302  else
303  {
304  if (start)
305  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
306  base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
307  else
308  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
309  base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
310  }
311  }
312  }
313 
314  private:
316  void freeCellData(CellData *cdata)
317  {
318  for (unsigned int i = 0; i < cdata->motions.size(); ++i)
319  freeMotion_(cdata->motions[i]);
320  delete cdata;
321  }
322 
326  static void computeImportance(Cell *cell, void *)
327  {
328  CellData &cd = *(cell->data);
329  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
330  }
331 
334  Grid grid_;
335 
338  std::size_t size_;
339 
341  unsigned int iteration_;
342 
344  Cell *recentCell_;
345 
347  FreeMotionFn freeMotion_;
348 
351  double selectBorderFraction_;
352 
354  RNG rng_;
355  };
356  }
357 }
358 
359 #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:174
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:120
void freeMemory()
Free the memory for the motions contained in a grid.
typename GridN< CellData * >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:52
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.
STL namespace.
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:58
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
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...
std::vector< Motion * > motions
The set of motions contained in this grid cell.
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...
The exception type for ompl.
Definition: Exception.h:46
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
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...
std::function< void(Motion *)> FreeMotionFn
The signature of a function that frees the memory for a motion.
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)
typename GridN< CellData * >::Coord Coord
Datatype for cell coordinates.
Definition: GridB.h:58
Definintion of an operator passed to the Grid structure, to order cells by importance.