ompl/tools/config/src/SelfConfig.cpp
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 /* Author: Ioan Sucan */ 00036 00037 #include "ompl/tools/config/SelfConfig.h" 00038 #include "ompl/tools/config/MagicConstants.h" 00039 #include "ompl/geometric/planners/rrt/RRTConnect.h" 00040 #include "ompl/geometric/planners/rrt/RRT.h" 00041 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h" 00042 #include "ompl/geometric/planners/kpiece/KPIECE1.h" 00043 #include "ompl/control/planners/rrt/RRT.h" 00044 #include "ompl/control/planners/kpiece/KPIECE1.h" 00045 #include "ompl/util/Console.h" 00046 #include <boost/thread/mutex.hpp> 00047 #include <boost/shared_ptr.hpp> 00048 #include <boost/weak_ptr.hpp> 00049 #include <algorithm> 00050 #include <limits> 00051 #include <cmath> 00052 #include <map> 00053 00055 namespace ompl 00056 { 00057 namespace tools 00058 { 00059 00060 class SelfConfig::SelfConfigImpl 00061 { 00062 friend class SelfConfig; 00063 00064 public: 00065 00066 SelfConfigImpl(const base::SpaceInformationPtr &si) : 00067 wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0) 00068 { 00069 } 00070 00071 double getProbabilityOfValidState() 00072 { 00073 base::SpaceInformationPtr si = wsi_.lock(); 00074 checkSetup(si); 00075 if (si && probabilityOfValidState_ < 0.0) 00076 probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT); 00077 return probabilityOfValidState_; 00078 } 00079 00080 double getAverageValidMotionLength() 00081 { 00082 base::SpaceInformationPtr si = wsi_.lock(); 00083 checkSetup(si); 00084 if (si && averageValidMotionLength_ < 0.0) 00085 averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT); 00086 return averageValidMotionLength_; 00087 } 00088 00089 void configureValidStateSamplingAttempts(unsigned int &attempts) 00090 { 00091 if (attempts == 0) 00092 attempts = magic::MAX_VALID_SAMPLE_ATTEMPTS; 00093 } 00094 00095 void configurePlannerRange(double &range, const std::string &context) 00096 { 00097 if (range < std::numeric_limits<double>::epsilon()) 00098 { 00099 base::SpaceInformationPtr si = wsi_.lock(); 00100 if (si) 00101 { 00102 range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION; 00103 OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range); 00104 } 00105 else 00106 OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str()); 00107 } 00108 } 00109 00110 void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context) 00111 { 00112 base::SpaceInformationPtr si = wsi_.lock(); 00113 checkSetup(si); 00114 if (!proj && si) 00115 { 00116 OMPL_INFORM("%sAttempting to use default projection.", context.c_str()); 00117 proj = si->getStateSpace()->getDefaultProjection(); 00118 } 00119 if (!proj) 00120 throw Exception("No projection evaluator specified"); 00121 proj->setup(); 00122 } 00123 00124 void print(std::ostream &out) const 00125 { 00126 base::SpaceInformationPtr si = wsi_.lock(); 00127 if (si) 00128 { 00129 out << "Configuration parameters for space '" << si->getStateSpace()->getName() << "'" << std::endl; 00130 out << " - probability of a valid state is " << probabilityOfValidState_ << std::endl; 00131 out << " - average length of a valid motion is " << averageValidMotionLength_ << std::endl; 00132 } 00133 else 00134 out << "EXPIRED" << std::endl; 00135 } 00136 00137 bool expired() const 00138 { 00139 return wsi_.expired(); 00140 } 00141 00142 private: 00143 00144 void checkSetup(const base::SpaceInformationPtr &si) 00145 { 00146 if (si) 00147 { 00148 if (!si->isSetup()) 00149 { 00150 si->setup(); 00151 probabilityOfValidState_ = -1.0; 00152 averageValidMotionLength_ = -1.0; 00153 } 00154 } 00155 else 00156 { 00157 probabilityOfValidState_ = -1.0; 00158 averageValidMotionLength_ = -1.0; 00159 } 00160 } 00161 00162 // we store weak pointers so that the SpaceInformation instances are not kept in 00163 // memory until termination of the program due to the use of a static ConfigMap below 00164 boost::weak_ptr<base::SpaceInformation> wsi_; 00165 00166 double probabilityOfValidState_; 00167 double averageValidMotionLength_; 00168 00169 boost::mutex lock_; 00170 }; 00171 00172 } 00173 } 00174 00176 00177 ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) : 00178 context_(context.empty() ? "" : context + ": ") 00179 { 00180 typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap; 00181 00182 static ConfigMap SMAP; 00183 static boost::mutex LOCK; 00184 00185 boost::mutex::scoped_lock smLock(LOCK); 00186 00187 // clean expired entries from the map 00188 ConfigMap::iterator dit = SMAP.begin(); 00189 while (dit != SMAP.end()) 00190 { 00191 if (dit->second->expired()) 00192 SMAP.erase(dit++); 00193 else 00194 ++dit; 00195 } 00196 00197 ConfigMap::const_iterator it = SMAP.find(si.get()); 00198 00199 if (it != SMAP.end()) 00200 impl_ = it->second.get(); 00201 else 00202 { 00203 impl_ = new SelfConfigImpl(si); 00204 SMAP[si.get()].reset(impl_); 00205 } 00206 } 00207 00208 ompl::tools::SelfConfig::~SelfConfig() 00209 { 00210 } 00211 00212 /* ------------------------------------------------------------------------ */ 00213 00214 double ompl::tools::SelfConfig::getProbabilityOfValidState() 00215 { 00216 boost::mutex::scoped_lock iLock(impl_->lock_); 00217 return impl_->getProbabilityOfValidState(); 00218 } 00219 00220 double ompl::tools::SelfConfig::getAverageValidMotionLength() 00221 { 00222 boost::mutex::scoped_lock iLock(impl_->lock_); 00223 return impl_->getAverageValidMotionLength(); 00224 } 00225 00226 void ompl::tools::SelfConfig::configureValidStateSamplingAttempts(unsigned int &attempts) 00227 { 00228 boost::mutex::scoped_lock iLock(impl_->lock_); 00229 impl_->configureValidStateSamplingAttempts(attempts); 00230 } 00231 00232 void ompl::tools::SelfConfig::configurePlannerRange(double &range) 00233 { 00234 boost::mutex::scoped_lock iLock(impl_->lock_); 00235 impl_->configurePlannerRange(range, context_); 00236 } 00237 00238 void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj) 00239 { 00240 boost::mutex::scoped_lock iLock(impl_->lock_); 00241 return impl_->configureProjectionEvaluator(proj, context_); 00242 } 00243 00244 void ompl::tools::SelfConfig::print(std::ostream &out) const 00245 { 00246 boost::mutex::scoped_lock iLock(impl_->lock_); 00247 impl_->print(out); 00248 } 00249 00250 ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::GoalPtr &goal) 00251 { 00252 base::PlannerPtr planner; 00253 if (!goal) 00254 throw Exception("Unable to allocate default planner for unspecified goal definition"); 00255 00256 base::SpaceInformationPtr si(goal->getSpaceInformation()); 00257 control::SpaceInformationPtr siC(boost::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si)); 00258 if (siC) // kinodynamic planning 00259 { 00260 // if we have a default projection 00261 if (siC->getStateSpace()->hasDefaultProjection()) 00262 planner = base::PlannerPtr(new control::KPIECE1(siC)); 00263 // otherwise use a single-tree planner 00264 else 00265 planner = base::PlannerPtr(new control::RRT(siC)); 00266 } 00267 // if we can sample the goal region, use a bi-directional planner 00268 else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION)) 00269 { 00270 // if we have a default projection 00271 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection()) 00272 planner = base::PlannerPtr(new geometric::LBKPIECE1(goal->getSpaceInformation())); 00273 else 00274 planner = base::PlannerPtr(new geometric::RRTConnect(goal->getSpaceInformation())); 00275 } 00276 // otherwise use a single-tree planner 00277 else 00278 { 00279 // if we have a default projection 00280 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection()) 00281 planner = base::PlannerPtr(new geometric::KPIECE1(goal->getSpaceInformation())); 00282 else 00283 planner = base::PlannerPtr(new geometric::RRT(goal->getSpaceInformation())); 00284 } 00285 00286 if (!planner) 00287 throw Exception("Unable to allocate default planner"); 00288 00289 return planner; 00290 }