All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(void)
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("There are no valid initial states!");
114  }
115 
116  if (!sampler_)
117  sampler_ = si_->allocStateSampler();
118  if (!controlSampler_)
119  controlSampler_ = siC_->allocDirectedControlSampler();
120 
121  OMPL_INFORM("Starting with %u states", 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  cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, xstate);
197 
198  if (cd >= siC_->getMinControlDuration())
199  {
200  /* create a motion */
201  Motion *motion = new Motion(siC_);
202  si_->copyState(motion->state, xstate);
203  siC_->copyControl(motion->control, rctrl);
204  motion->steps = cd;
205  motion->parent = nmotion;
206 
207  nn_->add(motion);
208  double dist = 0.0;
209  bool solv = goal->isSatisfied(motion->state, &dist);
210  if (solv)
211  {
212  approxdif = dist;
213  solution = motion;
214  break;
215  }
216  if (dist < approxdif)
217  {
218  approxdif = dist;
219  approxsol = motion;
220  }
221  }
222  }
223  }
224 
225  bool solved = false;
226  bool approximate = false;
227  if (solution == NULL)
228  {
229  solution = approxsol;
230  approximate = true;
231  }
232 
233  if (solution != NULL)
234  {
235  lastGoalMotion_ = solution;
236 
237  /* construct the solution path */
238  std::vector<Motion*> mpath;
239  while (solution != NULL)
240  {
241  mpath.push_back(solution);
242  solution = solution->parent;
243  }
244 
245  /* set the solution path */
246  PathControl *path = new PathControl(si_);
247  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
248  if (mpath[i]->parent)
249  path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
250  else
251  path->append(mpath[i]->state);
252  solved = true;
253  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
254  }
255 
256  if (rmotion->state)
257  si_->freeState(rmotion->state);
258  if (rmotion->control)
259  siC_->freeControl(rmotion->control);
260  delete rmotion;
261  si_->freeState(xstate);
262 
263  OMPL_INFORM("Created %u states", nn_->size());
264 
265  return base::PlannerStatus(solved, approximate);
266 }
267 
269 {
270  Planner::getPlannerData(data);
271 
272  std::vector<Motion*> motions;
273  if (nn_)
274  nn_->list(motions);
275 
276  double delta = siC_->getPropagationStepSize();
277 
278  if (lastGoalMotion_)
279  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
280 
281  for (unsigned int i = 0 ; i < motions.size() ; ++i)
282  {
283  const Motion* m = motions[i];
284  if (m->parent)
285  {
286  if (data.hasControls())
290  else
293  }
294  else
296  }
297 }