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_ = 0.0;
54  rO_ = std::numeric_limits<double>::infinity();
55  delayCC_ = true;
56 
57  Planner::declareParam<double>("range", this, &BallTreeRRTstar::setRange, &BallTreeRRTstar::getRange, "0.:1.:10000.");
58  Planner::declareParam<double>("goal_bias", this, &BallTreeRRTstar::setGoalBias, &BallTreeRRTstar::getGoalBias, "0.:.05:1.");
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_collision_checking", this, &BallTreeRRTstar::setDelayCC, &BallTreeRRTstar::getDelayCC, "0,1");
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  if (ballRadiusMax_ == 0.0)
77  ballRadiusMax_ = si_->getMaximumExtent();
78  if (ballRadiusConst_ == 0.0)
79  ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
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  base::OptimizationObjective *opt = pdef_->getOptimizationObjective().get();
102 
103  if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
104  {
105  opt = NULL;
106  OMPL_WARN("Optimization objective '%s' specified, but such an objective is not appropriate for %s. Only path length can be optimized.", getName().c_str(), opt->getDescription().c_str());
107  }
108 
109  if (!goal)
110  {
111  OMPL_ERROR("Goal undefined");
113  }
114 
115  while (const base::State *st = pis_.nextStart())
116  {
117  Motion *motion = new Motion(si_, rO_);
118  si_->copyState(motion->state, st);
119  addMotion(motion);
120  }
121 
122  if (nn_->size() == 0)
123  {
124  OMPL_ERROR("There are no valid initial states!");
126  }
127 
128  if (!sampler_)
129  sampler_ = si_->allocStateSampler();
130 
131  OMPL_INFORM("Starting with %u states", nn_->size());
132 
133  Motion *solution = NULL;
134  Motion *approximation = NULL;
135  double approximatedist = std::numeric_limits<double>::infinity();
136  bool sufficientlyShort = false;
137 
138  Motion *rmotion = new Motion(si_, rO_);
139  Motion *toTrim = NULL;
140  base::State *rstate = rmotion->state;
141  base::State *xstate = si_->allocState();
142  base::State *tstate = si_->allocState();
143  std::vector<Motion*> solCheck;
144  std::vector<Motion*> nbh;
145  std::vector<double> dists;
146  std::vector<int> valid;
147  long unsigned int rewireTest = 0;
148  double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
149 
150  std::pair<base::State*,double> lastValid(tstate, 0.0);
151 
152  while (ptc == false)
153  {
154  bool rejected = false;
155 
156  /* sample until a state not within any of the existing volumes is found */
157  do
158  {
159  /* sample random state (with goal biasing) */
160  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
161  goal_s->sampleGoal(rstate);
162  else
163  sampler_->sampleUniform(rstate);
164 
165  /* check to see if it is inside an existing volume */
166  if (inVolume(rstate))
167  {
168  rejected = true;
169 
170  /* see if the state is valid */
171  if(!si_->isValid(rstate))
172  {
173  /* if it's not, reduce the size of the nearest volume to the distance
174  between its center and the rejected state */
175  toTrim = nn_->nearest(rmotion);
176  double newRad = si_->distance(toTrim->state, rstate);
177  if (newRad < toTrim->volRadius)
178  toTrim->volRadius = newRad;
179  }
180 
181  }
182  else
183 
184  rejected = false;
185 
186  }
187  while (rejected);
188 
189  /* find closest state in the tree */
190  Motion *nmotion = nn_->nearest(rmotion);
191 
192  base::State *dstate = rstate;
193 
194  /* find state to add */
195  double d = si_->distance(nmotion->state, rstate);
196  if (d > maxDistance_)
197  {
198  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
199  dstate = xstate;
200  }
201 
202  if (si_->checkMotion(nmotion->state, dstate, lastValid))
203  {
204  /* create a motion */
205  double distN = si_->distance(dstate, nmotion->state);
206  Motion *motion = new Motion(si_, rO_);
207  si_->copyState(motion->state, dstate);
208  motion->parent = nmotion;
209  motion->cost = nmotion->cost + distN;
210 
211  /* find nearby neighbors */
212  double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
213  ballRadiusMax_);
214 
215  nn_->nearestR(motion, r, nbh);
216  rewireTest += nbh.size();
217 
218  // cache for distance computations
219  dists.resize(nbh.size());
220  // cache for motion validity
221  valid.resize(nbh.size());
222  std::fill(valid.begin(), valid.end(), 0);
223 
224  if (delayCC_)
225  {
226  // calculate all costs and distances
227  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
228  if (nbh[i] != nmotion)
229  {
230  double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
231  nbh[i]->cost = c;
232  }
233 
234  // sort the nodes
235  std::sort(nbh.begin(), nbh.end(), compareMotion);
236 
237  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
238  if (nbh[i] != nmotion)
239  {
240  dists[i] = si_->distance(nbh[i]->state, dstate);
241  nbh[i]->cost -= dists[i];
242  }
243  // collision check until a valid motion is found
244  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
245  if (nbh[i] != nmotion)
246  {
247 
248  dists[i] = si_->distance(nbh[i]->state, dstate);
249  double c = nbh[i]->cost + dists[i];
250  if (c < motion->cost)
251  {
252  if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
253  {
254  motion->cost = c;
255  motion->parent = nbh[i];
256  valid[i] = 1;
257  break;
258  }
259  else
260  {
261  valid[i] = -1;
262  /* if a collision is found, trim radius to distance from motion to last valid state */
263  double nR = si_->distance(nbh[i]->state, lastValid.first);
264  if (nR < nbh[i]->volRadius)
265  nbh[i]->volRadius = nR;
266  }
267  }
268  }
269  else
270  {
271  valid[i] = 1;
272  dists[i] = distN;
273  break;
274  }
275 
276  }
277  else{
278  /* find which one we connect the new state to*/
279  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
280  if (nbh[i] != nmotion)
281  {
282 
283  dists[i] = si_->distance(nbh[i]->state, dstate);
284  double c = nbh[i]->cost + dists[i];
285  if (c < motion->cost)
286  {
287  if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
288  {
289  motion->cost = c;
290  motion->parent = nbh[i];
291  valid[i] = 1;
292  }
293  else
294  {
295  valid[i] = -1;
296  /* if a collision is found, trim radius to distance from motion to last valid state */
297  double newR = si_->distance(nbh[i]->state, lastValid.first);
298  if (newR < nbh[i]->volRadius)
299  nbh[i]->volRadius = newR;
300 
301  }
302  }
303  }
304  else
305  {
306  valid[i] = 1;
307  dists[i] = distN;
308  }
309  }
310 
311  /* add motion to tree */
312  addMotion(motion);
313  motion->parent->children.push_back(motion);
314 
315  solCheck.resize(1);
316  solCheck[0] = motion;
317 
318  /* rewire tree if needed */
319  for (unsigned int i = 0 ; i < nbh.size() ; ++i)
320  if (nbh[i] != motion->parent)
321  {
322  double c = motion->cost + dists[i];
323  if (c < nbh[i]->cost)
324  {
325  bool v = false;
326  if (valid[i] == 0)
327  {
328  if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
329  {
330  /* if a collision is found, trim radius to distance from motion to last valid state */
331  double R = si_->distance(nbh[i]->state, lastValid.first);
332  if (R < nbh[i]->volRadius)
333  nbh[i]->volRadius = R;
334  }
335  else
336  {
337  v = true;
338  }
339  }
340  if (valid[i] == 1)
341  v = true;
342 
343  if (v)
344  {
345  // Remove this node from its parent list
346  removeFromParent (nbh[i]);
347  double delta = c - nbh[i]->cost;
348 
349  nbh[i]->parent = motion;
350  nbh[i]->cost = c;
351  nbh[i]->parent->children.push_back(nbh[i]);
352  solCheck.push_back(nbh[i]);
353 
354  // Update the costs of the node's children
355  updateChildCosts(nbh[i], delta);
356  }
357  }
358  }
359 
360  // Make sure to check the existing solution for improvement
361  if (solution)
362  solCheck.push_back(solution);
363 
364  // check if we found a solution
365  for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
366  {
367  double dist = 0.0;
368  bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
369  sufficientlyShort = solved ? (opt ? opt->isSatisfied(solCheck[i]->cost) : true) : false;
370 
371  if (solved)
372  {
373  if (sufficientlyShort)
374  {
375  solution = solCheck[i];
376  break;
377  }
378  else if (!solution || (solCheck[i]->cost < solution->cost))
379  {
380  solution = solCheck[i];
381  }
382  }
383  else if (!solution && dist < approximatedist)
384  {
385  approximation = solCheck[i];
386  approximatedist = dist;
387  }
388  }
389 
390  // terminate if a sufficient solution is found
391  if (solution && sufficientlyShort)
392  break;
393  }
394  else
395  {
396  /* if a collision is found, trim radius to distance from motion to last valid state */
397  toTrim = nn_->nearest(nmotion);
398  double newRadius = si_->distance(toTrim->state, lastValid.first);
399  if (newRadius < toTrim->volRadius)
400  toTrim->volRadius = newRadius;
401  }
402  }
403 
404  double solutionCost;
405  bool approximate = (solution == NULL);
406  bool addedSolution = false;
407  if (approximate)
408  {
409  solution = approximation;
410  solutionCost = approximatedist;
411  }
412  else
413  solutionCost = solution->cost;
414 
415  if (solution != NULL)
416  {
417  // construct the solution path
418  std::vector<Motion*> mpath;
419  while (solution != NULL)
420  {
421  mpath.push_back(solution);
422  solution = solution->parent;
423  }
424 
425  // set the solution path
426  PathGeometric *path = new PathGeometric(si_);
427  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
428  path->append(mpath[i]->state);
429  pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
430  addedSolution = true;
431  }
432 
433  si_->freeState(xstate);
434  if (rmotion->state)
435  si_->freeState(rmotion->state);
436  delete rmotion;
437 
438  OMPL_INFORM("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
439 
440  return base::PlannerStatus(addedSolution, approximate);
441 }
442 
444 {
445  std::vector<Motion*>::iterator it = m->parent->children.begin ();
446  while (it != m->parent->children.end ())
447  {
448  if (*it == m)
449  {
450  it = m->parent->children.erase(it);
451  it = m->parent->children.end ();
452  }
453  else
454  ++it;
455  }
456 }
457 
459 {
460  for (size_t i = 0; i < m->children.size(); ++i)
461  {
462  m->children[i]->cost += delta;
463  updateChildCosts(m->children[i], delta);
464  }
465 }
466 
468 {
469  if (nn_)
470  {
471  std::vector<Motion*> motions;
472  nn_->list(motions);
473  for (unsigned int i = 0 ; i < motions.size() ; ++i)
474  {
475  if (motions[i]->state)
476  si_->freeState(motions[i]->state);
477  delete motions[i];
478  }
479  }
480 }
481 
483 {
484  Planner::getPlannerData(data);
485 
486  std::vector<Motion*> motions;
487  if (nn_)
488  nn_->list(motions);
489 
490  for (unsigned int i = 0 ; i < motions.size() ; ++i)
491  data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
492  base::PlannerDataVertex (motions[i]->state));
493 }