37 #ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
38 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
40 #include "ompl/base/Planner.h"
41 #include "ompl/datastructures/GridB.h"
42 #include "ompl/util/Exception.h"
43 #include <boost/function.hpp>
58 template<
typename Motion>
123 freeMotion_(freeMotion)
126 selectBorderFraction_ = 0.9;
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;
151 return selectBorderFraction_;
169 void countIteration(
void)
174 std::size_t getMotionCount(
void)
const
179 std::size_t getCellCount(
void)
const
188 freeCellData(it->second->data);
204 unsigned int created = 0;
207 cell->
data->motions.push_back(motion);
208 cell->data->coverage += 1.0;
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);
238 if (scell->data->score < std::numeric_limits<double>::epsilon())
240 std::vector<CellData*> content;
241 content.reserve(grid_.
size());
243 for (
typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
244 (*it)->score += 1.0 +
log((
double)((*it)->iteration));
248 assert(scell && !scell->data->motions.empty());
250 ++scell->data->selections;
251 smotion = scell->data->motions[rng_.
halfNormalInt(0, scell->data->motions.size() - 1)];
254 bool removeMotion(Motion *motion,
const Coord &coord)
260 for (
unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
261 if (cell->data->motions[i] == motion)
263 cell->
data->motions.erase(cell->data->motions.begin() + i);
268 if (cell->data->motions.empty())
271 freeCellData(cell->data);
279 void updateCell(
Cell *cell)
284 const Grid& getGrid(
void)
const
289 void getPlannerData(base::PlannerData &data,
int tag,
bool start,
const Motion *lastGoalMotion)
const
291 std::vector<CellData*> cdata;
295 data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
297 for (
unsigned int i = 0 ; i < cdata.size() ; ++i)
298 for (
unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
300 if (cdata[i]->motions[j]->parent == NULL)
303 data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
305 data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
310 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
311 base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
313 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
314 base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
322 void freeCellData(CellData *cdata)
324 for (
unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
325 freeMotion_(cdata->motions[i]);
332 static void computeImportance(
Cell *cell,
void*)
334 CellData &cd = *(cell->data);
335 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
347 unsigned int iteration_;
357 double selectBorderFraction_;
virtual void add(Cell *cell)
Add the cell to the grid.
Grid::Coord Coord
The datatype for the maintained grid coordinates.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first...
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...
GridN< CellData * >::Coord Coord
Datatype for cell coordinates.
void onCellUpdate(EventCellUpdate event, void *arg)
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.
unsigned int iteration
The iteration at which this cell was created.
Cell * topExternal(void) const
Return the cell that is at the top of the heap maintaining external cells.
GridN< CellData * >::Cell Cell
Definition of a cell in this grid.
Cell * topInternal(void) const
Return the cell that is at the top of the heap maintaining internal cells.
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion lengths...
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_...
void update(Cell *cell)
Update the position in the heaps for a particular cell.
void setDimension(unsigned int dimension)
iterator begin(void) const
Return the begin() iterator for the grid.
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 ...
bool operator()(const CellData *const a, const CellData *const b) const
Order function.
void freeMemory(void)
Free the memory for the motions contained in a grid.
void updateAll(void)
Update all cells and reconstruct the heaps.
void clear(void)
Restore the discretization to its original form.
virtual bool remove(Cell *cell)
Remove a cell from the grid.
std::vector< Motion * > motions
The set of motions contained in this grid cell.
The exception type for ompl.
Grid::Cell Cell
The datatype for the maintained grid cells.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
Create a cell but do not add it to the grid; update neighboring cells however.
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...
double uniform01(void)
Generate a random real between 0 and 1.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
double getBorderFraction(void) const
Set the fraction of time for focusing on the border (between 0 and 1).
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...
double fracExternal(void) const
Return the fraction of external cells.
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
iterator end(void) const
Return the end() iterator for the grid.
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.
unsigned int size(void) const
Check the size of the grid.
virtual void clear(void)
Clear all cells in the grid.