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 == nullptr)
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;
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  auto *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());
146  break;
147  }
148  }
149 
150  Motion *existing = selectMotion(tree);
151  assert(existing);
152  if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
153  continue;
154 
155  /* create a motion */
156  auto *motion = new Motion(si_);
157  si_->copyState(motion->state, xstate);
158  motion->parent = existing;
159  motion->root = existing->root;
160  existing->children.push_back(motion);
161 
162  addMotion(tree, motion);
163 
164  if (checkSolution(!startTree, tree, otherTree, motion, solution))
165  {
166  auto path(std::make_shared<PathGeometric>(si_));
167  for (auto &i : solution)
168  path->append(i->state);
169 
170  pdef_->addSolutionPath(path, false, 0.0, getName());
171  solved = true;
172  break;
173  }
174  }
175 
176  si_->freeState(xstate);
177 
178  OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
179  tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(),
180  tStart_.grid.size(), tGoal_.grid.size());
181 
182  return solved ? base::PlannerStatus::EXACT_SOLUTION : status;
183 }
184 
185 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
186  std::vector<Motion *> &solution)
187 {
188  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
189  projectionEvaluator_->computeCoordinates(motion->state, coord);
190  Grid<MotionInfo>::Cell *cell = otherTree.grid.getCell(coord);
191 
192  if (cell && !cell->data.empty())
193  {
194  Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)];
195 
196  if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
197  start ? connectOther->root : motion->root))
198  {
199  auto *connect = new Motion(si_);
200 
201  si_->copyState(connect->state, connectOther->state);
202  connect->parent = motion;
203  connect->root = motion->root;
204  motion->children.push_back(connect);
205  addMotion(tree, connect);
206 
207  if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
208  {
209  if (start)
210  connectionPoint_ = std::make_pair(motion->state, connectOther->state);
211  else
212  connectionPoint_ = std::make_pair(connectOther->state, motion->state);
213 
214  /* extract the motions and put them in solution vector */
215 
216  std::vector<Motion *> mpath1;
217  while (motion != nullptr)
218  {
219  mpath1.push_back(motion);
220  motion = motion->parent;
221  }
222 
223  std::vector<Motion *> mpath2;
224  while (connectOther != nullptr)
225  {
226  mpath2.push_back(connectOther);
227  connectOther = connectOther->parent;
228  }
229 
230  if (!start)
231  mpath1.swap(mpath2);
232 
233  for (int i = mpath1.size() - 1; i >= 0; --i)
234  solution.push_back(mpath1[i]);
235  solution.insert(solution.end(), mpath2.begin(), mpath2.end());
236 
237  return true;
238  }
239  }
240  }
241  return false;
242 }
243 
245 {
246  std::vector<Motion *> mpath;
247 
248  /* construct the solution path */
249  while (motion != nullptr)
250  {
251  mpath.push_back(motion);
252  motion = motion->parent;
253  }
254 
255  /* check the path */
256  for (int i = mpath.size() - 1; i >= 0; --i)
257  if (!mpath[i]->valid)
258  {
259  if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
260  mpath[i]->valid = true;
261  else
262  {
263  removeMotion(tree, mpath[i]);
264  return false;
265  }
266  }
267  return true;
268 }
269 
271 {
272  GridCell *cell = tree.pdf.sample(rng_.uniform01());
273  return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : nullptr;
274 }
275 
277 {
278  /* remove from grid */
279 
280  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
281  projectionEvaluator_->computeCoordinates(motion->state, coord);
282  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
283  if (cell)
284  {
285  for (unsigned int i = 0; i < cell->data.size(); ++i)
286  {
287  if (cell->data[i] == motion)
288  {
289  cell->data.erase(cell->data.begin() + i);
290  tree.size--;
291  break;
292  }
293  }
294  if (cell->data.empty())
295  {
296  tree.pdf.remove(cell->data.elem_);
297  tree.grid.remove(cell);
298  tree.grid.destroyCell(cell);
299  }
300  else
301  {
302  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
303  }
304  }
305 
306  /* remove self from parent list */
307 
308  if (motion->parent)
309  {
310  for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
311  {
312  if (motion->parent->children[i] == motion)
313  {
314  motion->parent->children.erase(motion->parent->children.begin() + i);
315  break;
316  }
317  }
318  }
319 
320  /* remove children */
321  for (auto &i : motion->children)
322  {
323  i->parent = nullptr;
324  removeMotion(tree, i);
325  }
326 
327  if (motion->state)
328  si_->freeState(motion->state);
329  delete motion;
330 }
331 
333 {
334  Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
335  projectionEvaluator_->computeCoordinates(motion->state, coord);
336  Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
337  if (cell)
338  {
339  cell->data.push_back(motion);
340  tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
341  }
342  else
343  {
344  cell = tree.grid.createCell(coord);
345  cell->data.push_back(motion);
346  tree.grid.add(cell);
347  cell->data.elem_ = tree.pdf.add(cell, 1.0);
348  }
349  tree.size++;
350 }
351 
353 {
354  Planner::clear();
355 
356  sampler_.reset();
357 
358  freeMemory();
359 
360  tStart_.grid.clear();
361  tStart_.size = 0;
362  tStart_.pdf.clear();
363 
364  tGoal_.grid.clear();
365  tGoal_.size = 0;
366  tGoal_.pdf.clear();
367  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
368 }
369 
371 {
372  Planner::getPlannerData(data);
373 
374  std::vector<MotionInfo> motionInfo;
375  tStart_.grid.getContent(motionInfo);
376 
377  for (auto &m : motionInfo)
378  for (auto &motion : m.motions_)
379  if (motion->parent == nullptr)
380  data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
381  else
382  data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
383  base::PlannerDataVertex(motion->state, 1));
384 
385  motionInfo.clear();
386  tGoal_.grid.getContent(motionInfo);
387  for (auto &m : motionInfo)
388  for (auto &motion : m.motions_)
389  if (motion->parent == nullptr)
390  data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
391  else
392  // The edges in the goal tree are reversed so that they are in the same direction as start tree
393  data.addEdge(base::PlannerDataVertex(motion->state, 2),
394  base::PlannerDataVertex(motion->parent->state, 2));
395 
396  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
397 }
_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:352
@ 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:244
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:122
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:332
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.
StatusType
The possible values of the status returned by a planner.
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:185
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:370
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:276
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:270
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