All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SimpleSetup.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/geometric/SimpleSetup.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/geometric/planners/rrt/RRTConnect.h"
40 #include "ompl/geometric/planners/rrt/RRT.h"
41 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
43 
45 {
46  base::PlannerPtr planner;
47  if (!goal)
48  throw Exception("Unable to allocate default planner for unspecified goal definition");
49 
50  // if we can sample the goal region, use a bi-directional planner
51  if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
52  {
53  // if we have a default projection
54  if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
55  planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
56  else
57  planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
58  }
59  // other use a single-tree planner
60  else
61  {
62  // if we have a default projection
63  if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
64  planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
65  else
66  planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
67  }
68 
69  if (!planner)
70  throw Exception("Unable to allocate default planner");
71 
72  return planner;
73 }
74 
76  configured_(false), planTime_(0.0), simplifyTime_(0.0), last_status_(base::PlannerStatus::UNKNOWN)
77 {
78  si_.reset(new base::SpaceInformation(space));
79  pdef_.reset(new base::ProblemDefinition(si_));
80  psk_.reset(new PathSimplifier(si_));
81  params_.include(si_->params());
82 }
83 
85 {
86  if (!configured_ || !si_->isSetup() || !planner_->isSetup())
87  {
88  if (!si_->isSetup())
89  si_->setup();
90  if (!planner_)
91  {
92  if (pa_)
93  planner_ = pa_(si_);
94  if (!planner_)
95  {
96  OMPL_INFORM("No planner specified. Using default.");
97  planner_ = getDefaultPlanner(getGoal());
98  }
99  }
100  planner_->setProblemDefinition(pdef_);
101  if (!planner_->isSetup())
102  planner_->setup();
103 
104  params_.clear();
105  params_.include(si_->params());
106  params_.include(planner_->params());
107  configured_ = true;
108  }
109 }
110 
112 {
113  if (planner_)
114  planner_->clear();
115  if (pdef_)
116  pdef_->clearSolutionPaths();
117 }
118 
119 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
121 {
122  setup();
123  last_status_ = base::PlannerStatus::UNKNOWN;
124  time::point start = time::now();
125  last_status_ = planner_->solve(time);
126  planTime_ = time::seconds(time::now() - start);
127  if (last_status_)
128  OMPL_INFORM("Solution found in %f seconds", planTime_);
129  else
130  OMPL_INFORM("No solution found after %f seconds", planTime_);
131  return last_status_;
132 }
133 
135 {
136  setup();
137  last_status_ = base::PlannerStatus::UNKNOWN;
138  time::point start = time::now();
139  last_status_ = planner_->solve(ptc);
140  planTime_ = time::seconds(time::now() - start);
141  if (last_status_)
142  OMPL_INFORM("Solution found in %f seconds", planTime_);
143  else
144  OMPL_INFORM("No solution found after %f seconds", planTime_);
145  return last_status_;
146 }
147 
149 {
150  if (pdef_)
151  {
152  const base::PathPtr &p = pdef_->getSolutionPath();
153  if (p)
154  {
155  time::point start = time::now();
156  psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
157  simplifyTime_ = time::seconds(time::now() - start);
158  OMPL_INFORM("Path simplification took %f seconds", simplifyTime_);
159  return;
160  }
161  }
162  OMPL_WARN("No solution to simplify");
163 }
164 
166 {
167  if (pdef_)
168  {
169  const base::PathPtr &p = pdef_->getSolutionPath();
170  if (p)
171  {
172  time::point start = time::now();
173  if (duration < std::numeric_limits<double>::epsilon())
174  psk_->simplifyMax(static_cast<PathGeometric&>(*p));
175  else
176  psk_->simplify(static_cast<PathGeometric&>(*p), duration);
177  simplifyTime_ = time::seconds(time::now() - start);
178  OMPL_INFORM("Path simplification took %f seconds", simplifyTime_);
179  return;
180  }
181  }
182  OMPL_WARN("No solution to simplify");
183 }
184 
186 {
187  if (pdef_)
188  {
189  const base::PathPtr &p = pdef_->getSolutionPath();
190  if (p)
191  return static_cast<PathGeometric&>(*p);
192  }
193  throw Exception("No solution path");
194 }
195 
197 {
198  return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
199 }
200 
202 {
203  pd.clear();
204  if (planner_)
205  planner_->getPlannerData(pd);
206 }
207 
208 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
209 {
210  if (si_)
211  {
212  si_->printProperties(out);
213  si_->printSettings(out);
214  }
215  if (planner_)
216  {
217  planner_->printProperties(out);
218  planner_->printSettings(out);
219  }
220  if (pdef_)
221  pdef_->print(out);
222 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
void include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
A boost shared pointer wrapper for ompl::base::StateSpace.
base::ParamSet params_
The parameters that describe the planning context.
Definition: SimpleSetup.h:305
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:278
PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: SimpleSetup.h:290
bool haveExactSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
A boost shared pointer wrapper for ompl::base::Planner.
Kinematic Planning by Interior-Exterior Cell Exploration.
Definition: KPIECE1.h:74
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
RRT-Connect (RRTConnect)
Definition: RRTConnect.h:61
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:281
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Rapidly-exploring Random Trees.
Definition: RRT.h:65
This class contains routines that attempt to simplify geometric paths.
The base class for space information. This contains all the information about the space planning is d...
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:84
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Lazy Bi-directional KPIECE with one level of discretization.
Definition: LBKPIECE1.h:78
The exception type for ompl.
Definition: Exception.h:47
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
PathGeometric & getSolutionPath(void) const
Get the solution path. Throw an exception if no solution is available.
virtual void clear(void)
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
A boost shared pointer wrapper for ompl::base::Goal.
virtual void clear(void)
Clears the entire data structure.
Definition: PlannerData.cpp:78
Definition of a geometric path.
Definition: PathGeometric.h:55
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
SimpleSetup(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: SimpleSetup.cpp:75
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:44
point now(void)
Get the current time point.
Definition: Time.h:56
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68