All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SBL.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/sbl/SBL.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::geometric::SBL::SBL(const base::SpaceInformationPtr &si) : base::Planner(si, "SBL")
44 {
46  maxDistance_ = 0.0;
47  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
48 
49  Planner::declareParam<double>("range", this, &SBL::setRange, &SBL::getRange);
50 }
51 
52 ompl::geometric::SBL::~SBL(void)
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::setup();
60  tools::SelfConfig sc(si_, getName());
61  sc.configureProjectionEvaluator(projectionEvaluator_);
62  sc.configurePlannerRange(maxDistance_);
63 
64  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
65  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
66 }
67 
69 {
70  for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
71  {
72  for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
73  {
74  if (it->second->data[i]->state)
75  si_->freeState(it->second->data[i]->state);
76  delete it->second->data[i];
77  }
78  }
79 }
80 
82 {
83  checkValidity();
84  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
85 
86  if (!goal)
87  {
88  logError("Unknown type of goal (or goal undefined)");
90  }
91 
92  while (const base::State *st = pis_.nextStart())
93  {
94  Motion *motion = new Motion(si_);
95  si_->copyState(motion->state, st);
96  motion->valid = true;
97  motion->root = motion->state;
98  addMotion(tStart_, motion);
99  }
100 
101  if (tStart_.size == 0)
102  {
103  logError("Motion planning start tree could not be initialized!");
105  }
106 
107  if (!goal->couldSample())
108  {
109  logError("Insufficient states in sampleable goal region");
111  }
112 
113  if (!sampler_)
114  sampler_ = si_->allocValidStateSampler();
115 
116  logInform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
117 
118  std::vector<Motion*> solution;
119  base::State *xstate = si_->allocState();
120 
121  bool startTree = true;
122  bool solved = false;
123 
124  while (ptc() == false)
125  {
126  TreeData &tree = startTree ? tStart_ : tGoal_;
127  startTree = !startTree;
128  TreeData &otherTree = startTree ? tStart_ : tGoal_;
129 
130  // if we have not sampled too many goals already
131  if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
132  {
133  const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
134  if (st)
135  {
136  Motion* motion = new Motion(si_);
137  si_->copyState(motion->state, st);
138  motion->root = motion->state;
139  motion->valid = true;
140  addMotion(tGoal_, motion);
141  }
142  if (tGoal_.size == 0)
143  {
144  logError("Unable to sample any valid states for goal tree");
145  break;
146  }
147  }
148 
149  Motion *existing = selectMotion(tree);
150  assert(existing);
151  if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
152  continue;
153 
154  /* create a motion */
155  Motion *motion = new Motion(si_);
156  si_->copyState(motion->state, xstate);
157  motion->parent = existing;
158  motion->root = existing->root;
159  existing->children.push_back(motion);
160 
161  addMotion(tree, motion);
162 
163  if (checkSolution(!startTree, tree, otherTree, motion, solution))
164  {
165  PathGeometric *path = new PathGeometric(si_);
166  for (unsigned int i = 0 ; i < solution.size() ; ++i)
167  path->append(solution[i]->state);
168 
169  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
170  solved = true;
171  break;
172  }
173  }
174 
175  si_->freeState(xstate);
176 
177  logInform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
178  tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
179 
181 }
182 
183 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
184 {
186  projectionEvaluator_->computeCoordinates(motion->state, coord);
187  Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
188 
189  if (cell && !cell->data.empty())
190  {
191  Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)];
192 
193  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
194  {
195  Motion *connect = new Motion(si_);
196 
197  si_->copyState(connect->state, connectOther->state);
198  connect->parent = motion;
199  connect->root = motion->root;
200  motion->children.push_back(connect);
201  addMotion(tree, connect);
202 
203  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
204  {
205  if (start)
206  connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
207  else
208  connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
209 
210  /* extract the motions and put them in solution vector */
211 
212  std::vector<Motion*> mpath1;
213  while (motion != NULL)
214  {
215  mpath1.push_back(motion);
216  motion = motion->parent;
217  }
218 
219  std::vector<Motion*> mpath2;
220  while (connectOther != NULL)
221  {
222  mpath2.push_back(connectOther);
223  connectOther = connectOther->parent;
224  }
225 
226  if (!start)
227  mpath1.swap(mpath2);
228 
229  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
230  solution.push_back(mpath1[i]);
231  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
232 
233  return true;
234  }
235  }
236  }
237  return false;
238 }
239 
241 {
242  std::vector<Motion*> mpath;
243 
244  /* construct the solution path */
245  while (motion != NULL)
246  {
247  mpath.push_back(motion);
248  motion = motion->parent;
249  }
250 
251  /* check the path */
252  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
253  if (!mpath[i]->valid)
254  {
255  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
256  mpath[i]->valid = true;
257  else
258  {
259  removeMotion(tree, mpath[i]);
260  return false;
261  }
262  }
263  return true;
264 }
265 
267 {
268  GridCell* cell = tree.pdf.sample(rng_.uniform01());
269  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
270 }
271 
273 {
274  /* remove from grid */
275 
277  projectionEvaluator_->computeCoordinates(motion->state, coord);
278  Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
279  if (cell)
280  {
281  for (unsigned int i = 0 ; i < cell->data.size(); ++i)
282  {
283  if (cell->data[i] == motion)
284  {
285  cell->data.erase(cell->data.begin() + i);
286  tree.size--;
287  break;
288  }
289  }
290  if (cell->data.empty())
291  {
292  tree.pdf.remove(cell->data.elem_);
293  tree.grid.remove(cell);
294  tree.grid.destroyCell(cell);
295  }
296  else
297  {
298  tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
299  }
300  }
301 
302  /* remove self from parent list */
303 
304  if (motion->parent)
305  {
306  for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
307  {
308  if (motion->parent->children[i] == motion)
309  {
310  motion->parent->children.erase(motion->parent->children.begin() + i);
311  break;
312  }
313  }
314  }
315 
316  /* remove children */
317  for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
318  {
319  motion->children[i]->parent = NULL;
320  removeMotion(tree, motion->children[i]);
321  }
322 
323  if (motion->state)
324  si_->freeState(motion->state);
325  delete motion;
326 }
327 
329 {
331  projectionEvaluator_->computeCoordinates(motion->state, coord);
332  Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
333  if (cell)
334  {
335  cell->data.push_back(motion);
336  tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
337  }
338  else
339  {
340  cell = tree.grid.createCell(coord);
341  cell->data.push_back(motion);
342  tree.grid.add(cell);
343  cell->data.elem_ = tree.pdf.add(cell, 1.0);
344  }
345  tree.size++;
346 }
347 
349 {
350  Planner::clear();
351 
352  sampler_.reset();
353 
354  freeMemory();
355 
356  tStart_.grid.clear();
357  tStart_.size = 0;
358  tStart_.pdf.clear();
359 
360  tGoal_.grid.clear();
361  tGoal_.size = 0;
362  tGoal_.pdf.clear();
363  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
364 }
365 
367 {
368  Planner::getPlannerData(data);
369 
370  std::vector<MotionInfo> motions;
371  tStart_.grid.getContent(motions);
372 
373  for (unsigned int i = 0 ; i < motions.size() ; ++i)
374  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
375  if (motions[i][j]->parent == NULL)
376  data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
377  else
378  data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
379  base::PlannerDataVertex(motions[i][j]->state, 1));
380 
381  motions.clear();
382  tGoal_.grid.getContent(motions);
383  for (unsigned int i = 0 ; i < motions.size() ; ++i)
384  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
385  if (motions[i][j]->parent == NULL)
386  data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
387  else
388  // The edges in the goal tree are reversed so that they are in the same direction as start tree
389  data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
390  base::PlannerDataVertex(motions[i][j]->parent->state, 2));
391 
392  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
393 }