00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
00038 #define OMPL_GEOMETRIC_PLANNERS_KPIECE_DISCRETIZATION_
00039
00040 #include "ompl/base/Planner.h"
00041 #include "ompl/datastructures/GridB.h"
00042 #include "ompl/util/Exception.h"
00043 #include <boost/function.hpp>
00044 #include <vector>
00045 #include <limits>
00046 #include <cassert>
00047 #include <utility>
00048 #include <cstdlib>
00049 #include <cmath>
00050
00051 namespace ompl
00052 {
00053
00054 namespace geometric
00055 {
00056
00058 template<typename Motion>
00059 class Discretization
00060 {
00061 public:
00062
00064 struct CellData
00065 {
00066 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00067 {
00068 }
00069
00070 ~CellData(void)
00071 {
00072 }
00073
00075 std::vector<Motion*> motions;
00076
00080 double coverage;
00081
00084 unsigned int selections;
00085
00089 double score;
00090
00092 unsigned int iteration;
00093
00095 double importance;
00096 };
00097
00100 struct OrderCellsByImportance
00101 {
00102
00104 bool operator()(const CellData * const a, const CellData * const b) const
00105 {
00106 return a->importance > b->importance;
00107 }
00108 };
00109
00111 typedef GridB<CellData*, OrderCellsByImportance> Grid;
00112
00114 typedef typename Grid::Cell Cell;
00115
00117 typedef typename Grid::Coord Coord;
00118
00120 typedef typename boost::function<void(Motion*)> FreeMotionFn;
00121
00122 Discretization(const FreeMotionFn &freeMotion) : grid_(0), size_(0), iteration_(1), recentCell_(NULL),
00123 freeMotion_(freeMotion)
00124 {
00125 grid_.onCellUpdate(computeImportance, NULL);
00126 selectBorderFraction_ = 0.9;
00127 }
00128
00129 ~Discretization(void)
00130 {
00131 freeMemory();
00132 }
00133
00140 void setBorderFraction(double bp)
00141 {
00142 if (bp < std::numeric_limits<double>::epsilon() || bp > 1.0)
00143 throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00144 selectBorderFraction_ = bp;
00145 }
00146
00149 double getBorderFraction(void) const
00150 {
00151 return selectBorderFraction_;
00152 }
00153
00155 void setDimension(unsigned int dim)
00156 {
00157 grid_.setDimension(dim);
00158 }
00159
00161 void clear(void)
00162 {
00163 freeMemory();
00164 size_ = 0;
00165 iteration_ = 1;
00166 recentCell_ = NULL;
00167 }
00168
00169 void countIteration(void)
00170 {
00171 ++iteration_;
00172 }
00173
00174 std::size_t getMotionCount(void) const
00175 {
00176 return size_;
00177 }
00178
00179 std::size_t getCellCount(void) const
00180 {
00181 return grid_.size();
00182 }
00183
00185 void freeMemory(void)
00186 {
00187 for (typename Grid::iterator it = grid_.begin(); it != grid_.end() ; ++it)
00188 freeCellData(it->second->data);
00189 grid_.clear();
00190 }
00191
00200 unsigned int addMotion(Motion* motion, const Coord &coord, double dist = 0.0)
00201 {
00202 Cell *cell = grid_.getCell(coord);
00203
00204 unsigned int created = 0;
00205 if (cell)
00206 {
00207 cell->data->motions.push_back(motion);
00208 cell->data->coverage += 1.0;
00209 grid_.update(cell);
00210 }
00211 else
00212 {
00213 cell = grid_.createCell(coord);
00214 cell->data = new CellData();
00215 cell->data->motions.push_back(motion);
00216 cell->data->coverage = 1.0;
00217 cell->data->iteration = iteration_;
00218 cell->data->selections = 1;
00219 cell->data->score = (1.0 + log((double)(iteration_))) / (1.0 + dist);
00220 grid_.add(cell);
00221 recentCell_ = cell;
00222 created = 1;
00223 }
00224 ++size_;
00225 return created;
00226 }
00227
00231 void selectMotion(Motion* &smotion, Cell* &scell)
00232 {
00233 scell = rng_.uniform01() < std::max(selectBorderFraction_, grid_.fracExternal()) ?
00234 grid_.topExternal() : grid_.topInternal();
00235
00236
00237
00238 if (scell->data->score < std::numeric_limits<double>::epsilon())
00239 {
00240 std::vector<CellData*> content;
00241 content.reserve(grid_.size());
00242 grid_.getContent(content);
00243 for (typename std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00244 (*it)->score += 1.0 + log((double)((*it)->iteration));
00245 grid_.updateAll();
00246 }
00247
00248 assert(scell && !scell->data->motions.empty());
00249
00250 ++scell->data->selections;
00251 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00252 }
00253
00254 bool removeMotion(Motion *motion, const Coord &coord)
00255 {
00256 Cell* cell = grid_.getCell(coord);
00257 if (cell)
00258 {
00259 bool found = false;
00260 for (unsigned int i = 0 ; i < cell->data->motions.size(); ++i)
00261 if (cell->data->motions[i] == motion)
00262 {
00263 cell->data->motions.erase(cell->data->motions.begin() + i);
00264 found = true;
00265 --size_;
00266 break;
00267 }
00268 if (cell->data->motions.empty())
00269 {
00270 grid_.remove(cell);
00271 freeCellData(cell->data);
00272 grid_.destroyCell(cell);
00273 }
00274 return found;
00275 }
00276 return false;
00277 }
00278
00279 void updateCell(Cell *cell)
00280 {
00281 grid_.update(cell);
00282 }
00283
00284 const Grid& getGrid(void) const
00285 {
00286 return grid_;
00287 }
00288
00289 void getPlannerData(base::PlannerData &data, int tag, bool start, const Motion *lastGoalMotion) const
00290 {
00291 std::vector<CellData*> cdata;
00292 grid_.getContent(cdata);
00293
00294 if (lastGoalMotion)
00295 data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion->state, tag));
00296
00297 for (unsigned int i = 0 ; i < cdata.size() ; ++i)
00298 for (unsigned int j = 0 ; j < cdata[i]->motions.size() ; ++j)
00299 {
00300 if (cdata[i]->motions[j]->parent == NULL)
00301 {
00302 if (start)
00303 data.addStartVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00304 else
00305 data.addGoalVertex(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00306 }
00307 else
00308 {
00309 if (start)
00310 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag),
00311 base::PlannerDataVertex(cdata[i]->motions[j]->state, tag));
00312 else
00313 data.addEdge(base::PlannerDataVertex(cdata[i]->motions[j]->state, tag),
00314 base::PlannerDataVertex(cdata[i]->motions[j]->parent->state, tag));
00315 }
00316 }
00317 }
00318
00319 private:
00320
00322 void freeCellData(CellData *cdata)
00323 {
00324 for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00325 freeMotion_(cdata->motions[i]);
00326 delete cdata;
00327 }
00328
00332 static void computeImportance(Cell *cell, void*)
00333 {
00334 CellData &cd = *(cell->data);
00335 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00336 }
00337
00340 Grid grid_;
00341
00344 std::size_t size_;
00345
00347 unsigned int iteration_;
00348
00350 Cell *recentCell_;
00351
00353 FreeMotionFn freeMotion_;
00354
00357 double selectBorderFraction_;
00358
00360 RNG rng_;
00361
00362 };
00363 }
00364 }
00365
00366 #endif