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>
58  class Discretization
59  {
60  public:
62  struct CellData
63  {
64  CellData() = default;
65 
66  ~CellData() = default;
67 
69  std::vector<Motion *> motions;
70 
74  double coverage{0.0};
75 
78  unsigned int selections{1};
79 
83  double score{1.0};
84 
86  unsigned int iteration{0};
87 
89  double importance{0.0};
90  };
91 
94  struct OrderCellsByImportance
95  {
97  bool operator()(const CellData *const a, const CellData *const b) const
98  {
99  return a->importance > b->importance;
100  }
101  };
102 
104  using Grid = GridB<CellData *, OrderCellsByImportance>;
105 
107  using Cell = typename Grid::Cell;
108 
110  using Coord = typename Grid::Coord;
111 
113  using FreeMotionFn = typename std::function<void(Motion *)>;
114 
115  Discretization(FreeMotionFn freeMotion)
116  : grid_(0), size_(0), iteration_(1), recentCell_(nullptr), freeMotion_(std::move(freeMotion))
117  {
118  grid_.onCellUpdate(computeImportance, nullptr);
119  selectBorderFraction_ = 0.9;
120  }
121 
123  {
124  freeMemory();
125  }
126 
133  void setBorderFraction(double bp)
134  {
135  if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
136  throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
137  selectBorderFraction_ = bp;
138  }
139 
142  double getBorderFraction() const
143  {
144  return selectBorderFraction_;
145  }
146 
148  void setDimension(unsigned int dim)
149  {
150  grid_.setDimension(dim);
151  }
152 
154  void clear()
155  {
156  freeMemory();
157  size_ = 0;
158  iteration_ = 1;
159  recentCell_ = nullptr;
160  }
161 
162  void countIteration()
163  {
164  ++iteration_;
165  }
166 
167  std::size_t getMotionCount() const
168  {
169  return size_;
170  }
171 
172  std::size_t getCellCount() const
173  {
174  return grid_.size();
175  }
176 
178  void freeMemory()
179  {
180  for (auto it = grid_.begin(); it != grid_.end(); ++it)
181  freeCellData(it->second->data);
182  grid_.clear();
183  }
184 
193  unsigned int addMotion(Motion *motion, const Coord &coord, double dist = 0.0)
194  {
195  Cell *cell = grid_.getCell(coord);
196 
197  unsigned int created = 0;
198  if (cell)
199  {
200  cell->data->motions.push_back(motion);
201  cell->data->coverage += 1.0;
202  grid_.update(cell);
203  }
204  else
205  {
206  cell = grid_.createCell(coord);
207  cell->data = new CellData();
208  cell->data->motions.push_back(motion);
209  cell->data->coverage = 1.0;
210  cell->data->iteration = iteration_;
211  cell->data->selections = 1;
212  cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
213  grid_.add(cell);
214  recentCell_ = cell;
215  created = 1;
216  }
217  ++size_;
218  return created;
219  }
220 
224  void selectMotion(Motion *&smotion, Cell *&scell)
225  {
226  scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ? grid_.topExternal() :
227  grid_.topInternal();
228 
229  // We are running on finite precision, so our update scheme will end up
230  // with 0 values for the score. This is where we fix the problem
231  if (scell->data->score < std::numeric_limits<double>::epsilon())
232  {
233  std::vector<CellData *> content;
234  content.reserve(grid_.size());
235  grid_.getContent(content);
236  for (auto it = content.begin(); it != content.end(); ++it)
237  (*it)->score += 1.0 + log((double)((*it)->iteration));
238  grid_.updateAll();
239  }
240 
241  assert(scell && !scell->data->motions.empty());
242 
243  ++scell->data->selections;
244  smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
245  }
246 
247  bool removeMotion(Motion *motion, const Coord &coord)
248  {
249  Cell *cell = grid_.getCell(coord);
250  if (cell)
251  {
252  bool found = false;
253  for (unsigned int i = 0; i < cell->data->motions.size(); ++i)
254  if (cell->data->motions[i] == motion)
255  {
256  cell->data->motions.erase(cell->data->motions.begin() + i);
257  found = true;
258  --size_;
259  break;
260  }
261  if (cell->data->motions.empty())
262  {
263  grid_.remove(cell);
264  freeCellData(cell->data);
265  grid_.destroyCell(cell);
266  }
267  return found;
268  }
269  return false;
270  }
271 
272  void updateCell(Cell *cell)
273  {
274  grid_.update(cell);
275  }
276 
277  const Grid &getGrid() const
278  {
279  return grid_;
280  }
281 
282  void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
283  {
284  std::vector<CellData *> cdata;
285  grid_.getContent(cdata);
286 
287  if (lastGoalMotion)
288  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion->state, tag));
289 
290  for (unsigned int i = 0; i < cdata.size(); ++i)
291  for (unsigned int j = 0; j < cdata[i]->motions.size(); ++j)
292  {
293  if (cdata[i]->motions[j]->parent == nullptr)
294  {
295  if (start)
296  data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
297  else
298  data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
299  }
300  else
301  {
302  if (start)
303  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
304  base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
305  else
306  data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
307  base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
308  }
309  }
310  }
311 
312  private:
314  void freeCellData(CellData *cdata)
315  {
316  for (unsigned int i = 0; i < cdata->motions.size(); ++i)
317  freeMotion_(cdata->motions[i]);
318  delete cdata;
319  }
320 
324  static void computeImportance(Cell *cell, void * /*unused*/)
325  {
326  CellData &cd = *(cell->data);
327  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
328  }
329 
332  Grid grid_;
333 
336  std::size_t size_;
337 
339  unsigned int iteration_;
340 
342  Cell *recentCell_;
343 
345  FreeMotionFn freeMotion_;
346 
349  double selectBorderFraction_;
350 
352  RNG rng_;
353  };
354  }
355 }
356 
357 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
typename std::function< void(Motion *)> FreeMotionFn
The signature of a function that frees the memory for a motion.
void clear() override
Clear all cells in the grid.
Definition: GridB.h:336
void updateAll()
Update all cells and reconstruct the heaps.
Definition: GridB.h:220
iterator begin() const
Return the begin() iterator for the grid.
Definition: Grid.h:433
double fracExternal() const
Return the fraction of external cells.
Definition: GridB.h:196
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion lengths.
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.
void onCellUpdate(EventCellUpdate event, void *arg)
Definition: GridB.h:163
virtual bool remove(Cell *cell)
Remove a cell from the grid.
Definition: GridB.h:288
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
typename Grid::Cell Cell
The datatype for the maintained grid cells.
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
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 getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
Definition: Grid.h:322
int halfNormalInt(int r_min, int r_max, double focus=3.0)
Generate a random integer using a half-normal distribution. The value is within specified bounds ([r_...
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Create a cell but do not add it to the grid; update neighboring cells however.
Definition: GridB.h:231
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 ...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
virtual void add(Cell *cell)
Add the cell to the grid.
Definition: GridB.h:274
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Definition: GridB.h:170
typename GridN< CellData * >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:119
typename GridN< CellData * >::Coord Coord
Datatype for cell coordinates.
Definition: GridB.h:125
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first.
Definition: Grid.h:316
double getBorderFraction() const
Set the fraction of time for focusing on the border (between 0 and 1).
double importance
The computed importance (based on other class members)
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...
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
iterator end() const
Return the end() iterator for the grid.
Definition: Grid.h:439
unsigned int size() const
Check the size of the grid.
Definition: Grid.h:358
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Definition: GridB.h:208
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Definition: GridB.h:177
void setDimension(unsigned int dimension)
Definition: GridN.h:153
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: GridN.h:187
std::vector< Motion * > motions
The set of motions contained in this grid cell.
void setDimension(unsigned int dim)
Set the dimension of the grid to be maintained.
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...
void freeMemory()
Free the memory for the motions contained in a grid.
unsigned int iteration
The iteration at which this cell was created.
void clear()
Restore the discretization to its original form.
One-level discretization used for KPIECE.
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 addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
double uniform01()
Generate a random real between 0 and 1.
The exception type for ompl.
Definition: Exception.h:78
Representation of a simple grid.
Definition: Grid.h:83
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
bool operator()(const CellData *const a, const CellData *const b) const
Order function.
Main namespace. Contains everything in this library.