RRT.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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/control/planners/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
43 {
45  siC_ = si.get();
46  addIntermediateStates_ = false;
47  lastGoalMotion_ = NULL;
48 
49  goalBias_ = 0.05;
50 
51  Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1.");
52  Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates);
53 }
54 
55 ompl::control::RRT::~RRT()
56 {
57  freeMemory();
58 }
59 
61 {
63  if (!nn_)
64  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
65  nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
66 }
67 
69 {
70  Planner::clear();
71  sampler_.reset();
72  controlSampler_.reset();
73  freeMemory();
74  if (nn_)
75  nn_->clear();
76  lastGoalMotion_ = NULL;
77 }
78 
80 {
81  if (nn_)
82  {
83  std::vector<Motion*> motions;
84  nn_->list(motions);
85  for (unsigned int i = 0 ; i < motions.size() ; ++i)
86  {
87  if (motions[i]->state)
88  si_->freeState(motions[i]->state);
89  if (motions[i]->control)
90  siC_->freeControl(motions[i]->control);
91  delete motions[i];
92  }
93  }
94 }
95 
97 {
98  checkValidity();
99  base::Goal *goal = pdef_->getGoal().get();
100  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
101 
102  while (const base::State *st = pis_.nextStart())
103  {
104  Motion *motion = new Motion(siC_);
105  si_->copyState(motion->state, st);
106  siC_->nullControl(motion->control);
107  nn_->add(motion);
108  }
109 
110  if (nn_->size() == 0)
111  {
112  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
114  }
115 
116  if (!sampler_)
117  sampler_ = si_->allocStateSampler();
118  if (!controlSampler_)
119  controlSampler_ = siC_->allocDirectedControlSampler();
120 
121  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
122 
123  Motion *solution = NULL;
124  Motion *approxsol = NULL;
125  double approxdif = std::numeric_limits<double>::infinity();
126 
127  Motion *rmotion = new Motion(siC_);
128  base::State *rstate = rmotion->state;
129  Control *rctrl = rmotion->control;
130  base::State *xstate = si_->allocState();
131 
132  while (ptc == false)
133  {
134  /* sample random state (with goal biasing) */
135  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
136  goal_s->sampleGoal(rstate);
137  else
138  sampler_->sampleUniform(rstate);
139 
140  /* find closest state in the tree */
141  Motion *nmotion = nn_->nearest(rmotion);
142 
143  /* sample a random control that attempts to go towards the random state, and also sample a control duration */
144  unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state);
145 
146  if (addIntermediateStates_)
147  {
148  // this code is contributed by Jennifer Barry
149  std::vector<base::State *> pstates;
150  cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true);
151 
152  if (cd >= siC_->getMinControlDuration())
153  {
154  Motion *lastmotion = nmotion;
155  bool solved = false;
156  size_t p = 0;
157  for ( ; p < pstates.size(); ++p)
158  {
159  /* create a motion */
160  Motion *motion = new Motion();
161  motion->state = pstates[p];
162  //we need multiple copies of rctrl
163  motion->control = siC_->allocControl();
164  siC_->copyControl(motion->control, rctrl);
165  motion->steps = 1;
166  motion->parent = lastmotion;
167  lastmotion = motion;
168  nn_->add(motion);
169  double dist = 0.0;
170  solved = goal->isSatisfied(motion->state, &dist);
171  if (solved)
172  {
173  approxdif = dist;
174  solution = motion;
175  break;
176  }
177  if (dist < approxdif)
178  {
179  approxdif = dist;
180  approxsol = motion;
181  }
182  }
183 
184  //free any states after we hit the goal
185  while (++p < pstates.size())
186  si_->freeState(pstates[p]);
187  if (solved)
188  break;
189  }
190  else
191  for (size_t p = 0 ; p < pstates.size(); ++p)
192  si_->freeState(pstates[p]);
193  }
194  else
195  {
196  if (cd >= siC_->getMinControlDuration())
197  {
198  /* create a motion */
199  Motion *motion = new Motion(siC_);
200  si_->copyState(motion->state, rmotion->state);
201  siC_->copyControl(motion->control, rctrl);
202  motion->steps = cd;
203  motion->parent = nmotion;
204 
205  nn_->add(motion);
206  double dist = 0.0;
207  bool solv = goal->isSatisfied(motion->state, &dist);
208  if (solv)
209  {
210  approxdif = dist;
211  solution = motion;
212  break;
213  }
214  if (dist < approxdif)
215  {
216  approxdif = dist;
217  approxsol = motion;
218  }
219  }
220  }
221  }
222 
223  bool solved = false;
224  bool approximate = false;
225  if (solution == NULL)
226  {
227  solution = approxsol;
228  approximate = true;
229  }
230 
231  if (solution != NULL)
232  {
233  lastGoalMotion_ = solution;
234 
235  /* construct the solution path */
236  std::vector<Motion*> mpath;
237  while (solution != NULL)
238  {
239  mpath.push_back(solution);
240  solution = solution->parent;
241  }
242 
243  /* set the solution path */
244  PathControl *path = new PathControl(si_);
245  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
246  if (mpath[i]->parent)
247  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
248  else
249  path->append(mpath[i]->state);
250  solved = true;
251  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
252  }
253 
254  if (rmotion->state)
255  si_->freeState(rmotion->state);
256  if (rmotion->control)
257  siC_->freeControl(rmotion->control);
258  delete rmotion;
259  si_->freeState(xstate);
260 
261  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
262 
263  return base::PlannerStatus(solved, approximate);
264 }
265 
267 {
268  Planner::getPlannerData(data);
269 
270  std::vector<Motion*> motions;
271  if (nn_)
272  nn_->list(motions);
273 
274  double delta = siC_->getPropagationStepSize();
275 
276  if (lastGoalMotion_)
277  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
278 
279  for (unsigned int i = 0 ; i < motions.size() ; ++i)
280  {
281  const Motion *m = motions[i];
282  if (m->parent)
283  {
284  if (data.hasControls())
288  else
291  }
292  else
294  }
295 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
double getGoalBias() const
Get the goal bias the planner is using.
Definition: RRT.h:96
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:102
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
Definition: Control.h:48
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
virtual void clear()
Clear datastructures. Call this function if the input data to the planner has changed and you do not ...
Definition: RRT.cpp:68
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of goals.
Definition: Goal.h:63
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continue solving for some amount of time. Return true if solution was found.
Definition: RRT.cpp:96
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::State * state
The state contained by the motion.
Definition: RRT.h:149
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
RRT(const SpaceInformationPtr &si)
Constructor.
Definition: RRT.cpp:42
void freeMemory()
Free the memory allocated by this planner.
Definition: RRT.cpp:79
Definition of a control path.
Definition: PathControl.h:60
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition: RRT.h:186
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself...
Definition: RRT.h:108
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: RRT.cpp:60
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Motion * parent
The parent motion in the exploration tree.
Definition: RRT.h:158
Abstract definition of a goal region that can be sampled.
Representation of a motion.
Definition: RRT.h:131
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Control * control
The control contained by the motion.
Definition: RRT.h:152
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: RRT.h:192
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
A boost shared pointer wrapper for ompl::control::SpaceInformation.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
void setGoalBias(double goalBias)
Definition: RRT.h:90
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: RRT.h:165
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: RRT.h:177
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: RRT.cpp:266
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: RRT.h:183
unsigned int steps
The number of steps the control is applied for.
Definition: RRT.h:155
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68