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) : base::Planner(si, "KPIECE1")
00085 {
00086 specs_.approximateSolutions = true;
00087
00088 siC_ = si.get();
00089 nCloseSamples_ = 30;
00090 goalBias_ = 0.05;
00091 selectBorderFraction_ = 0.8;
00092 badScoreFactor_ = 0.45;
00093 goodScoreFactor_ = 0.9;
00094 tree_.grid.onCellUpdate(computeImportance, NULL);
00095 }
00096
00097 virtual ~KPIECE1(void)
00098 {
00099 freeMemory();
00100 }
00101
00102 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00103
00104 virtual void clear(void);
00105
00113 void setGoalBias(double goalBias)
00114 {
00115 goalBias_ = goalBias;
00116 }
00117
00119 double getGoalBias(void) const
00120 {
00121 return goalBias_;
00122 }
00123
00130 void setBorderFraction(double bp)
00131 {
00132 selectBorderFraction_ = bp;
00133 }
00134
00137 double getBorderFraction(void) const
00138 {
00139 return selectBorderFraction_;
00140 }
00141
00142
00149 void setCellScoreFactor(double good, double bad)
00150 {
00151 goodScoreFactor_ = good;
00152 badScoreFactor_ = bad;
00153 }
00154
00157 double getGoodCellScoreFactor(void) const
00158 {
00159 return goodScoreFactor_;
00160 }
00161
00164 double getBadCellScoreFactor(void) const
00165 {
00166 return badScoreFactor_;
00167 }
00168
00171 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00172 {
00173 projectionEvaluator_ = projectionEvaluator;
00174 }
00175
00178 void setProjectionEvaluator(const std::string &name)
00179 {
00180 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00181 }
00182
00184 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00185 {
00186 return projectionEvaluator_;
00187 }
00188
00189 virtual void setup(void);
00190 virtual void getPlannerData(base::PlannerData &data) const;
00191
00192 protected:
00193
00195 struct Motion
00196 {
00197 Motion(void) : state(NULL), control(NULL), steps(0), parent(NULL)
00198 {
00199 }
00200
00202 Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
00203 {
00204 }
00205
00206 ~Motion(void)
00207 {
00208 }
00209
00211 base::State *state;
00212
00214 Control *control;
00215
00217 unsigned int steps;
00218
00220 Motion *parent;
00221 };
00222
00224 struct CellData
00225 {
00226 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00227 {
00228 }
00229
00230 ~CellData(void)
00231 {
00232 }
00233
00235 std::vector<Motion*> motions;
00236
00240 double coverage;
00241
00244 unsigned int selections;
00245
00249 double score;
00250
00252 unsigned int iteration;
00253
00255 double importance;
00256 };
00257
00260 struct OrderCellsByImportance
00261 {
00262 bool operator()(const CellData * const a, const CellData * const b) const
00263 {
00264 return a->importance > b->importance;
00265 }
00266 };
00267
00269 typedef GridB<CellData*, OrderCellsByImportance> Grid;
00270
00272 struct CloseSample
00273 {
00275 CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
00276 {
00277 }
00278
00280 Grid::Cell *cell;
00281
00283 Motion *motion;
00284
00286 double distance;
00287
00289 bool operator<(const CloseSample &other) const
00290 {
00291 return distance < other.distance;
00292 }
00293 };
00294
00296 struct CloseSamples
00297 {
00299 CloseSamples(unsigned int size) : maxSize(size)
00300 {
00301 }
00302
00308 bool consider(Grid::Cell *cell, Motion *motion, double distance);
00309
00315 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00316
00318 bool canSample(void) const
00319 {
00320 return samples.size() > 0;
00321 }
00322
00324 unsigned int maxSize;
00325
00327 std::set<CloseSample> samples;
00328 };
00329
00330
00332 struct TreeData
00333 {
00334 TreeData(void) : grid(0), size(0), iteration(1)
00335 {
00336 }
00337
00340 Grid grid;
00341
00344 unsigned int size;
00345
00347 unsigned int iteration;
00348 };
00349
00353 static void computeImportance(Grid::Cell *cell, void*)
00354 {
00355 CellData &cd = *(cell->data);
00356 cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00357 }
00358
00360 void freeMemory(void);
00361
00363 void freeGridMotions(Grid &grid);
00364
00366 void freeCellData(CellData *cdata);
00367
00369 void freeMotion(Motion *motion);
00370
00376 Grid::Cell* addMotion(Motion* motion, double dist);
00377
00381 bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00382
00386 unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
00387
00389 ControlSamplerPtr controlSampler_;
00390
00392 TreeData tree_;
00393
00395 const SpaceInformation *siC_;
00396
00400 base::ProjectionEvaluatorPtr projectionEvaluator_;
00401
00405 double goodScoreFactor_;
00406
00410 double badScoreFactor_;
00411
00412 unsigned int nCloseSamples_;
00413
00416 double selectBorderFraction_;
00417
00419 double goalBias_;
00420
00422 RNG rng_;
00423 };
00424
00425 }
00426 }
00427
00428 #endif