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