ompl/geometric/planners/rrt/RRTstar.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, 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 /* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Luis G. Torres, Ioan Sucan, Javier V Gomez */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_ 00038 #define OMPL_GEOMETRIC_PLANNERS_RRT_RRTSTAR_ 00039 00040 #include "ompl/geometric/planners/PlannerIncludes.h" 00041 #include "ompl/base/OptimizationObjective.h" 00042 #include "ompl/datastructures/NearestNeighbors.h" 00043 00044 #include <limits> 00045 #include <vector> 00046 #include <utility> 00047 00048 00049 namespace ompl 00050 { 00051 00052 namespace geometric 00053 { 00054 00076 class RRTstar : public base::Planner 00077 { 00078 public: 00079 00080 RRTstar(const base::SpaceInformationPtr &si); 00081 00082 virtual ~RRTstar(); 00083 00084 virtual void getPlannerData(base::PlannerData &data) const; 00085 00086 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00087 00088 virtual void clear(); 00089 00099 void setGoalBias(double goalBias) 00100 { 00101 goalBias_ = goalBias; 00102 } 00103 00105 double getGoalBias() const 00106 { 00107 return goalBias_; 00108 } 00109 00115 void setRange(double distance) 00116 { 00117 maxDistance_ = distance; 00118 } 00119 00121 double getRange() const 00122 { 00123 return maxDistance_; 00124 } 00125 00127 template<template<typename T> class NN> 00128 void setNearestNeighbors() 00129 { 00130 nn_.reset(new NN<Motion*>()); 00131 } 00132 00140 void setDelayCC(bool delayCC) 00141 { 00142 delayCC_ = delayCC; 00143 } 00144 00146 bool getDelayCC() const 00147 { 00148 return delayCC_; 00149 } 00150 00152 void setPrune(const bool prune) 00153 { 00154 prune_ = prune; 00155 } 00156 00158 bool getPrune() const 00159 { 00160 return prune_; 00161 } 00162 00165 void setPruneStatesImprovementThreshold(const double pp) 00166 { 00167 pruneStatesThreshold_ = pp; 00168 } 00169 00171 double getPruneStatesImprovementThreshold () const 00172 { 00173 return pruneStatesThreshold_; 00174 } 00175 00176 virtual void setup(); 00177 00179 // Planner progress property functions 00180 std::string getIterationCount() const 00181 { 00182 return boost::lexical_cast<std::string>(iterations_); 00183 } 00184 std::string getBestCost() const 00185 { 00186 return boost::lexical_cast<std::string>(bestCost_); 00187 } 00188 00189 protected: 00190 00192 class Motion 00193 { 00194 public: 00196 Motion(const base::SpaceInformationPtr &si) : 00197 state(si->allocState()), 00198 parent(NULL) 00199 { 00200 } 00201 00202 ~Motion() 00203 { 00204 } 00205 00207 base::State *state; 00208 00210 Motion *parent; 00211 00213 base::Cost cost; 00214 00216 base::Cost incCost; 00217 00219 std::vector<Motion*> children; 00220 }; 00221 00223 void freeMemory(); 00224 00225 // For sorting a list of costs and getting only their sorted indices 00226 struct CostIndexCompare 00227 { 00228 CostIndexCompare(const std::vector<base::Cost>& costs, 00229 const base::OptimizationObjective &opt) : 00230 costs_(costs), opt_(opt) 00231 {} 00232 bool operator()(unsigned i, unsigned j) 00233 { 00234 return opt_.isCostBetterThan(costs_[i],costs_[j]); 00235 } 00236 const std::vector<base::Cost>& costs_; 00237 const base::OptimizationObjective &opt_; 00238 }; 00239 00241 double distanceFunction(const Motion *a, const Motion *b) const 00242 { 00243 return si_->distance(a->state, b->state); 00244 } 00245 00247 void removeFromParent(Motion *m); 00248 00250 void updateChildCosts(Motion *m); 00251 00254 int pruneTree(const base::Cost pruneTreeCost); 00255 00257 void deleteBranch(Motion *motion); 00258 00262 base::Cost costToGo(const Motion *motion, const bool shortest = true) const; 00263 00265 base::StateSamplerPtr sampler_; 00266 00268 boost::shared_ptr< NearestNeighbors<Motion*> > nn_; 00269 00271 double goalBias_; 00272 00274 double maxDistance_; 00275 00277 RNG rng_; 00278 00280 bool delayCC_; 00281 00283 base::OptimizationObjectivePtr opt_; 00284 00286 Motion *lastGoalMotion_; 00287 00289 std::vector<Motion*> goalMotions_; 00290 00292 bool prune_; 00293 00295 double pruneStatesThreshold_; 00296 00297 struct PruneScratchSpace { std::vector<Motion*> newTree, toBePruned, candidates; } pruneScratchSpace_; 00298 00300 Motion * startMotion_; 00301 00303 // Planner progress properties 00305 unsigned int iterations_; 00307 base::Cost bestCost_; 00308 }; 00309 } 00310 } 00311 00312 #endif