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