ompl/geometric/planners/rrt/LBTRRT.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Oren Salzman 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 Willow Garage 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: Oren Salzman, Sertac Karaman, Ioan Sucan */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_ 00038 #define OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_ 00039 00040 #include "ompl/geometric/planners/PlannerIncludes.h" 00041 #include "ompl/base/OptimizationObjective.h" 00042 #include "ompl/datastructures/NearestNeighbors.h" 00043 00044 #include <fstream> 00045 00046 namespace ompl 00047 { 00048 00049 namespace geometric 00050 { 00051 00068 class LBTRRT : public base::Planner 00069 { 00070 public: 00071 00073 LBTRRT (const base::SpaceInformationPtr &si); 00074 00075 virtual ~LBTRRT (); 00076 00077 virtual void getPlannerData(base::PlannerData &data) const; 00078 00079 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00080 00081 virtual void clear(); 00082 00092 void setGoalBias(double goalBias) 00093 { 00094 goalBias_ = goalBias; 00095 } 00096 00098 double getGoalBias() const 00099 { 00100 return goalBias_; 00101 } 00102 00108 void setRange(double distance) 00109 { 00110 maxDistance_ = distance; 00111 } 00112 00114 double getRange() const 00115 { 00116 return maxDistance_; 00117 } 00118 00120 template<template<typename T> class NN> 00121 void setNearestNeighbors() 00122 { 00123 nn_.reset(new NN<Motion*>()); 00124 } 00125 00126 virtual void setup(); 00127 00129 void setApproximationFactor (double epsilon) 00130 { 00131 epsilon_ = epsilon; 00132 } 00133 00135 double getApproximationFactor () const 00136 { 00137 return epsilon_; 00138 } 00139 00141 // Planner progress property functions 00142 std::string getIterationCount() const 00143 { 00144 return boost::lexical_cast<std::string>(iterations_); 00145 } 00146 std::string getBestCost() const 00147 { 00148 return boost::lexical_cast<std::string>(bestCost_); 00149 } 00150 00151 protected: 00152 00154 static const double kRRG; // = 5.5 00155 00160 class Motion 00161 { 00162 public: 00163 00164 Motion() : state(NULL), parentLb_(NULL), parentApx_(NULL), costLb_(0.0), costApx_(0.0) 00165 { 00166 } 00167 00169 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parentLb_(NULL), parentApx_(NULL), costLb_(0.0), costApx_(0.0) 00170 { 00171 } 00172 00173 ~Motion() 00174 { 00175 } 00176 00178 base::State *state; 00179 00181 Motion *parentLb_; 00182 00184 Motion *parentApx_; 00185 00187 base::Cost costLb_; 00189 base::Cost costApx_; 00191 base::Cost incCost_; 00192 00193 std::vector<Motion*> childrenLb_; 00194 std::vector<Motion*> childrenApx_; 00195 }; 00196 00197 struct CostCompare 00198 { 00199 CostCompare(const base::OptimizationObjective &opt, Motion *motion_): opt_(opt), motion(motion_) 00200 { 00201 } 00202 00203 bool operator() (const Motion *motionA, const Motion *motionB) 00204 { 00205 base::Cost costA = opt_.combineCosts(motionA->costLb_, opt_.motionCost(motionA->state, motion->state)); 00206 base::Cost costB = opt_.combineCosts(motionB->costLb_, opt_.motionCost(motionB->state, motion->state)); 00207 return opt_.isCostBetterThan(costA, costB); 00208 } 00209 const base::OptimizationObjective &opt_; 00210 Motion *motion; 00211 }; //IsLessThan 00212 00214 bool attemptNodeUpdate(Motion *potentialParent, Motion *child); 00215 00217 void updateChildCostsLb(Motion *m); 00218 00220 void updateChildCostsApx(Motion *m); 00221 00223 void removeFromParentLb(Motion *m); 00224 00226 void removeFromParentApx(Motion *m); 00227 00229 void removeFromParent(const Motion *m, std::vector<Motion*>& vec); 00230 00232 void freeMemory(); 00233 00235 double distanceFunction(const Motion *a, const Motion *b) const 00236 { 00237 return si_->distance(a->state, b->state); 00238 } 00239 /* \brief Compute cost to move from state in motion a to state in motion b */ 00240 base::Cost costFunction(const Motion *a, const Motion *b) const 00241 { 00242 return opt_->motionCost(a->state, b->state); 00243 } 00245 base::StateSamplerPtr sampler_; 00246 00248 boost::shared_ptr< NearestNeighbors<Motion*> > nn_; 00249 00251 double goalBias_; 00252 00254 double maxDistance_; 00255 00257 double epsilon_; 00258 00260 RNG rng_; 00261 00263 base::OptimizationObjectivePtr opt_; 00264 00266 Motion *lastGoalMotion_; 00267 00269 std::vector<Motion*> goalMotions_; 00270 00272 // Planner progress properties 00274 unsigned int iterations_; 00276 base::Cost bestCost_; 00277 00278 }; 00279 00280 } 00281 } 00282 00283 #endif //OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_