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 *>(nullptr, nullptr);
48 
49  Planner::declareParam<double>("range", this, &SBL::setRange, &SBL::getRange, "0.:1.:10000.");
50 }
51 
52 ompl::geometric::SBL::~SBL()
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::setup();
63 
64  tStart_.grid.setDimension(projectionEvaluator_->getDimension());
65  tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
66 }
67 
69 {
70  for (const auto &it : grid)
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  auto *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 planning with %d states already in datastructure", getName().c_str(),
117  (int)(tStart_.size + tGoal_.size));
118 
119  std::vector<Motion *> solution;
120  base::State *xstate = si_->allocState();
121 
122  bool startTree = true;
123  bool solved = false;
124 
125  while (ptc == false)
126  {
127  TreeData &tree = startTree ? tStart_ : tGoal_;
128  startTree = !startTree;
129  TreeData &otherTree = startTree ? tStart_ : tGoal_;
130 
131  // if we have not sampled too many goals already
132  if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
133  {
134  const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
135  if (st)
136  {
137  auto *motion = new Motion(si_);
138  si_->copyState(motion->state, st);
139  motion->root = motion->state;
140  motion->valid = true;
141  addMotion(tGoal_, motion);
142  }
143  if (tGoal_.size == 0)
144  {
145  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(),
180  tStart_.grid.size(), tGoal_.grid.size());
181 
183 }
184 
185 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
186  std::vector<Motion *> &solution)
187 {
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 
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 {
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 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
TreeData tGoal_
The goal tree.
Definition: SBL.h:265
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
Motion * parent
The parent motion – it contains the state this motion originates at.
Definition: SBL.h:167
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:173
Representation of a simple grid.
Definition: Grid.h:50
The planner failed to find a solution.
Definition: PlannerStatus.h:62
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:81
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
virtual bool couldSample() 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.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:269
void clear()
Clears the PDF.
Definition: PDF.h:242
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
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:332
void freeMemory()
Free the memory allocated by the planner.
Definition: SBL.h:226
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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...
_T data
The data we store in the cell.
Definition: Grid.h:60
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: SBL.h:259
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
base::State * state
The state this motion leads to.
Definition: SBL.h:164
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:54
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:270
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
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:58
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:178
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:222
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:219
double getRange() const
Get the range the planner is using.
Definition: SBL.h:123
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SBL.cpp:57
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:161
A shared pointer wrapper for ompl::base::SpaceInformation.
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:276
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition: SBL.cpp:43
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:49
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:216
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SBL.h:117
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
Definition of a cell in this grid.
Definition: Grid.h:57
base::ValidStateSamplerPtr sampler_
The employed state sampler.
Definition: SBL.h:256
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:231
_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
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
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
RNG rng_
The random number generator to be used.
Definition: SBL.h:271
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SBL.cpp:352
Representation of a motion.
Definition: SBL.h:146
Representation of a search tree. Two instances will be used. One for start and one for goal...
Definition: SBL.h:209
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:274
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
double maxDistance_
The maximum length of a motion to be added in the tree.
Definition: SBL.h:268
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
TreeData tStart_
The start tree.
Definition: SBL.h:262