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 
00045 namespace ompl
00046 {
00047 
00048     namespace control
00049     {
00050 
00078         class KPIECE1 : public base::Planner
00079         {
00080         public:
00081 
00083             KPIECE1(const SpaceInformationPtr &si) : base::Planner(si, "KPIECE1")
00084             {
00085                 type_ = base::PLAN_TO_GOAL_ANY;
00086 
00087                 siC_ = si.get();
00088                 goalBias_ = 0.05;
00089                 selectBorderFraction_ = 0.7;
00090                 badScoreFactor_ = 0.3;
00091                 goodScoreFactor_ = 0.9;
00092                 tree_.grid.onCellUpdate(computeImportance, NULL);
00093             }
00094 
00095             virtual ~KPIECE1(void)
00096             {
00097                 freeMemory();
00098             }
00099 
00100             virtual bool solve(const base::PlannerTerminationCondition &ptc);
00101 
00102             virtual void clear(void);
00103 
00111             void setGoalBias(double goalBias)
00112             {
00113                 goalBias_ = goalBias;
00114             }
00115 
00117             double getGoalBias(void) const
00118             {
00119                 return goalBias_;
00120             }
00121 
00128             void setBorderFraction(double bp)
00129             {
00130                 selectBorderFraction_ = bp;
00131             }
00132 
00135             double getBorderFraction(void) const
00136             {
00137                 return selectBorderFraction_;
00138             }
00139 
00140 
00147             void setCellScoreFactor(double good, double bad)
00148             {
00149                 goodScoreFactor_ = good;
00150                 badScoreFactor_ = bad;
00151             }
00152 
00155             double getGoodCellScoreFactor(void) const
00156             {
00157                 return goodScoreFactor_;
00158             }
00159 
00162             double getBadCellScoreFactor(void) const
00163             {
00164                 return badScoreFactor_;
00165             }
00166 
00169             void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00170             {
00171                 projectionEvaluator_ = projectionEvaluator;
00172             }
00173 
00176             void setProjectionEvaluator(const std::string &name)
00177             {
00178                 projectionEvaluator_ = si_->getStateManifold()->getProjection(name);
00179             }
00180 
00182             const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00183             {
00184                 return projectionEvaluator_;
00185             }
00186 
00187             virtual void setup(void);
00188             virtual void getPlannerData(base::PlannerData &data) const;
00189 
00190         protected:
00191 
00193             class Motion
00194             {
00195             public:
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             };
00223 
00225             struct CellData
00226             {
00227                 CellData(void) : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
00228                 {
00229                 }
00230 
00231                 ~CellData(void)
00232                 {
00233                 }
00234 
00236                 std::vector<Motion*> motions;
00237 
00241                 double               coverage;
00242 
00245                 unsigned int         selections;
00246 
00250                 double               score;
00251 
00253                 unsigned int         iteration;
00254 
00256                 double               importance;
00257             };
00258 
00261             struct OrderCellsByImportance
00262             {
00263                 bool operator()(const CellData * const a, const CellData * const b) const
00264                 {
00265                     return a->importance > b->importance;
00266                 }
00267             };
00268 
00270             typedef GridB<CellData*, OrderCellsByImportance> Grid;
00271 
00273             struct TreeData
00274             {
00275                 TreeData(void) : grid(0), size(0), iteration(1)
00276                 {
00277                 }
00278 
00281                 Grid         grid;
00282 
00285                 unsigned int size;
00286 
00288                 unsigned int iteration;
00289             };
00290 
00294             static void computeImportance(Grid::Cell *cell, void*)
00295             {
00296                 CellData &cd = *(cell->data);
00297                 cd.importance =  cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
00298             }
00299 
00301             void freeMemory(void);
00302 
00304             void freeGridMotions(Grid &grid);
00305 
00307             void freeCellData(CellData *cdata);
00308 
00310             void freeMotion(Motion *motion);
00311 
00317             unsigned int addMotion(Motion* motion, double dist);
00318 
00322             bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
00323 
00327             unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
00328 
00330             ControlSamplerPtr             controlSampler_;
00331 
00333             TreeData                      tree_;
00334 
00336             const SpaceInformation       *siC_;
00337 
00341             base::ProjectionEvaluatorPtr  projectionEvaluator_;
00342 
00346             double                        goodScoreFactor_;
00347 
00351             double                        badScoreFactor_;
00352 
00353 
00356             double                        selectBorderFraction_;
00357 
00359             double                        goalBias_;
00360 
00362             RNG                           rng_;
00363         };
00364 
00365     }
00366 }
00367 
00368 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator