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