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 }