All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SBL.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/sbl/SBL.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 #include <cassert>
42 
43 ompl::geometric::SBL::SBL(const base::SpaceInformationPtr &si) : base::Planner(si, "SBL")
44 {
46  maxDistance_ = 0.0;
47  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
48 
49  Planner::declareParam<double>("range", this, &SBL::setRange, &SBL::getRange, "0.:1.:10000.");
50 }
51 
52 ompl::geometric::SBL::~SBL(void)
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::setup();
60  tools::SelfConfig sc(si_, getName());
61  sc.configureProjectionEvaluator(projectionEvaluator_);
62  sc.configurePlannerRange(maxDistance_);
63 
64  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
65  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
66 }
67 
69 {
70  for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
71  {
72  for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
73  {
74  if (it->second->data[i]->state)
75  si_->freeState(it->second->data[i]->state);
76  delete it->second->data[i];
77  }
78  }
79 }
80 
82 {
83  checkValidity();
84  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
85 
86  if (!goal)
87  {
88  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
90  }
91 
92  while (const base::State *st = pis_.nextStart())
93  {
94  Motion *motion = new Motion(si_);
95  si_->copyState(motion->state, st);
96  motion->valid = true;
97  motion->root = motion->state;
98  addMotion(tStart_, motion);
99  }
100 
101  if (tStart_.size == 0)
102  {
103  OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
105  }
106 
107  if (!goal->couldSample())
108  {
109  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
111  }
112 
113  if (!sampler_)
114  sampler_ = si_->allocValidStateSampler();
115 
116  OMPL_INFORM("%s: Starting with %d states", getName().c_str(), (int)(tStart_.size + tGoal_.size));
117 
118  std::vector<Motion*> solution;
119  base::State *xstate = si_->allocState();
120 
121  bool startTree = true;
122  bool solved = false;
123 
124  while (ptc == false)
125  {
126  TreeData &tree = startTree ? tStart_ : tGoal_;
127  startTree = !startTree;
128  TreeData &otherTree = startTree ? tStart_ : tGoal_;
129 
130  // if we have not sampled too many goals already
131  if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
132  {
133  const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
134  if (st)
135  {
136  Motion* motion = new Motion(si_);
137  si_->copyState(motion->state, st);
138  motion->root = motion->state;
139  motion->valid = true;
140  addMotion(tGoal_, motion);
141  }
142  if (tGoal_.size == 0)
143  {
144  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
145  break;
146  }
147  }
148 
149  Motion *existing = selectMotion(tree);
150  assert(existing);
151  if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
152  continue;
153 
154  /* create a motion */
155  Motion *motion = new Motion(si_);
156  si_->copyState(motion->state, xstate);
157  motion->parent = existing;
158  motion->root = existing->root;
159  existing->children.push_back(motion);
160 
161  addMotion(tree, motion);
162 
163  if (checkSolution(!startTree, tree, otherTree, motion, solution))
164  {
165  PathGeometric *path = new PathGeometric(si_);
166  for (unsigned int i = 0 ; i < solution.size() ; ++i)
167  path->append(solution[i]->state);
168 
169  pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
170  solved = true;
171  break;
172  }
173  }
174 
175  si_->freeState(xstate);
176 
177  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)",
178  getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
179  tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
180 
182 }
183 
184 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
185 {
187  projectionEvaluator_->computeCoordinates(motion->state, coord);
188  Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
189 
190  if (cell && !cell->data.empty())
191  {
192  Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)];
193 
194  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
195  {
196  Motion *connect = new Motion(si_);
197 
198  si_->copyState(connect->state, connectOther->state);
199  connect->parent = motion;
200  connect->root = motion->root;
201  motion->children.push_back(connect);
202  addMotion(tree, connect);
203 
204  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
205  {
206  if (start)
207  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
208  else
209  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
210 
211  /* extract the motions and put them in solution vector */
212 
213  std::vector<Motion*> mpath1;
214  while (motion != NULL)
215  {
216  mpath1.push_back(motion);
217  motion = motion->parent;
218  }
219 
220  std::vector<Motion*> mpath2;
221  while (connectOther != NULL)
222  {
223  mpath2.push_back(connectOther);
224  connectOther = connectOther->parent;
225  }
226 
227  if (!start)
228  mpath1.swap(mpath2);
229 
230  for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
231  solution.push_back(mpath1[i]);
232  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
233 
234  return true;
235  }
236  }
237  }
238  return false;
239 }
240 
242 {
243  std::vector<Motion*> mpath;
244 
245  /* construct the solution path */
246  while (motion != NULL)
247  {
248  mpath.push_back(motion);
249  motion = motion->parent;
250  }
251 
252  /* check the path */
253  for (int i = mpath.size() - 1 ; i >= 0 ; --i)
254  if (!mpath[i]->valid)
255  {
256  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
257  mpath[i]->valid = true;
258  else
259  {
260  removeMotion(tree, mpath[i]);
261  return false;
262  }
263  }
264  return true;
265 }
266 
268 {
269  GridCell* cell = tree.pdf.sample(rng_.uniform01());
270  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
271 }
272 
274 {
275  /* remove from grid */
276 
278  projectionEvaluator_->computeCoordinates(motion->state, coord);
279  Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
280  if (cell)
281  {
282  for (unsigned int i = 0 ; i < cell->data.size(); ++i)
283  {
284  if (cell->data[i] == motion)
285  {
286  cell->data.erase(cell->data.begin() + i);
287  tree.size--;
288  break;
289  }
290  }
291  if (cell->data.empty())
292  {
293  tree.pdf.remove(cell->data.elem_);
294  tree.grid.remove(cell);
295  tree.grid.destroyCell(cell);
296  }
297  else
298  {
299  tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
300  }
301  }
302 
303  /* remove self from parent list */
304 
305  if (motion->parent)
306  {
307  for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
308  {
309  if (motion->parent->children[i] == motion)
310  {
311  motion->parent->children.erase(motion->parent->children.begin() + i);
312  break;
313  }
314  }
315  }
316 
317  /* remove children */
318  for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
319  {
320  motion->children[i]->parent = NULL;
321  removeMotion(tree, motion->children[i]);
322  }
323 
324  if (motion->state)
325  si_->freeState(motion->state);
326  delete motion;
327 }
328 
330 {
332  projectionEvaluator_->computeCoordinates(motion->state, coord);
333  Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
334  if (cell)
335  {
336  cell->data.push_back(motion);
337  tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
338  }
339  else
340  {
341  cell = tree.grid.createCell(coord);
342  cell->data.push_back(motion);
343  tree.grid.add(cell);
344  cell->data.elem_ = tree.pdf.add(cell, 1.0);
345  }
346  tree.size++;
347 }
348 
350 {
351  Planner::clear();
352 
353  sampler_.reset();
354 
355  freeMemory();
356 
357  tStart_.grid.clear();
358  tStart_.size = 0;
359  tStart_.pdf.clear();
360 
361  tGoal_.grid.clear();
362  tGoal_.size = 0;
363  tGoal_.pdf.clear();
364  connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
365 }
366 
368 {
369  Planner::getPlannerData(data);
370 
371  std::vector<MotionInfo> motions;
372  tStart_.grid.getContent(motions);
373 
374  for (unsigned int i = 0 ; i < motions.size() ; ++i)
375  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
376  if (motions[i][j]->parent == NULL)
377  data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
378  else
379  data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
380  base::PlannerDataVertex(motions[i][j]->state, 1));
381 
382  motions.clear();
383  tGoal_.grid.getContent(motions);
384  for (unsigned int i = 0 ; i < motions.size() ; ++i)
385  for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
386  if (motions[i][j]->parent == NULL)
387  data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
388  else
389  // The edges in the goal tree are reversed so that they are in the same direction as start tree
390  data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
391  base::PlannerDataVertex(motions[i][j]->parent->state, 2));
392 
393  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
394 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
Motion * parent
The parent motion – it contains the state this motion originates at.
Definition: SBL.h:170
Representation of a simple grid.
Definition: Grid.h:51
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:56
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:329
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:176
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SBL.cpp:57
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
_T data
The data we store in the cell.
Definition: Grid.h:62
bool isPathValid(TreeData &tree, Motion *motion)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: SBL.cpp:241
base::State * state
The state this motion leads to.
Definition: SBL.h:167
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:267
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
Definition: SBL.cpp:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
Definition: PDF.h:177
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Definition: SBL.h:225
Abstract definition of a goal region that can be sampled.
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:222
iterator begin(void) const
Return the begin() iterator for the grid.
Definition: Grid.h:377
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
double getRange(void) const
Get the range the planner is using.
Definition: SBL.h:125
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
const base::State * root
The root of the tree this motion would get to, if we were to follow parent pointers.
Definition: SBL.h:164
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:273
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition: SBL.cpp:43
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:219
bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector< Motion * > &solution)
Check if a solution can be obtained by connecting two trees using a specified motion.
Definition: SBL.cpp:184
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SBL.h:119
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
Definition of a cell in this grid.
Definition: Grid.h:59
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:232
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:276
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
Definition: PDF.h:97
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:226
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:56
bool valid
Flag indicating whether this motion has been checked for validity.
Definition: SBL.h:173
Representation of a motion.
Definition: SBL.h:149
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SBL.cpp:367
Representation of a search tree. Two instances will be used. One for start and one for goal...
Definition: SBL.h:212
Definition of a geometric path.
Definition: PathGeometric.h:55
iterator end(void) const
Return the end() iterator for the grid.
Definition: Grid.h:383
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: SBL.cpp:81
double maxDistance_
The maximum length of a motion to be added in the tree.
Definition: SBL.h:270
A boost shared pointer wrapper for ompl::base::Path.
CoordHash::const_iterator iterator
We only allow const iterators.
Definition: Grid.h:374
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SBL.cpp:349
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68