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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines