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/geometric/SimpleSetup.h"
00038 #include "ompl/base/goals/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
00051 if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
00052 {
00053
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
00060 else
00061 {
00062
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)
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 logInform("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_)
00116 pdef_->clearSolutionPaths();
00117 }
00118
00119 ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(double time)
00120 {
00121 setup();
00122 invalid_request_ = false;
00123 time::point start = time::now();
00124 base::PlannerStatus result = planner_->solve(time);
00125 planTime_ = time::seconds(time::now() - start);
00126 if (result)
00127 logInform("Solution found in %f seconds", planTime_);
00128 else
00129 {
00130 if (planTime_ < time)
00131 invalid_request_ = true;
00132 logInform("No solution found after %f seconds", planTime_);
00133 }
00134 return result;
00135 }
00136
00137 ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
00138 {
00139 setup();
00140 invalid_request_ = false;
00141 time::point start = time::now();
00142 base::PlannerStatus result = planner_->solve(ptc);
00143 planTime_ = time::seconds(time::now() - start);
00144 if (result)
00145 logInform("Solution found in %f seconds", planTime_);
00146 else
00147 {
00148 if (!ptc())
00149 invalid_request_ = true;
00150 logInform("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_)
00158 {
00159 const base::PathPtr &p = pdef_->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 logInform("Path simplification took %f seconds", simplifyTime_);
00166 return;
00167 }
00168 }
00169 logWarn("No solution to simplify");
00170 }
00171
00172 void ompl::geometric::SimpleSetup::simplifySolution(double duration)
00173 {
00174 if (pdef_)
00175 {
00176 const base::PathPtr &p = pdef_->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 logInform("Path simplification took %f seconds", simplifyTime_);
00186 return;
00187 }
00188 }
00189 logWarn("No solution to simplify");
00190 }
00191
00192 ompl::geometric::PathGeometric& ompl::geometric::SimpleSetup::getSolutionPath(void) const
00193 {
00194 if (pdef_)
00195 {
00196 const base::PathPtr &p = pdef_->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() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
00206 }
00207
00208 void ompl::geometric::SimpleSetup::getPlannerData(base::PlannerData &pd) const
00209 {
00210 pd.clear();
00211 if (planner_)
00212 planner_->getPlannerData(pd);
00213 }
00214
00215 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
00216 {
00217 if (si_)
00218 {
00219 si_->printProperties(out);
00220 si_->printSettings(out);
00221 }
00222 if (planner_)
00223 {
00224 planner_->printProperties(out);
00225 planner_->printSettings(out);
00226 }
00227 if (pdef_)
00228 pdef_->print(out);
00229 }