ompl/control/planners/kpiece/KPIECE1.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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(); 00085 00086 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00087 00088 virtual void clear(); 00089 00097 void setGoalBias(double goalBias) 00098 { 00099 goalBias_ = goalBias; 00100 } 00101 00103 double getGoalBias() const 00104 { 00105 return goalBias_; 00106 } 00107 00114 void setBorderFraction(double bp) 00115 { 00116 selectBorderFraction_ = bp; 00117 } 00118 00121 double getBorderFraction() 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() const 00153 { 00154 return goodScoreFactor_; 00155 } 00156 00159 double getBadCellScoreFactor() const 00160 { 00161 return badScoreFactor_; 00162 } 00163 00166 void setMaxCloseSamplesCount(unsigned int nCloseSamples) 00167 { 00168 nCloseSamples_ = nCloseSamples; 00169 } 00170 00172 unsigned int getMaxCloseSamplesCount() 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() const 00193 { 00194 return projectionEvaluator_; 00195 } 00196 00197 virtual void setup(); 00198 virtual void getPlannerData(base::PlannerData &data) const; 00199 00200 protected: 00201 00203 struct Motion 00204 { 00205 Motion() : 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() 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() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0) 00235 { 00236 } 00237 00238 ~CellData() 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() 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() : 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(); 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