00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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), msg_("SimpleSetup")
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 msg_.inform("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_ && pdef_->getGoal())
00096 pdef_->getGoal()->clearSolutionPaths();
00097 }
00098
00099 bool ompl::control::SimpleSetup::solve(double time)
00100 {
00101 setup();
00102 invalid_request_ = false;
00103 time::point start = time::now();
00104 bool result = planner_->solve(time);
00105 planTime_ = time::seconds(time::now() - start);
00106 if (result)
00107 msg_.inform("Solution found in %f seconds", planTime_);
00108 else
00109 {
00110 if (planTime_ < time)
00111 invalid_request_ = true;
00112 msg_.inform("No solution found after %f seconds", planTime_);
00113 }
00114 return result;
00115 }
00116
00117 bool ompl::control::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
00118 {
00119 setup();
00120 invalid_request_ = false;
00121 time::point start = time::now();
00122 bool result = planner_->solve(ptc);
00123 planTime_ = time::seconds(time::now() - start);
00124 if (result)
00125 msg_.inform("Solution found in %f seconds", planTime_);
00126 else
00127 {
00128 if (!ptc())
00129 invalid_request_ = true;
00130 msg_.inform("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_ && pdef_->getGoal())
00138 {
00139 const base::PathPtr &p = pdef_->getGoal()->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() && (!getGoal()->isApproximate() || getGoal()->getDifference() < std::numeric_limits<double>::epsilon());
00149 }
00150
00151 ompl::control::PlannerData ompl::control::SimpleSetup::getPlannerData(void) const
00152 {
00153 control::PlannerData pd;
00154 if (planner_)
00155 planner_->getPlannerData(pd);
00156 return pd;
00157 }
00158
00159 void ompl::control::SimpleSetup::print(std::ostream &out) const
00160 {
00161 if (si_)
00162 {
00163 si_->printProperties(out);
00164 si_->printSettings(out);
00165 }
00166 if (planner_)
00167 {
00168 planner_->printProperties(out);
00169 planner_->printSettings(out);
00170 }
00171 if (pdef_)
00172 pdef_->print(out);
00173 }