ompl/geometric/planners/fmt/FMT.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Autonomous Systems Laboratory, Stanford 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 Stanford 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 /* Authors: Ashley Clark (Stanford) and Wolfgang Pointner (AIT) */ 00036 /* Co-developers: Brice Rebsamen (Stanford) and Tim Wheeler (Stanford) */ 00037 /* Algorithm design: Lucas Janson (Stanford) and Marco Pavone (Stanford) */ 00038 /* Acknowledgements for insightful comments: Edward Schmerling (Stanford), 00039 * Oren Salzman (Tel Aviv University), Joseph Starek (Stanford), and Evan Clark (Stanford) */ 00040 00041 #ifndef OMPL_GEOMETRIC_PLANNERS_FMT_ 00042 #define OMPL_GEOMETRIC_PLANNERS_FMT_ 00043 00044 #include <ompl/geometric/planners/PlannerIncludes.h> 00045 #include <ompl/base/goals/GoalSampleableRegion.h> 00046 #include <ompl/datastructures/NearestNeighbors.h> 00047 #include <ompl/datastructures/BinaryHeap.h> 00048 #include <ompl/base/OptimizationObjective.h> 00049 #include <map> 00050 00051 00052 namespace ompl 00053 { 00054 00055 namespace geometric 00056 { 00057 00077 class FMT : public ompl::base::Planner 00078 { 00079 public: 00080 00081 FMT(const base::SpaceInformationPtr &si); 00082 00083 virtual ~FMT(); 00084 00085 virtual void setup(); 00086 00087 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00088 00089 virtual void clear(); 00090 00091 virtual void getPlannerData(base::PlannerData &data) const; 00092 00098 void setNumSamples(const unsigned int numSamples) 00099 { 00100 numSamples_ = numSamples; 00101 } 00102 00104 unsigned int getNumSamples() const 00105 { 00106 return numSamples_; 00107 } 00108 00119 void setRadiusMultiplier(const double radiusMultiplier) 00120 { 00121 if (radiusMultiplier <= 0.0) 00122 throw Exception("Radius multiplier must be greater than zero"); 00123 radiusMultiplier_ = radiusMultiplier; 00124 } 00125 00128 double getRadiusMultiplier() const 00129 { 00130 return radiusMultiplier_; 00131 } 00132 00136 void setFreeSpaceVolume(const double freeSpaceVolume) 00137 { 00138 if (freeSpaceVolume < 0.0) 00139 throw Exception("Free space volume should be greater than zero"); 00140 freeSpaceVolume_ = freeSpaceVolume; 00141 } 00142 00145 double getFreeSpaceVolume() const 00146 { 00147 return freeSpaceVolume_; 00148 } 00149 00150 protected: 00153 class Motion 00154 { 00155 public: 00156 00164 enum SetType { SET_NULL, SET_H, SET_W }; 00165 00166 Motion() 00167 : state_(NULL), parent_(NULL), cost_(0.0), currentSet_(SET_NULL) 00168 { 00169 } 00170 00172 Motion(const base::SpaceInformationPtr &si) 00173 : state_(si->allocState()), parent_(NULL), cost_(0.0), currentSet_(SET_NULL) 00174 { 00175 } 00176 00177 ~Motion() 00178 { 00179 } 00180 00182 void setState(base::State *state) 00183 { 00184 state_ = state; 00185 } 00186 00188 base::State* getState() const 00189 { 00190 return state_; 00191 } 00192 00194 void setParent(Motion *parent) 00195 { 00196 parent_ = parent; 00197 } 00198 00200 Motion* getParent() const 00201 { 00202 return parent_; 00203 } 00204 00206 void setCost(const base::Cost cost) 00207 { 00208 cost_ = cost; 00209 } 00210 00212 base::Cost getCost() const 00213 { 00214 return cost_; 00215 } 00216 00218 void setSetType(const SetType currentSet) 00219 { 00220 currentSet_ = currentSet; 00221 } 00222 00224 SetType getSetType() const 00225 { 00226 return currentSet_; 00227 } 00228 00229 protected: 00230 00232 base::State *state_; 00233 00235 Motion *parent_; 00236 00238 base::Cost cost_; 00239 00241 SetType currentSet_; 00242 }; 00243 00245 struct MotionCompare 00246 { 00247 MotionCompare() : opt_(NULL) 00248 { 00249 } 00250 00251 /* Returns true if m1 is lower cost than m2. m1 and m2 must 00252 have been instantiated with the same optimization objective */ 00253 bool operator()(const Motion *m1, const Motion *m2) const 00254 { 00255 return opt_->isCostBetterThan(m1->getCost(), m2->getCost()); 00256 } 00257 00258 base::OptimizationObjective* opt_; 00259 }; 00260 00265 double distanceFunction(const Motion *a, const Motion *b) const 00266 { 00267 return opt_->motionCost(a->getState(), b->getState()).value(); 00268 } 00269 00271 void freeMemory(); 00272 00275 void sampleFree(const ompl::base::PlannerTerminationCondition &ptc); 00276 00283 void assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal); 00284 00286 double calculateUnitBallVolume(const unsigned int dimension) const; 00287 00297 double calculateRadius(unsigned int dimension, unsigned int n) const; 00298 00300 void saveNeighborhood(Motion *m, const double r); 00301 00305 void traceSolutionPathThroughTree(Motion *goalMotion); 00306 00313 bool expandTreeFromNode(Motion *&z, const double r); 00314 00317 typedef ompl::BinaryHeap<Motion*, MotionCompare> MotionBinHeap; 00318 00323 MotionBinHeap H_; 00324 00327 std::map<Motion*, MotionBinHeap::Element*> hElements_; 00328 00331 std::map<Motion*, std::vector<Motion*> > neighborhoods_; 00332 00334 unsigned int numSamples_; 00335 00337 double freeSpaceVolume_; 00338 00351 double radiusMultiplier_; 00352 00354 boost::shared_ptr< NearestNeighbors<Motion*> > nn_; 00355 00357 base::StateSamplerPtr sampler_; 00358 00360 base::OptimizationObjectivePtr opt_; 00361 00363 Motion *lastGoalMotion_; 00364 00365 }; 00366 } 00367 } 00368 00369 00370 #endif // OMPL_GEOMETRIC_PLANNERS_FMT_