All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RRTConnect.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/RRTConnect.h"
38 #include "ompl/datastructures/NearestNeighborsGNAT.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 
42 ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTConnect")
43 {
45  specs_.directed = true;
46 
47  maxDistance_ = 0.0;
48 
49  Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange);
50  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
51 }
52 
53 ompl::geometric::RRTConnect::~RRTConnect(void)
54 {
55  freeMemory();
56 }
57 
59 {
60  Planner::setup();
61  tools::SelfConfig sc(si_, getName());
62  sc.configurePlannerRange(maxDistance_);
63 
64  if (!tStart_)
65  tStart_.reset(new NearestNeighborsGNAT<Motion*>());
66  if (!tGoal_)
67  tGoal_.reset(new NearestNeighborsGNAT<Motion*>());
68  tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
69  tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
70 }
71 
73 {
74  std::vector<Motion*> motions;
75 
76  if (tStart_)
77  {
78  tStart_->list(motions);
79  for (unsigned int i = 0 ; i < motions.size() ; ++i)
80  {
81  if (motions[i]->state)
82  si_->freeState(motions[i]->state);
83  delete motions[i];
84  }
85  }
86 
87  if (tGoal_)
88  {
89  tGoal_->list(motions);
90  for (unsigned int i = 0 ; i < motions.size() ; ++i)
91  {
92  if (motions[i]->state)
93  si_->freeState(motions[i]->state);
94  delete motions[i];
95  }
96  }
97 }
98 
100 {
101  Planner::clear();
102  sampler_.reset();
103  freeMemory();
104  if (tStart_)
105  tStart_->clear();
106  if (tGoal_)
107  tGoal_->clear();
108  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
109 }
110 
112 {
113  /* find closest state in the tree */
114  Motion *nmotion = tree->nearest(rmotion);
115 
116  /* assume we can reach the state we go towards */
117  bool reach = true;
118 
119  /* find state to add */
120  base::State *dstate = rmotion->state;
121  double d = si_->distance(nmotion->state, rmotion->state);
122  if (d > maxDistance_)
123  {
124  si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
125  dstate = tgi.xstate;
126  reach = false;
127  }
128  // if we are in the start tree, we just check the motion like we normally do;
129  // if we are in the goal tree, we need to check the motion in reverse, but checkMotion() assumes the first state it receives as argument is valid,
130  // so we check that one first
131  bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
132 
133  if (validMotion)
134  {
135  /* create a motion */
136  Motion *motion = new Motion(si_);
137  si_->copyState(motion->state, dstate);
138  motion->parent = nmotion;
139  motion->root = nmotion->root;
140  tgi.xmotion = motion;
141 
142  tree->add(motion);
143  if (reach)
144  return REACHED;
145  else
146  return ADVANCED;
147  }
148  else
149  return TRAPPED;
150 }
151 
153 {
154  checkValidity();
155  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
156 
157  if (!goal)
158  {
159  logError("Unknown type of goal (or goal undefined)");
161  }
162 
163  while (const base::State *st = pis_.nextStart())
164  {
165  Motion *motion = new Motion(si_);
166  si_->copyState(motion->state, st);
167  motion->root = motion->state;
168  tStart_->add(motion);
169  }
170 
171  if (tStart_->size() == 0)
172  {
173  logError("Motion planning start tree could not be initialized!");
175  }
176 
177  if (!goal->couldSample())
178  {
179  logError("Insufficient states in sampleable goal region");
181  }
182 
183  if (!sampler_)
184  sampler_ = si_->allocStateSampler();
185 
186  logInform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));
187 
188  TreeGrowingInfo tgi;
189  tgi.xstate = si_->allocState();
190 
191  Motion *rmotion = new Motion(si_);
192  base::State *rstate = rmotion->state;
193  bool startTree = true;
194  bool solved = false;
195 
196  while (ptc() == false)
197  {
198  TreeData &tree = startTree ? tStart_ : tGoal_;
199  tgi.start = startTree;
200  startTree = !startTree;
201  TreeData &otherTree = startTree ? tStart_ : tGoal_;
202 
203  if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
204  {
205  const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
206  if (st)
207  {
208  Motion* motion = new Motion(si_);
209  si_->copyState(motion->state, st);
210  motion->root = motion->state;
211  tGoal_->add(motion);
212  }
213 
214  if (tGoal_->size() == 0)
215  {
216  logError("Unable to sample any valid states for goal tree");
217  break;
218  }
219  }
220 
221  /* sample random state */
222  sampler_->sampleUniform(rstate);
223 
224  GrowState gs = growTree(tree, tgi, rmotion);
225 
226  if (gs != TRAPPED)
227  {
228  /* remember which motion was just added */
229  Motion *addedMotion = tgi.xmotion;
230 
231  /* attempt to connect trees */
232 
233  /* if reached, it means we used rstate directly, no need top copy again */
234  if (gs != REACHED)
235  si_->copyState(rstate, tgi.xstate);
236 
237  GrowState gsc = ADVANCED;
238  tgi.start = startTree;
239  while (gsc == ADVANCED)
240  gsc = growTree(otherTree, tgi, rmotion);
241 
242  Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
243  Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
244 
245  /* if we connected the trees in a valid way (start and goal pair is valid)*/
246  if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
247  {
248  // it must be the case that either the start tree or the goal tree has made some progress
249  // so one of the parents is not NULL. We go one step 'back' to avoid having a duplicate state
250  // on the solution path
251  if (startMotion->parent)
252  startMotion = startMotion->parent;
253  else
254  goalMotion = goalMotion->parent;
255 
256  connectionPoint_ = std::make_pair<base::State*, base::State*>(startMotion->state, goalMotion->state);
257 
258  /* construct the solution path */
259  Motion *solution = startMotion;
260  std::vector<Motion*> mpath1;
261  while (solution != NULL)
262  {
263  mpath1.push_back(solution);
264  solution = solution->parent;
265  }
266 
267  solution = goalMotion;
268  std::vector<Motion*> mpath2;
269  while (solution != NULL)
270  {
271  mpath2.push_back(solution);
272  solution = solution->parent;
273  }
274 
275  PathGeometric *path = new PathGeometric(si_);
276  path->getStates().reserve(mpath1.size() + mpath2.size());
277  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
278  path->append(mpath1[i]->state);
279  for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
280  path->append(mpath2[i]->state);
281 
282  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
283  solved = true;
284  break;
285  }
286  }
287  }
288 
289  si_->freeState(tgi.xstate);
290  si_->freeState(rstate);
291  delete rmotion;
292 
293  logInform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
294 
296 }
297 
299 {
300  Planner::getPlannerData(data);
301 
302  std::vector<Motion*> motions;
303  if (tStart_)
304  tStart_->list(motions);
305 
306  for (unsigned int i = 0 ; i < motions.size() ; ++i)
307  {
308  if (motions[i]->parent == NULL)
309  data.addStartVertex(base::PlannerDataVertex(motions[i]->state, 1));
310  else
311  {
312  data.addEdge(base::PlannerDataVertex(motions[i]->parent->state, 1),
313  base::PlannerDataVertex(motions[i]->state, 1));
314  }
315  }
316 
317  motions.clear();
318  if (tGoal_)
319  tGoal_->list(motions);
320 
321  for (unsigned int i = 0 ; i < motions.size() ; ++i)
322  {
323  if (motions[i]->parent == NULL)
324  data.addGoalVertex(base::PlannerDataVertex(motions[i]->state, 2));
325  else
326  {
327  // The edges in the goal tree are reversed to be consistent with start tree
328  data.addEdge(base::PlannerDataVertex(motions[i]->state, 2),
329  base::PlannerDataVertex(motions[i]->parent->state, 2));
330  }
331  }
332 
333  // Add the edge connecting the two trees
334  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
335 }