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