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() = 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 
95  {
97  bool operator()(const CellData *const a, const CellData *const b) const
98  {
99  return a->importance > b->importance;
100  }
101  };
102 
105 
107  typedef typename Grid::Cell Cell;
108 
110  typedef typename Grid::Coord Coord;
111 
113  typedef typename std::function<void(Motion *)> FreeMotionFn;
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 
122  ~Discretization()
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
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.