All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
LazyRRT.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/geometric/planners/rrt/LazyRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <cassert>
42 
43 ompl::geometric::LazyRRT::LazyRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "LazyRRT")
44 {
45  specs_.directed = true;
46  goalBias_ = 0.05;
47  maxDistance_ = 0.0;
48  lastGoalMotion_ = NULL;
49 
50  Planner::declareParam<double>("range", this, &LazyRRT::setRange, &LazyRRT::getRange);
51  Planner::declareParam<double>("goal_bias", this, &LazyRRT::setGoalBias, &LazyRRT::getGoalBias);
52 }
53 
54 ompl::geometric::LazyRRT::~LazyRRT(void)
55 {
56  freeMemory();
57 }
58 
60 {
61  Planner::setup();
62  tools::SelfConfig sc(si_, getName());
63  sc.configurePlannerRange(maxDistance_);
64 
65  if (!nn_)
66  nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
67  nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
68 }
69 
71 {
72  Planner::clear();
73  sampler_.reset();
74  freeMemory();
75  if (nn_)
76  nn_->clear();
77  lastGoalMotion_ = NULL;
78 }
79 
81 {
82  if (nn_)
83  {
84  std::vector<Motion*> motions;
85  nn_->list(motions);
86  for (unsigned int i = 0 ; i < motions.size() ; ++i)
87  {
88  if (motions[i]->state)
89  si_->freeState(motions[i]->state);
90  delete motions[i];
91  }
92  }
93 }
94 
96 {
97  checkValidity();
98  base::Goal *goal = pdef_->getGoal().get();
99  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
100 
101  while (const base::State *st = pis_.nextStart())
102  {
103  Motion *motion = new Motion(si_);
104  si_->copyState(motion->state, st);
105  motion->valid = true;
106  nn_->add(motion);
107  }
108 
109  if (nn_->size() == 0)
110  {
111  logError("There are no valid initial states!");
113  }
114 
115  if (!sampler_)
116  sampler_ = si_->allocStateSampler();
117 
118  logInform("Starting with %u states", nn_->size());
119 
120  Motion *solution = NULL;
121  double distsol = -1.0;
122  Motion *rmotion = new Motion(si_);
123  base::State *rstate = rmotion->state;
124  base::State *xstate = si_->allocState();
125 
126  bool solutionFound = false;
127 
128  while (ptc() == false && !solutionFound)
129  {
130  /* sample random state (with goal biasing) */
131  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
132  goal_s->sampleGoal(rstate);
133  else
134  sampler_->sampleUniform(rstate);
135 
136  /* find closest state in the tree */
137  Motion *nmotion = nn_->nearest(rmotion);
138  assert(nmotion != rmotion);
139  base::State *dstate = rstate;
140 
141  /* find state to add */
142  double d = si_->distance(nmotion->state, rstate);
143  if (d > maxDistance_)
144  {
145  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
146  dstate = xstate;
147  }
148 
149  /* create a motion */
150  Motion *motion = new Motion(si_);
151  si_->copyState(motion->state, dstate);
152  motion->parent = nmotion;
153  nmotion->children.push_back(motion);
154  nn_->add(motion);
155 
156  double dist = 0.0;
157  if (goal->isSatisfied(motion->state, &dist))
158  {
159  distsol = dist;
160  solution = motion;
161  solutionFound = true;
162 
163  lastGoalMotion_ = solution;
164 
165  // Check that the solution is valid:
166  // construct the solution path
167  std::vector<Motion*> mpath;
168  while (solution != NULL)
169  {
170  mpath.push_back(solution);
171  solution = solution->parent;
172  }
173 
174  // check each segment along the path for validity
175  for (int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
176  if (!mpath[i]->valid)
177  {
178  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
179  mpath[i]->valid = true;
180  else
181  {
182  removeMotion(mpath[i]);
183  solutionFound = false;
184  }
185  }
186 
187  if (solutionFound)
188  {
189  // set the solution path
190  PathGeometric *path = new PathGeometric(si_);
191  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
192  path->append(mpath[i]->state);
193 
194  pdef_->addSolutionPath(base::PathPtr(path), false, distsol);
195  }
196  }
197  }
198 
199  si_->freeState(xstate);
200  si_->freeState(rstate);
201  delete rmotion;
202 
203  logInform("Created %u states", nn_->size());
204 
206 }
207 
209 {
210  nn_->remove(motion);
211 
212  /* remove self from parent list */
213 
214  if (motion->parent)
215  {
216  for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
217  if (motion->parent->children[i] == motion)
218  {
219  motion->parent->children.erase(motion->parent->children.begin() + i);
220  break;
221  }
222  }
223 
224  /* remove children */
225  for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
226  {
227  motion->children[i]->parent = NULL;
228  removeMotion(motion->children[i]);
229  }
230 
231  if (motion->state)
232  si_->freeState(motion->state);
233  delete motion;
234 }
235 
237 {
238  Planner::getPlannerData(data);
239 
240  std::vector<Motion*> motions;
241  if (nn_)
242  nn_->list(motions);
243 
244  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));
245 
246  for (unsigned int i = 0 ; i < motions.size() ; ++i)
247  {
248  if (motions[i]->parent == NULL)
249  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
250  else
251  data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
252  base::PlannerDataVertex(motions[i]->state));
253 
254  data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
255  }
256 }