All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/control/src/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/control/SimpleSetup.h"
00038 #include "ompl/control/planners/rrt/RRT.h"
00039 #include "ompl/control/planners/kpiece/KPIECE1.h"
00040 
00041 ompl::base::PlannerPtr ompl::control::getDefaultPlanner(const base::GoalPtr &goal)
00042 {
00043     base::PlannerPtr planner;
00044     if (!goal)
00045         throw Exception("Unable to allocate default planner for unspecified goal definition");
00046 
00047     SpaceInformationPtr si = boost::static_pointer_cast<SpaceInformation, base::SpaceInformation>(goal->getSpaceInformation());
00048     if (si->getStateSpace()->hasDefaultProjection())
00049         planner = base::PlannerPtr(new KPIECE1(si));
00050     else
00051         planner = base::PlannerPtr(new RRT(si));
00052 
00053     return planner;
00054 }
00055 
00056 ompl::control::SimpleSetup::SimpleSetup(const ControlSpacePtr &space) :
00057     configured_(false), planTime_(0.0), invalid_request_(false)
00058 {
00059     si_.reset(new SpaceInformation(space->getStateSpace(), space));
00060     pdef_.reset(new base::ProblemDefinition(si_));
00061     params_.include(si_->params());
00062 }
00063 
00064 void ompl::control::SimpleSetup::setup(void)
00065 {
00066     if (!configured_ || !si_->isSetup() || !planner_->isSetup())
00067     {
00068         if (!si_->isSetup())
00069             si_->setup();
00070         if (!planner_)
00071         {
00072             if (pa_)
00073                 planner_ = pa_(si_);
00074             if (!planner_)
00075             {
00076                 logInform("No planner specified. Using default.");
00077                 planner_ = getDefaultPlanner(getGoal());
00078             }
00079         }
00080         planner_->setProblemDefinition(pdef_);
00081         if (!planner_->isSetup())
00082             planner_->setup();
00083 
00084         params_.clear();
00085         params_.include(si_->params());
00086         params_.include(planner_->params());
00087         configured_ = true;
00088     }
00089 }
00090 
00091 void ompl::control::SimpleSetup::clear(void)
00092 {
00093     if (planner_)
00094         planner_->clear();
00095     if (pdef_)
00096         pdef_->clearSolutionPaths();
00097 }
00098 
00099 ompl::base::PlannerStatus ompl::control::SimpleSetup::solve(double time)
00100 {
00101     setup();
00102     invalid_request_ = false;
00103     time::point start = time::now();
00104     base::PlannerStatus result = planner_->solve(time);
00105     planTime_ = time::seconds(time::now() - start);
00106     if (result)
00107         logInform("Solution found in %f seconds", planTime_);
00108     else
00109     {
00110         if (planTime_ < time)
00111             invalid_request_ = true;
00112         logInform("No solution found after %f seconds", planTime_);
00113     }
00114     return result;
00115 }
00116 
00117 ompl::base::PlannerStatus ompl::control::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
00118 {
00119     setup();
00120     invalid_request_ = false;
00121     time::point start = time::now();
00122     base::PlannerStatus result = planner_->solve(ptc);
00123     planTime_ = time::seconds(time::now() - start);
00124     if (result)
00125         logInform("Solution found in %f seconds", planTime_);
00126     else
00127     {
00128         if (!ptc())
00129             invalid_request_ = true;
00130         logInform("No solution found after %f seconds", planTime_);
00131     }
00132     return result;
00133 }
00134 
00135 ompl::control::PathControl& ompl::control::SimpleSetup::getSolutionPath(void) const
00136 {
00137     if (pdef_)
00138     {
00139         const base::PathPtr &p = pdef_->getSolutionPath();
00140         if (p)
00141             return static_cast<PathControl&>(*p);
00142     }
00143     throw Exception("No solution path");
00144 }
00145 
00146 bool ompl::control::SimpleSetup::haveExactSolutionPath(void) const
00147 {
00148     return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
00149 }
00150 
00151 void ompl::control::SimpleSetup::getPlannerData(base::PlannerData &pd) const
00152 {
00153     pd.clear();
00154     if (planner_)
00155         planner_->getPlannerData(pd);
00156 }
00157 
00158 void ompl::control::SimpleSetup::print(std::ostream &out) const
00159 {
00160     if (si_)
00161     {
00162         si_->printProperties(out);
00163         si_->printSettings(out);
00164     }
00165     if (planner_)
00166     {
00167         planner_->printProperties(out);
00168         planner_->printSettings(out);
00169     }
00170     if (pdef_)
00171         pdef_->print(out);
00172 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines