ompl/geometric/planners/AnytimePathShortening.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, 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: Ryan Luna */
00036 
00037 #include "ompl/geometric/planners/AnytimePathShortening.h"
00038 #include "ompl/geometric/PathHybridization.h"
00039 #include "ompl/geometric/PathSimplifier.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00042 
00043 #include <boost/thread.hpp>
00044 
00045 ompl::geometric::AnytimePathShortening::AnytimePathShortening (const ompl::base::SpaceInformationPtr &si) :
00046     ompl::base::Planner(si, "APS"),
00047     shortcut_(true),
00048     hybridize_(true),
00049     maxHybridPaths_(24),
00050     defaultNumPlanners_(std::max(1u, boost::thread::hardware_concurrency()))
00051 {
00052     specs_.approximateSolutions = true;
00053     specs_.multithreaded = true;
00054     specs_.optimizingPaths = true;
00055 
00056     Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut, &AnytimePathShortening::isShortcutting, "0,1");
00057     Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize, &AnytimePathShortening::isHybridizing, "0,1");
00058     Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath, &AnytimePathShortening::maxHybridizationPaths, "0:1:50");
00059     Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners, &AnytimePathShortening::getDefaultNumPlanners, "0:64");
00060 
00061     addPlannerProgressProperty("best cost REAL",
00062                                boost::bind(&AnytimePathShortening::getBestCost, this));
00063 }
00064 
00065 ompl::geometric::AnytimePathShortening::~AnytimePathShortening()
00066 {
00067 }
00068 
00069 void ompl::geometric::AnytimePathShortening::addPlanner(base::PlannerPtr &planner)
00070 {
00071     if (planner && planner->getSpaceInformation().get() != si_.get())
00072     {
00073         OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
00074         return;
00075     }
00076 
00077     // Ensure all planners are unique instances
00078     for(size_t i = 0; i < planners_.size(); ++i)
00079     {
00080         if (planner.get() == planners_[i].get())
00081         {
00082             OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
00083             return;
00084         }
00085     }
00086 
00087     planners_.push_back(planner);
00088 }
00089 
00090 void ompl::geometric::AnytimePathShortening::setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef)
00091 {
00092     ompl::base::Planner::setProblemDefinition(pdef);
00093     for (size_t i = 0; i < planners_.size(); ++i)
00094         planners_[i]->setProblemDefinition(pdef);
00095 }
00096 
00097 ompl::base::PlannerStatus ompl::geometric::AnytimePathShortening::solve(const ompl::base::PlannerTerminationCondition &ptc)
00098 {
00099     base::Goal *goal = pdef_->getGoal().get();
00100     std::vector<boost::thread*> threads(planners_.size());
00101     geometric::PathHybridization phybrid(si_);
00102     base::Path *bestSln = NULL;
00103 
00104     base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
00105     if (!opt)
00106     {
00107         OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
00108         opt.reset(new base::PathLengthOptimizationObjective(si_));
00109         pdef_->setOptimizationObjective(opt);
00110     }
00111     else
00112     {
00113         if (!dynamic_cast<base::PathLengthOptimizationObjective*>(opt.get()))
00114             OMPL_WARN("The optimization objective is not set for path length.  The specified optimization criteria may not be optimized over.");
00115     }
00116 
00117     // Disable output from the motion planners, except for errors
00118     msg::LogLevel currentLogLevel = msg::getLogLevel();
00119     msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
00120     while (!ptc())
00121     {
00122         // We have found a solution that is good enough
00123         if (bestSln && opt->isSatisfied(base::Cost(bestSln->length())))
00124             break;
00125 
00126         // Clear any previous planning data for the set of planners
00127         clear();
00128 
00129         // Spawn a thread for each planner.  This will shortcut the best path after solving.
00130         for (size_t i = 0; i < threads.size(); ++i)
00131             threads[i] = new boost::thread(boost::bind(&AnytimePathShortening::threadSolve, this, planners_[i].get(), ptc));
00132 
00133         // Join each thread, and then delete it
00134         for (std::size_t i = 0 ; i < threads.size() ; ++i)
00135         {
00136             threads[i]->join();
00137             delete threads[i];
00138         }
00139 
00140         // Hybridize the set of paths computed.  Add the new hybrid path to the mix.
00141         if (hybridize_ && !ptc())
00142         {
00143             const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
00144             for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
00145                 phybrid.recordPath(paths[j].path_, false);
00146 
00147             phybrid.computeHybridPath();
00148             const base::PathPtr &hsol = phybrid.getHybridPath();
00149             if (hsol)
00150             {
00151                 geometric::PathGeometric *pg = static_cast<geometric::PathGeometric*>(hsol.get());
00152                 double difference = 0.0;
00153                 bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
00154                 pdef_->addSolutionPath(hsol, approximate, difference);
00155             }
00156         }
00157 
00158         // keep track of the best solution found so far
00159         if (pdef_->getSolutionCount() > 0)
00160             bestSln = pdef_->getSolutionPath().get();
00161     }
00162     msg::setLogLevel(currentLogLevel);
00163 
00164     if (bestSln)
00165     {
00166         if (goal->isSatisfied (static_cast<geometric::PathGeometric*>(bestSln)->getStates().back()))
00167             return base::PlannerStatus::EXACT_SOLUTION;
00168         return base::PlannerStatus::APPROXIMATE_SOLUTION;
00169     }
00170     return base::PlannerStatus::UNKNOWN;
00171 }
00172 
00173 void ompl::geometric::AnytimePathShortening::threadSolve(base::Planner* planner, const base::PlannerTerminationCondition &ptc)
00174 {
00175     // compute a motion plan
00176     base::PlannerStatus status = planner->solve(ptc);
00177 
00178     // Shortcut the best solution found so far
00179     if (shortcut_ && status == base::PlannerStatus::EXACT_SOLUTION)
00180     {
00181         geometric::PathGeometric* sln = static_cast<geometric::PathGeometric*>(pdef_->getSolutionPath().get());
00182         geometric::PathGeometric* pathCopy = new geometric::PathGeometric(*sln);
00183         geometric::PathSimplifier ps(pdef_->getSpaceInformation());
00184         if (ps.shortcutPath(*pathCopy))
00185         {
00186             double difference = 0.0;
00187             bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
00188             pdef_->addSolutionPath(base::PathPtr(pathCopy), approximate, difference);
00189         }
00190         else delete pathCopy;
00191     }
00192 }
00193 
00194 void ompl::geometric::AnytimePathShortening::clear(void)
00195 {
00196     Planner::clear();
00197     for (size_t i = 0; i < planners_.size(); ++i)
00198         planners_[i]->clear();
00199 }
00200 
00201 void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data) const
00202 {
00203     if (planners_.size() == 0)
00204         return;
00205 
00206     OMPL_WARN("Returning planner data for planner #0");
00207     getPlannerData(data, 0);
00208 }
00209 
00210 void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const
00211 {
00212     if(planners_.size() < idx)
00213         return;
00214     planners_[idx]->getPlannerData(data);
00215 }
00216 
00217 void ompl::geometric::AnytimePathShortening::setup(void)
00218 {
00219     Planner::setup();
00220 
00221     if (planners_.size() == 0)
00222     {
00223         planners_.reserve(defaultNumPlanners_);
00224         for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
00225         {
00226             planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
00227             planners_.back()->setProblemDefinition(pdef_);
00228         }
00229         OMPL_INFORM("%s: No planners specified; using %u instances of %s",
00230             getName().c_str(), planners_.size(), planners_[0]->getName().c_str());
00231     }
00232 
00233     for (size_t i = 0; i < planners_.size(); ++i)
00234         planners_[i]->setup();
00235 }
00236 
00237 void ompl::geometric::AnytimePathShortening::checkValidity(void)
00238 {
00239     for (size_t i = 0; i < planners_.size(); ++i)
00240         planners_[i]->checkValidity();
00241 }
00242 
00243 unsigned int ompl::geometric::AnytimePathShortening::getNumPlanners(void) const
00244 {
00245     return planners_.size();
00246 }
00247 
00248 ompl::base::PlannerPtr ompl::geometric::AnytimePathShortening::getPlanner(unsigned int idx) const
00249 {
00250     assert(idx < planners_.size());
00251     return planners_[idx];
00252 }
00253 
00254 bool ompl::geometric::AnytimePathShortening::isShortcutting(void) const
00255 {
00256     return shortcut_;
00257 }
00258 
00259 void ompl::geometric::AnytimePathShortening::setShortcut(bool shortcut)
00260 {
00261     shortcut_ = shortcut;
00262 }
00263 
00264 bool ompl::geometric::AnytimePathShortening::isHybridizing(void) const
00265 {
00266     return hybridize_;
00267 }
00268 
00269 void ompl::geometric::AnytimePathShortening::setHybridize(bool hybridize)
00270 {
00271     hybridize_ = hybridize;
00272 }
00273 
00274 unsigned int ompl::geometric::AnytimePathShortening::maxHybridizationPaths(void) const
00275 {
00276     return maxHybridPaths_;
00277 }
00278 
00279 void ompl::geometric::AnytimePathShortening::setMaxHybridizationPath(unsigned int maxPathCount)
00280 {
00281     maxHybridPaths_ = maxPathCount;
00282 }
00283 
00284 void ompl::geometric::AnytimePathShortening::setDefaultNumPlanners(unsigned int numPlanners)
00285 {
00286     defaultNumPlanners_ = numPlanners;
00287 }
00288 
00289 unsigned int ompl::geometric::AnytimePathShortening::getDefaultNumPlanners() const
00290 {
00291     return defaultNumPlanners_;
00292 }
00293 
00294 std::string ompl::geometric::AnytimePathShortening::getBestCost() const
00295 {
00296     base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
00297     if (pdef_ && pdef_->getSolutionCount() > 0)
00298         bestCost = base::Cost(pdef_->getSolutionPath()->length());
00299     return boost::lexical_cast<std::string>(bestCost);
00300 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines