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_CONTROL_PLANNERS_KPIECE_KPIECE1_
00038 #define OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
00039
00040 #include "ompl/control/planners/PlannerIncludes.h"
00041 #include "ompl/base/ProjectionEvaluator.h"
00042 #include "ompl/datastructures/GridB.h"
00043 #include <vector>
00044 #include <set>
00045
00046 namespace ompl
00047 {
00048
00049 namespace control
00050 {
00051
00077 class KPIECE1 : public base::Planner
00078 {
00079 public:
00080
00082 KPIECE1(const SpaceInformationPtr &si);
00083
00084 virtual ~KPIECE1(void);
00085
00086 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00087
00088 virtual void clear(void);
00089
00097 void setGoalBias(double goalBias)
00098 {
00099 goalBias_ = goalBias;
00100 }
00101
00103 double getGoalBias(void) const
00104 {
00105 return goalBias_;
00106 }
00107
00114 void setBorderFraction(double bp)
00115 {
00116 selectBorderFraction_ = bp;
00117 }
00118
00121 double getBorderFraction(void) const
00122 {
00123 return selectBorderFraction_;
00124 }
00125
00132 void setCellScoreFactor(double good, double bad)
00133 {
00134 setGoodCellScoreFactor(good);
00135 setBadCellScoreFactor(bad);
00136 }
00137
00139 void setBadCellScoreFactor(double bad)
00140 {
00141 badScoreFactor_ = bad;
00142 }
00143
00145 void setGoodCellScoreFactor(double good)
00146 {
00147 goodScoreFactor_ = good;
00148 }
00149
00152 double getGoodCellScoreFactor(void) const
00153 {
00154 return goodScoreFactor_;
00155 }
00156
00159 double getBadCellScoreFactor(void) const
00160 {
00161 return badScoreFactor_;
00162 }
00163
00166 void setMaxCloseSamplesCount(unsigned int nCloseSamples)
00167 {
00168 nCloseSamples_ = nCloseSamples;
00169 }
00170
00172 unsigned int getMaxCloseSamplesCount(void) const
00173 {
00174 return nCloseSamples_;
00175 }
00176
00179 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00180 {
00181 projectionEvaluator_ = projectionEvaluator;
00182 }
00183
00186 void setProjectionEvaluator(const std::string &name)
00187 {
00188 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00189 }
00190
00192 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00193 {
00194 return projectionEvaluator_;
00195 }
00196
00197 virtual void setup(void);
00198 virtual void getPlannerData(base::PlannerData &data) const;
00199
00200 protected:
00201
00203 struct Motion
00204 {
00205 Motion(void) : state(NULL), control(NULL), steps(0), parent(NULL)
00206 {
00207 }
00208
00210 Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
00211 {
00212 }
00213
00214 ~Motion(void)
00215 {
00216 }
00217
00219 base::State *state;
00220
00222 Control *control;
00223
00225 unsigned int steps;
00226
00228 Motion *parent;
00229 };
00230
00232 struct CellData
00233 {
00234 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00235 {
00236 }
00237
00238 ~CellData(void)
00239 {
00240 }
00241
00243 std::vector<Motion*> motions;
00244
00248 double coverage;
00249
00252 unsigned int selections;
00253
00257 double score;
00258
00260 unsigned int iteration;
00261
00263 double importance;
00264 };
00265
00268 struct OrderCellsByImportance
00269 {
00270 bool operator()(const CellData * const a, const CellData * const b) const
00271 {
00272 return a->importance > b->importance;
00273 }
00274 };
00275
00277 typedef GridB<CellData*, OrderCellsByImportance> Grid;
00278
00280 struct CloseSample
00281 {
00283 CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
00284 {
00285 }
00286
00288 Grid::Cell *cell;
00289
00291 Motion *motion;
00292
00294 double distance;
00295
00297 bool operator<(const CloseSample &other) const
00298 {
00299 return distance < other.distance;
00300 }
00301 };
00302
00304 struct CloseSamples
00305 {
00307 CloseSamples(unsigned int size) : maxSize(size)
00308 {
00309 }
00310
00316 bool consider(Grid::Cell *cell, Motion *motion, double distance);
00317
00323 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00324
00326 bool canSample(void) const
00327 {
00328 return samples.size() > 0;
00329 }
00330
00332 unsigned int maxSize;
00333
00335 std::set<CloseSample> samples;
00336 };
00337
00338
00340 struct TreeData
00341 {
00342 TreeData(void) : grid(0), size(0), iteration(1)
00343 {
00344 }
00345
00348 Grid grid;
00349
00352 unsigned int size;
00353
00355 unsigned int iteration;
00356 };
00357
00361 static void computeImportance(Grid::Cell *cell, void*)
00362 {
00363 CellData &cd = *(cell->data);
00364 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00365 }
00366
00368 void freeMemory(void);
00369
00371 void freeGridMotions(Grid &grid);
00372
00374 void freeCellData(CellData *cdata);
00375
00377 void freeMotion(Motion *motion);
00378
00384 Grid::Cell* addMotion(Motion* motion, double dist);
00385
00389 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00390
00394 unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
00395
00397 ControlSamplerPtr controlSampler_;
00398
00400 TreeData tree_;
00401
00403 const SpaceInformation *siC_;
00404
00408 base::ProjectionEvaluatorPtr projectionEvaluator_;
00409
00413 double goodScoreFactor_;
00414
00418 double badScoreFactor_;
00419
00423 unsigned int nCloseSamples_;
00424
00427 double selectBorderFraction_;
00428
00430 double goalBias_;
00431
00433 RNG rng_;
00434
00436 Motion *lastGoalMotion_;
00437 };
00438
00439 }
00440 }
00441
00442 #endif