All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
LBKPIECE1.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/kpiece/LBKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <cassert>
41 
42 ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "LBKPIECE1"),
43  dStart_(boost::bind(&LBKPIECE1::freeMotion, this, _1)),
44  dGoal_(boost::bind(&LBKPIECE1::freeMotion, this, _1))
45 {
47 
49  maxDistance_ = 0.0;
50  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
51 
52  Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange);
53  Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction);
54  Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction, &LBKPIECE1::getMinValidPathFraction);
55 }
56 
57  ompl::geometric::LBKPIECE1::~LBKPIECE1(void)
58 {
59 }
60 
62 {
63  Planner::setup();
64  tools::SelfConfig sc(si_, getName());
65  sc.configureProjectionEvaluator(projectionEvaluator_);
66  sc.configurePlannerRange(maxDistance_);
67 
68  if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
69  throw Exception("The minimum valid path fraction must be in the range (0,1]");
70 
71  dStart_.setDimension(projectionEvaluator_->getDimension());
72  dGoal_.setDimension(projectionEvaluator_->getDimension());
73 }
74 
76 {
77  checkValidity();
78  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
79 
80  if (!goal)
81  {
82  logError("Unknown type of goal (or goal undefined)");
84  }
85 
87 
88  while (const base::State *st = pis_.nextStart())
89  {
90  Motion* motion = new Motion(si_);
91  si_->copyState(motion->state, st);
92  motion->root = st;
93  motion->valid = true;
94  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
95  dStart_.addMotion(motion, xcoord);
96  }
97 
98  if (dStart_.getMotionCount() == 0)
99  {
100  logError("Motion planning start tree could not be initialized!");
102  }
103 
104  if (!goal->couldSample())
105  {
106  logError("Insufficient states in sampleable goal region");
108  }
109 
110  if (!sampler_)
111  sampler_ = si_->allocStateSampler();
112 
113  logInform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
114 
115  base::State *xstate = si_->allocState();
116  bool startTree = true;
117  bool solved = false;
118 
119  while (ptc() == false)
120  {
121  Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
122  startTree = !startTree;
123  Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
124  disc.countIteration();
125 
126  // if we have not sampled too many goals already
127  if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
128  {
129  const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
130  if (st)
131  {
132  Motion* motion = new Motion(si_);
133  si_->copyState(motion->state, st);
134  motion->root = motion->state;
135  motion->valid = true;
136  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
137  dGoal_.addMotion(motion, xcoord);
138  }
139  if (dGoal_.getMotionCount() == 0)
140  {
141  logError("Unable to sample any valid states for goal tree");
142  break;
143  }
144  }
145 
146  Discretization<Motion>::Cell *ecell = NULL;
147  Motion *existing = NULL;
148  disc.selectMotion(existing, ecell);
149  assert(existing);
150  sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
151 
152  /* create a motion */
153  Motion* motion = new Motion(si_);
154  si_->copyState(motion->state, xstate);
155  motion->parent = existing;
156  motion->root = existing->root;
157  existing->children.push_back(motion);
158  projectionEvaluator_->computeCoordinates(motion->state, xcoord);
159  disc.addMotion(motion, xcoord);
160 
161  /* attempt to connect trees */
162  Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
163  if (ocell && !ocell->data->motions.empty())
164  {
165  Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
166 
167  if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
168  {
169  Motion* connect = new Motion(si_);
170  si_->copyState(connect->state, connectOther->state);
171  connect->parent = motion;
172  connect->root = motion->root;
173  motion->children.push_back(connect);
174  projectionEvaluator_->computeCoordinates(connect->state, xcoord);
175  disc.addMotion(connect, xcoord);
176 
177  if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
178  {
179  if (startTree)
180  connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
181  else
182  connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
183 
184  /* extract the motions and put them in solution vector */
185 
186  std::vector<Motion*> mpath1;
187  while (motion != NULL)
188  {
189  mpath1.push_back(motion);
190  motion = motion->parent;
191  }
192 
193  std::vector<Motion*> mpath2;
194  while (connectOther != NULL)
195  {
196  mpath2.push_back(connectOther);
197  connectOther = connectOther->parent;
198  }
199 
200  if (startTree)
201  mpath1.swap(mpath2);
202 
203  PathGeometric *path = new PathGeometric(si_);
204  path->getStates().reserve(mpath1.size() + mpath2.size());
205  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
206  path->append(mpath1[i]->state);
207  for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
208  path->append(mpath2[i]->state);
209 
210  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
211  solved = true;
212  break;
213  }
214  }
215  }
216  }
217 
218  si_->freeState(xstate);
219 
220  logInform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
221  dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
222  dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
223  dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
224 
226 }
227 
229 {
230  std::vector<Motion*> mpath;
231 
232  /* construct the solution path */
233  while (motion != NULL)
234  {
235  mpath.push_back(motion);
236  motion = motion->parent;
237  }
238 
239  std::pair<base::State*, double> lastValid;
240  lastValid.first = temp;
241 
242  /* check the path */
243  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
244  if (!mpath[i]->valid)
245  {
246  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
247  mpath[i]->valid = true;
248  else
249  {
250  Motion *parent = mpath[i]->parent;
251  removeMotion(disc, mpath[i]);
252 
253  // add the valid part of the path, if sufficiently long
254  if (lastValid.second > minValidPathFraction_)
255  {
256  Motion* reAdd = new Motion(si_);
257  si_->copyState(reAdd->state, lastValid.first);
258  reAdd->parent = parent;
259  reAdd->root = parent->root;
260  parent->children.push_back(reAdd);
261  reAdd->valid = true;
263  projectionEvaluator_->computeCoordinates(reAdd->state, coord);
264  disc.addMotion(reAdd, coord);
265  }
266 
267  return false;
268  }
269  }
270  return true;
271 }
272 
274 {
275  /* remove from grid */
276 
278  projectionEvaluator_->computeCoordinates(motion->state, coord);
279  disc.removeMotion(motion, coord);
280 
281  /* remove self from parent list */
282 
283  if (motion->parent)
284  {
285  for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
286  if (motion->parent->children[i] == motion)
287  {
288  motion->parent->children.erase(motion->parent->children.begin() + i);
289  break;
290  }
291  }
292 
293  /* remove children */
294  for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
295  {
296  motion->children[i]->parent = NULL;
297  removeMotion(disc, motion->children[i]);
298  }
299 
300  freeMotion(motion);
301 }
302 
303 
305 {
306  if (motion->state)
307  si_->freeState(motion->state);
308  delete motion;
309 }
310 
312 {
313  Planner::clear();
314 
315  sampler_.reset();
316  dStart_.clear();
317  dGoal_.clear();
318  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
319 }
320 
322 {
323  Planner::getPlannerData(data);
324  dStart_.getPlannerData(data, 1, true, NULL);
325  dGoal_.getPlannerData(data, 2, false, NULL);
326 
327  // Insert the edge connecting the two trees
328  data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
329 }