All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 
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