All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SimpleSetup.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, 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/geometric/SimpleSetup.h"
00038 #include "ompl/base/GoalSampleableRegion.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 
00044 ompl::base::PlannerPtr ompl::geometric::getDefaultPlanner(const base::GoalPtr &goal)
00045 {
00046     base::PlannerPtr planner;
00047     if (!goal)
00048         throw Exception("Unable to allocate default planner for unspecified goal definition");
00049 
00050     // if we can sample the goal region, use a bi-directional planner
00051     if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
00052     {
00053         // if we have a default projection
00054         if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00055             planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
00056         else
00057             planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
00058     }
00059     // other use a single-tree planner
00060     else
00061     {
00062         // if we have a default projection
00063         if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00064             planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
00065         else
00066             planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
00067     }
00068 
00069     if (!planner)
00070         throw Exception("Unable to allocate default planner");
00071 
00072     return planner;
00073 }
00074 
00075 ompl::geometric::SimpleSetup::SimpleSetup(const base::StateSpacePtr &space) :
00076     configured_(false), planTime_(0.0), simplifyTime_(0.0), invalid_request_(false), msg_("SimpleSetup")
00077 {
00078     si_.reset(new base::SpaceInformation(space));
00079     pdef_.reset(new base::ProblemDefinition(si_));
00080     psk_.reset(new PathSimplifier(si_));
00081     params_.include(si_->params());
00082 }
00083 
00084 void ompl::geometric::SimpleSetup::setup(void)
00085 {
00086     if (!configured_ || !si_->isSetup() || !planner_->isSetup())
00087     {
00088         if (!si_->isSetup())
00089             si_->setup();
00090         if (!planner_)
00091         {
00092             if (pa_)
00093                 planner_ = pa_(si_);
00094             if (!planner_)
00095             {
00096                 msg_.inform("No planner specified. Using default.");
00097                 planner_ = getDefaultPlanner(getGoal());
00098             }
00099         }
00100         planner_->setProblemDefinition(pdef_);
00101         if (!planner_->isSetup())
00102             planner_->setup();
00103 
00104         params_.clear();
00105         params_.include(si_->params());
00106         params_.include(planner_->params());
00107         configured_ = true;
00108     }
00109 }
00110 
00111 void ompl::geometric::SimpleSetup::clear(void)
00112 {
00113     if (planner_)
00114         planner_->clear();
00115     if (pdef_ && pdef_->getGoal())
00116         pdef_->getGoal()->clearSolutionPaths();
00117 }
00118 
00119 bool ompl::geometric::SimpleSetup::solve(double time)
00120 {
00121     setup();
00122     invalid_request_ = false;
00123     time::point start = time::now();
00124     bool result = planner_->solve(time);
00125     planTime_ = time::seconds(time::now() - start);
00126     if (result)
00127         msg_.inform("Solution found in %f seconds", planTime_);
00128     else
00129     {
00130         if (planTime_ < time)
00131             invalid_request_ = true;
00132         msg_.inform("No solution found after %f seconds", planTime_);
00133     }
00134     return result;
00135 }
00136 
00137 bool ompl::geometric::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
00138 {
00139     setup();
00140     invalid_request_ = false;
00141     time::point start = time::now();
00142     bool result = planner_->solve(ptc);
00143     planTime_ = time::seconds(time::now() - start);
00144     if (result)
00145         msg_.inform("Solution found in %f seconds", planTime_);
00146     else
00147     {
00148         if (!ptc())
00149             invalid_request_ = true;
00150         msg_.inform("No solution found after %f seconds", planTime_);
00151     }
00152     return result;
00153 }
00154 
00155 void ompl::geometric::SimpleSetup::simplifySolution(const base::PlannerTerminationCondition &ptc)
00156 {
00157     if (pdef_ && pdef_->getGoal())
00158     {
00159         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00160         if (p)
00161         {
00162             time::point start = time::now();
00163             psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
00164             simplifyTime_ = time::seconds(time::now() - start);
00165             msg_.inform("Path simplification took %f seconds", simplifyTime_);
00166             return;
00167         }
00168     }
00169     msg_.warn("No solution to simplify");
00170 }
00171 
00172 void ompl::geometric::SimpleSetup::simplifySolution(double duration)
00173 {
00174     if (pdef_ && pdef_->getGoal())
00175     {
00176         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00177         if (p)
00178         {
00179             time::point start = time::now();
00180             if (duration < std::numeric_limits<double>::epsilon())
00181                 psk_->simplifyMax(static_cast<PathGeometric&>(*p));
00182             else
00183                 psk_->simplify(static_cast<PathGeometric&>(*p), duration);
00184             simplifyTime_ = time::seconds(time::now() - start);
00185             msg_.inform("Path simplification took %f seconds", simplifyTime_);
00186             return;
00187         }
00188     }
00189     msg_.warn("No solution to simplify");
00190 }
00191 
00192 ompl::geometric::PathGeometric& ompl::geometric::SimpleSetup::getSolutionPath(void) const
00193 {
00194     if (pdef_ && pdef_->getGoal())
00195     {
00196         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00197         if (p)
00198             return static_cast<PathGeometric&>(*p);
00199     }
00200     throw Exception("No solution path");
00201 }
00202 
00203 bool ompl::geometric::SimpleSetup::haveExactSolutionPath(void) const
00204 {
00205     return haveSolutionPath() && (!getGoal()->isApproximate() || getGoal()->getDifference() < std::numeric_limits<double>::epsilon());
00206 }
00207 
00208 ompl::base::PlannerData ompl::geometric::SimpleSetup::getPlannerData(void) const
00209 {
00210     base::PlannerData pd;
00211     if (planner_)
00212         planner_->getPlannerData(pd);
00213     return pd;
00214 }
00215 
00216 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
00217 {
00218     if (si_)
00219     {
00220         si_->printProperties(out);
00221         si_->printSettings(out);
00222     }
00223     if (planner_)
00224     {
00225         planner_->printProperties(out);
00226         planner_->printSettings(out);
00227     }
00228     if (pdef_)
00229         pdef_->print(out);
00230 }