All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
BallTreeRRTstar.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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 /* Authors: Alejandro Perez, Sertac Karaman, Ioan Sucan */
36 
37 #include "ompl/contrib/rrt_star/BallTreeRRTstar.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <algorithm>
42 #include <limits>
43 #include <map>
44 
45 ompl::geometric::BallTreeRRTstar::BallTreeRRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "BallTreeRRTstar")
46 {
48  specs_.optimizingPaths = true;
49 
50  goalBias_ = 0.05;
51  maxDistance_ = 0.0;
52  ballRadiusMax_ = 0.0;
53  ballRadiusConst_ = 1.0;
54  rO_ = std::numeric_limits<double>::infinity();
55  delayCC_ = true;
56 
57  Planner::declareParam<double>("range", this, &BallTreeRRTstar::setRange, &BallTreeRRTstar::getRange);
58  Planner::declareParam<double>("goal_bias", this, &BallTreeRRTstar::setGoalBias, &BallTreeRRTstar::getGoalBias);
59  Planner::declareParam<double>("ball_radius_constant", this, &BallTreeRRTstar::setBallRadiusConstant, &BallTreeRRTstar::getBallRadiusConstant);
60  Planner::declareParam<double>("max_ball_radius", this, &BallTreeRRTstar::setMaxBallRadius, &BallTreeRRTstar::getMaxBallRadius);
61  Planner::declareParam<double>("initial_volume_radius", this, &BallTreeRRTstar::setInitialVolumeRadius, &BallTreeRRTstar::getInitialVolumeRadius);
62  Planner::declareParam<bool>("delay_cc", this, &BallTreeRRTstar::setDelayCC, &BallTreeRRTstar::getDelayCC);
63 }
64 
65 ompl::geometric::BallTreeRRTstar::~BallTreeRRTstar(void)
66 {
67  freeMemory();
68 }
69 
71 {
72  Planner::setup();
73  tools::SelfConfig sc(si_, getName());
74  sc.configurePlannerRange(maxDistance_);
75 
76  ballRadiusMax_ = si_->getMaximumExtent();
77  ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
78 
79  delayCC_ = true;
80 
81  if (!nn_)
82  nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
83  nn_->setDistanceFunction(boost::bind(&BallTreeRRTstar::distanceFunction, this, _1, _2));
84 }
85 
87 {
88  Planner::clear();
89  sampler_.reset();
90  motions_.clear();
91  freeMemory();
92  if (nn_)
93  nn_->clear();
94 }
95 
97 {
98  checkValidity();
99  base::Goal *goal = pdef_->getGoal().get();
100  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
101 
102  if (!goal)
103  {
104  logError("Goal undefined");
106  }
107 
108  while (const base::State *st = pis_.nextStart())
109  {
110  Motion *motion = new Motion(si_, rO_);
111  si_->copyState(motion->state, st);
112  addMotion(motion);
113  }
114 
115  if (nn_->size() == 0)
116  {
117  logError("There are no valid initial states!");
119  }
120 
121  if (!sampler_)
122  sampler_ = si_->allocStateSampler();
123 
124  logInform("Starting with %u states", nn_->size());
125 
126  Motion *solution = NULL;
127  Motion *approximation = NULL;
128  double approximatedist = std::numeric_limits<double>::infinity();
129  bool sufficientlyShort = false;
130 
131  Motion *rmotion = new Motion(si_, rO_);
132  Motion *toTrim = NULL;
133  base::State *rstate = rmotion->state;
134  base::State *xstate = si_->allocState();
135  base::State *tstate = si_->allocState();
136  std::vector<Motion*> solCheck;
137  std::vector<Motion*> nbh;
138  std::vector<double> dists;
139  std::vector<int> valid;
140  long unsigned int rewireTest = 0;
141  double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
142 
143  std::pair<base::State*,double> lastValid(tstate, 0.0);
144 
145  while (ptc() == false)
146  {
147  bool rejected = false;
148 
149  /* sample until a state not within any of the existing volumes is found */
150  do
151  {
152  /* sample random state (with goal biasing) */
153  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
154  goal_s->sampleGoal(rstate);
155  else
156  sampler_->sampleUniform(rstate);
157 
158  /* check to see if it is inside an existing volume */
159  if (inVolume(rstate))
160  {
161  rejected = true;
162 
163  /* see if the state is valid */
164  if(!si_->isValid(rstate))
165  {
166  /* if it's not, reduce the size of the nearest volume to the distance
167  between its center and the rejected state */
168  toTrim = nn_->nearest(rmotion);
169  double newRad = si_->distance(toTrim->state, rstate);
170  if (newRad < toTrim->volRadius)
171  toTrim->volRadius = newRad;
172  }
173 
174  }
175  else
176 
177  rejected = false;
178 
179  }
180  while (rejected);
181 
182  /* find closest state in the tree */
183  Motion *nmotion = nn_->nearest(rmotion);
184 
185  base::State *dstate = rstate;
186 
187  /* find state to add */
188  double d = si_->distance(nmotion->state, rstate);
189  if (d > maxDistance_)
190  {
191  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
192  dstate = xstate;
193  }
194 
195  if (si_->checkMotion(nmotion->state, dstate, lastValid))
196  {
197  /* create a motion */
198  double distN = si_->distance(dstate, nmotion->state);
199  Motion *motion = new Motion(si_, rO_);
200  si_->copyState(motion->state, dstate);
201  motion->parent = nmotion;
202  motion->cost = nmotion->cost + distN;
203 
204  /* find nearby neighbors */
205  double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
206  ballRadiusMax_);
207 
208  nn_->nearestR(motion, r, nbh);
209  rewireTest += nbh.size();
210 
211  // cache for distance computations
212  dists.resize(nbh.size());
213  // cache for motion validity
214  valid.resize(nbh.size());
215  std::fill(valid.begin(), valid.end(), 0);
216 
217  if (delayCC_)
218  {
219  // calculate all costs and distances
220  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
221  if (nbh[i] != nmotion)
222  {
223  double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
224  nbh[i]->cost = c;
225  }
226 
227  // sort the nodes
228  std::sort(nbh.begin(), nbh.end(), compareMotion);
229 
230  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
231  if (nbh[i] != nmotion)
232  {
233  dists[i] = si_->distance(nbh[i]->state, dstate);
234  nbh[i]->cost -= dists[i];
235  }
236  // collision check until a valid motion is found
237  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
238  if (nbh[i] != nmotion)
239  {
240 
241  dists[i] = si_->distance(nbh[i]->state, dstate);
242  double c = nbh[i]->cost + dists[i];
243  if (c < motion->cost)
244  {
245  if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
246  {
247  motion->cost = c;
248  motion->parent = nbh[i];
249  valid[i] = 1;
250  break;
251  }
252  else
253  {
254  valid[i] = -1;
255  /* if a collision is found, trim radius to distance from motion to last valid state */
256  double nR = si_->distance(nbh[i]->state, lastValid.first);
257  if (nR < nbh[i]->volRadius)
258  nbh[i]->volRadius = nR;
259  }
260  }
261  }
262  else
263  {
264  valid[i] = 1;
265  dists[i] = distN;
266  break;
267  }
268 
269  }
270  else{
271  /* find which one we connect the new state to*/
272  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
273  if (nbh[i] != nmotion)
274  {
275 
276  dists[i] = si_->distance(nbh[i]->state, dstate);
277  double c = nbh[i]->cost + dists[i];
278  if (c < motion->cost)
279  {
280  if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
281  {
282  motion->cost = c;
283  motion->parent = nbh[i];
284  valid[i] = 1;
285  }
286  else
287  {
288  valid[i] = -1;
289  /* if a collision is found, trim radius to distance from motion to last valid state */
290  double newR = si_->distance(nbh[i]->state, lastValid.first);
291  if (newR < nbh[i]->volRadius)
292  nbh[i]->volRadius = newR;
293 
294  }
295  }
296  }
297  else
298  {
299  valid[i] = 1;
300  dists[i] = distN;
301  }
302  }
303 
304  /* add motion to tree */
305  addMotion(motion);
306  motion->parent->children.push_back(motion);
307 
308  solCheck.resize(1);
309  solCheck[0] = motion;
310 
311  /* rewire tree if needed */
312  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
313  if (nbh[i] != motion->parent)
314  {
315  double c = motion->cost + dists[i];
316  if (c < nbh[i]->cost)
317  {
318  bool v = false;
319  if (valid[i] == 0)
320  {
321  if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
322  {
323  /* if a collision is found, trim radius to distance from motion to last valid state */
324  double R = si_->distance(nbh[i]->state, lastValid.first);
325  if (R < nbh[i]->volRadius)
326  nbh[i]->volRadius = R;
327  }
328  else
329  {
330  v = true;
331  }
332  }
333  if (valid[i] == 1)
334  v = true;
335 
336  if (v)
337  {
338  // Remove this node from its parent list
339  removeFromParent (nbh[i]);
340  double delta = c - nbh[i]->cost;
341 
342  nbh[i]->parent = motion;
343  nbh[i]->cost = c;
344  nbh[i]->parent->children.push_back(nbh[i]);
345  solCheck.push_back(nbh[i]);
346 
347  // Update the costs of the node's children
348  updateChildCosts(nbh[i], delta);
349  }
350  }
351  }
352 
353  // Make sure to check the existing solution for improvement
354  if (solution)
355  solCheck.push_back(solution);
356 
357  // check if we found a solution
358  for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
359  {
360  double dist = 0.0;
361  bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
362  sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
363 
364  if (solved)
365  {
366  if (sufficientlyShort)
367  {
368  solution = solCheck[i];
369  break;
370  }
371  else if (!solution || (solCheck[i]->cost < solution->cost))
372  {
373  solution = solCheck[i];
374  }
375  }
376  else if (!solution && dist < approximatedist)
377  {
378  approximation = solCheck[i];
379  approximatedist = dist;
380  }
381  }
382 
383  // terminate if a sufficient solution is found
384  if (solution && sufficientlyShort)
385  break;
386  }
387  else
388  {
389  /* if a collision is found, trim radius to distance from motion to last valid state */
390  toTrim = nn_->nearest(nmotion);
391  double newRadius = si_->distance(toTrim->state, lastValid.first);
392  if (newRadius < toTrim->volRadius)
393  toTrim->volRadius = newRadius;
394  }
395  }
396 
397  double solutionCost;
398  bool approximate = (solution == NULL);
399  bool addedSolution = false;
400  if (approximate)
401  {
402  solution = approximation;
403  solutionCost = approximatedist;
404  }
405  else
406  solutionCost = solution->cost;
407 
408  if (solution != NULL)
409  {
410  // construct the solution path
411  std::vector<Motion*> mpath;
412  while (solution != NULL)
413  {
414  mpath.push_back(solution);
415  solution = solution->parent;
416  }
417 
418  // set the solution path
419  PathGeometric *path = new PathGeometric(si_);
420  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
421  path->append(mpath[i]->state);
422  pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
423  addedSolution = true;
424  }
425 
426  si_->freeState(xstate);
427  if (rmotion->state)
428  si_->freeState(rmotion->state);
429  delete rmotion;
430 
431  logInform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
432 
433  return base::PlannerStatus(addedSolution, approximate);
434 }
435 
437 {
438  std::vector<Motion*>::iterator it = m->parent->children.begin ();
439  while (it != m->parent->children.end ())
440  {
441  if (*it == m)
442  {
443  it = m->parent->children.erase(it);
444  it = m->parent->children.end ();
445  }
446  else
447  ++it;
448  }
449 }
450 
452 {
453  for (size_t i = 0; i < m->children.size(); ++i)
454  {
455  m->children[i]->cost += delta;
456  updateChildCosts(m->children[i], delta);
457  }
458 }
459 
461 {
462  if (nn_)
463  {
464  std::vector<Motion*> motions;
465  nn_->list(motions);
466  for (unsigned int i = 0 ; i < motions.size() ; ++i)
467  {
468  if (motions[i]->state)
469  si_->freeState(motions[i]->state);
470  delete motions[i];
471  }
472  }
473 }
474 
476 {
477  Planner::getPlannerData(data);
478 
479  std::vector<Motion*> motions;
480  if (nn_)
481  nn_->list(motions);
482 
483  for (unsigned int i = 0 ; i < motions.size() ; ++i)
484  data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
485  base::PlannerDataVertex (motions[i]->state));
486 }