ompl/control/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 #ifndef OMPL_CONTROL_PLANNERS_PDST_PDST_ 00039 #define OMPL_CONTROL_PLANNERS_PDST_PDST_ 00040 00041 #include "ompl/base/Planner.h" 00042 #include "ompl/base/goals/GoalSampleableRegion.h" 00043 #include "ompl/control/PathControl.h" 00044 #include "ompl/control/PlannerData.h" 00045 #include "ompl/datastructures/BinaryHeap.h" 00046 00047 00048 namespace ompl 00049 { 00050 00051 namespace control 00052 { 00053 00081 00082 class PDST : public base::Planner 00083 { 00084 public: 00085 00086 PDST(const SpaceInformationPtr &si); 00087 00088 virtual ~PDST(); 00089 00090 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00091 virtual void clear(); 00092 virtual void setup(); 00093 00095 virtual void getPlannerData(base::PlannerData &data) const; 00096 00098 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator) 00099 { 00100 projectionEvaluator_ = projectionEvaluator; 00101 } 00102 00104 void setProjectionEvaluator(const std::string &name) 00105 { 00106 projectionEvaluator_ = si_->getStateSpace()->getProjection(name); 00107 } 00108 00110 const base::ProjectionEvaluatorPtr& getProjectionEvaluator() const 00111 { 00112 return projectionEvaluator_; 00113 } 00114 00122 void setGoalBias(double goalBias) 00123 { 00124 goalBias_ = goalBias; 00125 } 00127 double getGoalBias() const 00128 { 00129 return goalBias_; 00130 } 00131 00132 protected: 00133 struct Cell; 00134 struct Motion; 00135 00137 struct MotionCompare 00138 { 00140 bool operator() (Motion *p1, Motion *p2) const 00141 { 00142 // lowest priority means highest score 00143 return p1->score() < p2->score(); 00144 } 00145 }; 00146 00148 struct Motion 00149 { 00150 public: 00151 Motion(base::State *startState, base::State *endState, Control *control, 00152 unsigned int controlDuration, double priority, Motion *parent) 00153 : startState_(startState), endState_(endState), control_(control), 00154 controlDuration_(controlDuration), priority_(priority), parent_(parent), 00155 cell_(NULL), heapElement_(NULL), isSplit_(false) 00156 { 00157 } 00159 Motion(base::State *state) 00160 : startState_(state), endState_(state), control_(NULL), controlDuration_(0), 00161 priority_(0.), parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false) 00162 { 00163 } 00165 double score() const 00166 { 00167 return priority_ / cell_->volume_; 00168 } 00169 void updatePriority() 00170 { 00171 priority_ = priority_ * 2. + 1.; 00172 } 00173 00175 base::State *startState_; 00177 base::State *endState_; 00179 control::Control *control_; 00181 unsigned int controlDuration_; 00183 double priority_; 00185 Motion *parent_; 00187 Cell* cell_; 00189 BinaryHeap<Motion*, MotionCompare>::Element *heapElement_; 00192 bool isSplit_; 00193 }; 00194 00196 struct Cell 00197 { 00198 Cell(double volume, const base::RealVectorBounds &bounds, 00199 unsigned int splitDimension = 0) 00200 : volume_(volume), splitDimension_(splitDimension), splitValue_(0.0), 00201 left_(NULL), right_(NULL), bounds_(bounds) 00202 { 00203 } 00204 00205 ~Cell() 00206 { 00207 if (left_) 00208 { 00209 delete left_; 00210 delete right_; 00211 } 00212 } 00213 00215 void subdivide(unsigned int spaceDimension); 00216 00218 Cell* stab(const base::EuclideanProjection& projection) const 00219 { 00220 Cell *containingCell = const_cast<Cell*>(this); 00221 while (containingCell->left_ != NULL) 00222 { 00223 if (projection[containingCell->splitDimension_] <= containingCell->splitValue_) 00224 containingCell = containingCell->left_; 00225 else 00226 containingCell = containingCell->right_; 00227 } 00228 return containingCell; 00229 } 00231 void addMotion(Motion *motion) 00232 { 00233 motions_.push_back(motion); 00234 motion->cell_ = this; 00235 } 00236 00238 unsigned int size() const 00239 { 00240 unsigned int sz = 1; 00241 if (left_) 00242 sz += left_->size() + right_->size(); 00243 return sz; 00244 } 00245 00247 double volume_; 00249 unsigned int splitDimension_; 00251 double splitValue_; 00253 Cell* left_; 00255 Cell* right_; 00257 base::RealVectorBounds bounds_; 00259 std::vector<Motion*> motions_; 00260 }; 00261 00262 00265 void addMotion(Motion *motion, Cell *cell, 00266 base::State*, base::State*, base::EuclideanProjection&, base::EuclideanProjection&); 00268 void updateHeapElement(Motion *motion) 00269 { 00270 if (motion->heapElement_) 00271 priorityQueue_.update(motion->heapElement_); 00272 else 00273 motion->heapElement_ = priorityQueue_.insert(motion); 00274 } 00278 Motion* propagateFrom(Motion *motion, base::State*, base::State*); 00285 unsigned int findDurationAndAncestor(Motion *motion, base::State *state, 00286 base::State *scratch, Motion*& ancestor) const; 00287 00288 void freeMemory(); 00289 00291 base::StateSamplerPtr sampler_; 00293 DirectedControlSamplerPtr controlSampler_; 00295 const SpaceInformation *siC_; 00296 // Random number generator 00297 RNG rng_; 00300 std::vector<Motion*> startMotions_; 00302 BinaryHeap<Motion*, MotionCompare> priorityQueue_; 00304 Cell* bsp_; 00306 base::ProjectionEvaluatorPtr projectionEvaluator_; 00308 double goalBias_; 00310 base::GoalSampleableRegion *goalSampler_; 00312 unsigned int iteration_; 00314 Motion *lastGoalMotion_; 00315 }; 00316 } 00317 } 00318 00319 #endif