All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RRT.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/rrt/RRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <limits>
42 
43 ompl::geometric::RRT::RRT(const base::SpaceInformationPtr &si) : base::Planner(si, "RRT")
44 {
46  specs_.directed = true;
47 
48  goalBias_ = 0.05;
49  maxDistance_ = 0.0;
50  lastGoalMotion_ = NULL;
51 
52  Planner::declareParam<double>("range", this, &RRT::setRange, &RRT::getRange);
53  Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias);
54 }
55 
56 ompl::geometric::RRT::~RRT(void)
57 {
58  freeMemory();
59 }
60 
62 {
63  Planner::clear();
64  sampler_.reset();
65  freeMemory();
66  if (nn_)
67  nn_->clear();
68  lastGoalMotion_ = NULL;
69 }
70 
72 {
73  Planner::setup();
74  tools::SelfConfig sc(si_, getName());
75  sc.configurePlannerRange(maxDistance_);
76 
77  if (!nn_)
78  nn_.reset(new NearestNeighborsGNAT<Motion*>());
79  nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
80 }
81 
83 {
84  if (nn_)
85  {
86  std::vector<Motion*> motions;
87  nn_->list(motions);
88  for (unsigned int i = 0 ; i < motions.size() ; ++i)
89  {
90  if (motions[i]->state)
91  si_->freeState(motions[i]->state);
92  delete motions[i];
93  }
94  }
95 }
96 
98 {
99  checkValidity();
100  base::Goal *goal = pdef_->getGoal().get();
101  base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
102 
103  while (const base::State *st = pis_.nextStart())
104  {
105  Motion *motion = new Motion(si_);
106  si_->copyState(motion->state, st);
107  nn_->add(motion);
108  }
109 
110  if (nn_->size() == 0)
111  {
112  logError("There are no valid initial states!");
114  }
115 
116  if (!sampler_)
117  sampler_ = si_->allocStateSampler();
118 
119  logInform("Starting with %u states", nn_->size());
120 
121  Motion *solution = NULL;
122  Motion *approxsol = NULL;
123  double approxdif = std::numeric_limits<double>::infinity();
124  Motion *rmotion = new Motion(si_);
125  base::State *rstate = rmotion->state;
126  base::State *xstate = si_->allocState();
127 
128  while (ptc() == false)
129  {
130 
131  /* sample random state (with goal biasing) */
132  if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
133  goal_s->sampleGoal(rstate);
134  else
135  sampler_->sampleUniform(rstate);
136 
137  /* find closest state in the tree */
138  Motion *nmotion = nn_->nearest(rmotion);
139  base::State *dstate = rstate;
140 
141  /* find state to add */
142  double d = si_->distance(nmotion->state, rstate);
143  if (d > maxDistance_)
144  {
145  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
146  dstate = xstate;
147  }
148 
149  if (si_->checkMotion(nmotion->state, dstate))
150  {
151  /* create a motion */
152  Motion *motion = new Motion(si_);
153  si_->copyState(motion->state, dstate);
154  motion->parent = nmotion;
155 
156  nn_->add(motion);
157  double dist = 0.0;
158  bool sat = goal->isSatisfied(motion->state, &dist);
159  if (sat)
160  {
161  approxdif = dist;
162  solution = motion;
163  break;
164  }
165  if (dist < approxdif)
166  {
167  approxdif = dist;
168  approxsol = motion;
169  }
170  }
171  }
172 
173  bool solved = false;
174  bool approximate = false;
175  if (solution == NULL)
176  {
177  solution = approxsol;
178  approximate = true;
179  }
180 
181  if (solution != NULL)
182  {
183  lastGoalMotion_ = solution;
184 
185  /* construct the solution path */
186  std::vector<Motion*> mpath;
187  while (solution != NULL)
188  {
189  mpath.push_back(solution);
190  solution = solution->parent;
191  }
192 
193  /* set the solution path */
194  PathGeometric *path = new PathGeometric(si_);
195  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
196  path->append(mpath[i]->state);
197  pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
198  solved = true;
199  }
200 
201  si_->freeState(xstate);
202  if (rmotion->state)
203  si_->freeState(rmotion->state);
204  delete rmotion;
205 
206  logInform("Created %u states", nn_->size());
207 
208  return base::PlannerStatus(solved, approximate);
209 }
210 
212 {
213  Planner::getPlannerData(data);
214 
215  std::vector<Motion*> motions;
216  if (nn_)
217  nn_->list(motions);
218 
219  if (lastGoalMotion_)
220  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
221 
222  for (unsigned int i = 0 ; i < motions.size() ; ++i)
223  {
224  if (motions[i]->parent == NULL)
225  data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
226  else
227  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
228  base::PlannerDataVertex(motions[i]->state));
229  }
230 }