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