Loading...
Searching...
No Matches
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
52namespace 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 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
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
The exception type for ompl.
Definition Exception.h:47
void updateAll()
Update all cells and reconstruct the heaps.
Definition GridB.h:156
virtual void add(Cell *cell)
Add the cell to the grid.
Definition GridB.h:210
virtual bool remove(Cell *cell)
Remove a cell from the grid.
Definition GridB.h:224
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Definition GridB.h:113
typename GridN< CellData * >::Coord Coord
Definition GridB.h:61
void onCellUpdate(EventCellUpdate event, void *arg)
Definition GridB.h:99
typename GridN< CellData * >::Cell Cell
Definition GridB.h:55
void clear() override
Clear all cells in the grid.
Definition GridB.h:272
double fracExternal() const
Return the fraction of external cells.
Definition GridB.h:132
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Definition GridB.h:106
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Definition GridB.h:144
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:167
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition GridN.h:123
void setDimension(unsigned int dimension)
Definition GridN.h:89
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:252
unsigned int size() const
Check the size of the grid.
Definition Grid.h:294
iterator end() const
Return the end() iterator for the grid.
Definition Grid.h:375
iterator begin() const
Return the begin() iterator for the grid.
Definition Grid.h:369
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
Definition Grid.h:258
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_...
double uniform01()
Generate a random real between 0 and 1.
One-level discretization used for KPIECE.
void freeMemory()
Free the memory for the motions contained in a grid.
typename Grid::Coord Coord
The datatype for the maintained grid coordinates.
double getBorderFraction() const
Set the fraction of time for focusing on the border (between 0 and 1).
void clear()
Restore the discretization to its original form.
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 ...
typename 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...
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 setDimension(unsigned int dim)
Set the dimension of the grid to be maintained.
typename std::function< void(Motion *)> FreeMotionFn
The signature of a function that frees the memory for a motion.
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
Main namespace. Contains everything in this library.
STL namespace.
The data held by a cell in the grid of motions.
std::vector< Motion * > motions
The set of motions contained in this grid cell.
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
unsigned int selections
The number of times this cell has been selected for expansion.
double importance
The computed importance (based on other class members)
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion lengths.
unsigned int iteration
The iteration at which this cell was created.
Definintion of an operator passed to the Grid structure, to order cells by importance.
bool operator()(const CellData *const a, const CellData *const b) const
Order function.