ompl/geometric/planners/pdst/PDST.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, 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: Jonathan Sobieski, Mark Moll */
00036 
00037 
00038 
00039 #ifndef OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
00040 #define OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
00041 
00042 #include "ompl/base/Planner.h"
00043 #include "ompl/base/goals/GoalSampleableRegion.h"
00044 #include "ompl/geometric/PathGeometric.h"
00045 #include "ompl/base/PlannerData.h"
00046 #include "ompl/datastructures/BinaryHeap.h"
00047 
00048 
00049 namespace ompl
00050 {
00051 
00052     namespace geometric
00053     {
00054 
00079 
00080         class PDST : public base::Planner
00081         {
00082         public:
00083 
00084             PDST(const base::SpaceInformationPtr &si);
00085 
00086             virtual ~PDST();
00087 
00088             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00089             virtual void clear();
00090             virtual void setup();
00091 
00093             virtual void getPlannerData(base::PlannerData &data) const;
00094 
00096             void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00097             {
00098                 projectionEvaluator_ = projectionEvaluator;
00099             }
00100 
00102             void setProjectionEvaluator(const std::string &name)
00103             {
00104                 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00105             }
00106 
00108             const base::ProjectionEvaluatorPtr& getProjectionEvaluator() const
00109             {
00110                 return projectionEvaluator_;
00111             }
00112 
00120             void setGoalBias(double goalBias)
00121             {
00122                 goalBias_ = goalBias;
00123             }
00125             double getGoalBias() const
00126             {
00127                 return goalBias_;
00128             }
00129 
00130         protected:
00131             struct Cell;
00132             struct Motion;
00133 
00135             struct MotionCompare
00136             {
00138                 bool operator()(Motion *p1, Motion *p2) const
00139                 {
00140                     // lowest priority means highest score
00141                     return p1->score() < p2->score();
00142                 }
00143             };
00144 
00146             struct Motion
00147             {
00148             public:
00149                 Motion(base::State *startState, base::State *endState, double priority, Motion *parent)
00150                     : startState_(startState), endState_(endState), priority_(priority),
00151                     parent_(parent), cell_(NULL), heapElement_(NULL), isSplit_(false)
00152                 {
00153                 }
00155                 Motion(base::State *state)
00156                     : startState_(state), endState_(state), priority_(0.),
00157                     parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
00158                 {
00159                 }
00161                 double score() const
00162                 {
00163                     return priority_ / cell_->volume_;
00164                 }
00165                 void updatePriority()
00166                 {
00167                     priority_ = priority_ * 2.0 + 1.0;
00168                 }
00169                 Motion* ancestor() const
00170                 {
00171                     Motion *m = const_cast<Motion*>(this);
00172                     while (m->parent_ && m->parent_->endState_ == m->startState_)
00173                         m = m->parent_;
00174                     return m;
00175                 }
00176 
00178                 ompl::base::State               *startState_;
00180                 ompl::base::State               *endState_;
00182                 double                           priority_;
00184                 Motion *parent_;
00186                 Cell*                            cell_;
00188                 ompl::BinaryHeap<Motion*, MotionCompare>::Element *heapElement_;
00191                 bool                             isSplit_;
00192             };
00193 
00195             struct Cell
00196             {
00197                 Cell(double volume, const ompl::base::RealVectorBounds &bounds,
00198                     unsigned int splitDimension = 0)
00199                     : volume_(volume), splitDimension_(splitDimension), splitValue_(0.0),
00200                     left_(NULL), right_(NULL), bounds_(bounds)
00201                 {
00202                 }
00203 
00204                 ~Cell()
00205                 {
00206                     if (left_)
00207                     {
00208                         delete left_;
00209                         delete right_;
00210                     }
00211                 }
00212 
00214                 void subdivide(unsigned int spaceDimension);
00215 
00217                 Cell* stab(const ompl::base::EuclideanProjection& projection) const
00218                 {
00219                     Cell *containingCell = const_cast<Cell*>(this);
00220                     while (containingCell->left_ != NULL)
00221                     {
00222                         if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
00223                             containingCell = containingCell->left_;
00224                         else
00225                             containingCell = containingCell->right_;
00226                     }
00227                     return containingCell;
00228                 }
00230                 void addMotion(Motion *motion)
00231                 {
00232                     motions_.push_back(motion);
00233                     motion->cell_ = this;
00234                 }
00235 
00237                 unsigned int size() const
00238                 {
00239                     unsigned int sz = 1;
00240                     if (left_)
00241                         sz += left_->size() + right_->size();
00242                     return sz;
00243                 }
00244 
00246                 double                       volume_;
00248                 unsigned int                 splitDimension_;
00250                 double                       splitValue_;
00252                 Cell*                        left_;
00254                 Cell*                        right_;
00256                 ompl::base::RealVectorBounds bounds_;
00258                 std::vector<Motion*>         motions_;
00259             };
00260 
00261 
00263             void addMotion(Motion *motion, Cell *cell, base::State*, base::EuclideanProjection&);
00265             void updateHeapElement(Motion *motion)
00266             {
00267                 if (motion->heapElement_)
00268                     priorityQueue_.update(motion->heapElement_);
00269                 else
00270                     motion->heapElement_ = priorityQueue_.insert(motion);
00271             }
00275             Motion* propagateFrom(Motion *motion, base::State*, base::State*);
00276 
00277             void freeMemory();
00278 
00280             ompl::base::StateSamplerPtr              sampler_;
00281             // Random number generator
00282             RNG                                      rng_;
00285             std::vector<Motion*>                     startMotions_;
00287             ompl::BinaryHeap<Motion*, MotionCompare> priorityQueue_;
00289             Cell*                                    bsp_;
00291             ompl::base::ProjectionEvaluatorPtr       projectionEvaluator_;
00293             double                                   goalBias_;
00295             ompl::base::GoalSampleableRegion        *goalSampler_;
00297             unsigned int                             iteration_;
00299             Motion                                  *lastGoalMotion_;
00300         };
00301     }
00302 }
00303 
00304 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines