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