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
00079 class KPIECE1 : public base::Planner
00080 {
00081 public:
00082
00084 KPIECE1(const SpaceInformationPtr &si);
00085
00086 virtual ~KPIECE1(void);
00087
00088 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00089
00090 virtual void clear(void);
00091
00099 void setGoalBias(double goalBias)
00100 {
00101 goalBias_ = goalBias;
00102 }
00103
00105 double getGoalBias(void) const
00106 {
00107 return goalBias_;
00108 }
00109
00116 void setBorderFraction(double bp)
00117 {
00118 selectBorderFraction_ = bp;
00119 }
00120
00123 double getBorderFraction(void) const
00124 {
00125 return selectBorderFraction_;
00126 }
00127
00134 void setCellScoreFactor(double good, double bad)
00135 {
00136 setGoodCellScoreFactor(good);
00137 setBadCellScoreFactor(bad);
00138 }
00139
00141 void setBadCellScoreFactor(double bad)
00142 {
00143 badScoreFactor_ = bad;
00144 }
00145
00147 void setGoodCellScoreFactor(double good)
00148 {
00149 goodScoreFactor_ = good;
00150 }
00151
00154 double getGoodCellScoreFactor(void) const
00155 {
00156 return goodScoreFactor_;
00157 }
00158
00161 double getBadCellScoreFactor(void) const
00162 {
00163 return badScoreFactor_;
00164 }
00165
00168 void setMaxCloseSamplesCount(unsigned int nCloseSamples)
00169 {
00170 nCloseSamples_ = nCloseSamples;
00171 }
00172
00174 unsigned int getMaxCloseSamplesCount(void) const
00175 {
00176 return nCloseSamples_;
00177 }
00178
00181 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00182 {
00183 projectionEvaluator_ = projectionEvaluator;
00184 }
00185
00188 void setProjectionEvaluator(const std::string &name)
00189 {
00190 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00191 }
00192
00194 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00195 {
00196 return projectionEvaluator_;
00197 }
00198
00199 virtual void setup(void);
00200 virtual void getPlannerData(base::PlannerData &data) const;
00201
00202 protected:
00203
00205 struct Motion
00206 {
00207 Motion(void) : state(NULL), control(NULL), steps(0), parent(NULL)
00208 {
00209 }
00210
00212 Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
00213 {
00214 }
00215
00216 ~Motion(void)
00217 {
00218 }
00219
00221 base::State *state;
00222
00224 Control *control;
00225
00227 unsigned int steps;
00228
00230 Motion *parent;
00231 };
00232
00234 struct CellData
00235 {
00236 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00237 {
00238 }
00239
00240 ~CellData(void)
00241 {
00242 }
00243
00245 std::vector<Motion*> motions;
00246
00250 double coverage;
00251
00254 unsigned int selections;
00255
00259 double score;
00260
00262 unsigned int iteration;
00263
00265 double importance;
00266 };
00267
00270 struct OrderCellsByImportance
00271 {
00272 bool operator()(const CellData * const a, const CellData * const b) const
00273 {
00274 return a->importance > b->importance;
00275 }
00276 };
00277
00279 typedef GridB<CellData*, OrderCellsByImportance> Grid;
00280
00282 struct CloseSample
00283 {
00285 CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
00286 {
00287 }
00288
00290 Grid::Cell *cell;
00291
00293 Motion *motion;
00294
00296 double distance;
00297
00299 bool operator<(const CloseSample &other) const
00300 {
00301 return distance < other.distance;
00302 }
00303 };
00304
00306 struct CloseSamples
00307 {
00309 CloseSamples(unsigned int size) : maxSize(size)
00310 {
00311 }
00312
00318 bool consider(Grid::Cell *cell, Motion *motion, double distance);
00319
00325 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00326
00328 bool canSample(void) const
00329 {
00330 return samples.size() > 0;
00331 }
00332
00334 unsigned int maxSize;
00335
00337 std::set<CloseSample> samples;
00338 };
00339
00340
00342 struct TreeData
00343 {
00344 TreeData(void) : grid(0), size(0), iteration(1)
00345 {
00346 }
00347
00350 Grid grid;
00351
00354 unsigned int size;
00355
00357 unsigned int iteration;
00358 };
00359
00363 static void computeImportance(Grid::Cell *cell, void*)
00364 {
00365 CellData &cd = *(cell->data);
00366 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00367 }
00368
00370 void freeMemory(void);
00371
00373 void freeGridMotions(Grid &grid);
00374
00376 void freeCellData(CellData *cdata);
00377
00379 void freeMotion(Motion *motion);
00380
00386 Grid::Cell* addMotion(Motion* motion, double dist);
00387
00391 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00392
00396 unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
00397
00399 ControlSamplerPtr controlSampler_;
00400
00402 TreeData tree_;
00403
00405 const SpaceInformation *siC_;
00406
00410 base::ProjectionEvaluatorPtr projectionEvaluator_;
00411
00415 double goodScoreFactor_;
00416
00420 double badScoreFactor_;
00421
00425 unsigned int nCloseSamples_;
00426
00429 double selectBorderFraction_;
00430
00432 double goalBias_;
00433
00435 RNG rng_;
00436 };
00437
00438 }
00439 }
00440
00441 #endif