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
43ompl::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
50ompl::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
183bool 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}
Representation of a simple grid.
Definition: Grid.h:52
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Definition: Grid.h:55
_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,...
Definition: PDF.h:97
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
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
Abstract definition of a goal region that can be sampled.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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 addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
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...
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...
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:429
Definition of an abstract state.
Definition: State.h:50
Representation of a motion.
Definition: SBL.h:147
base::State * state
The state this motion leads to.
Definition: SBL.h:162
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:171
Motion * parent
The parent motion – it contains the state this motion originates at.
Definition: SBL.h:165
const base::State * root
The root of the tree this motion would get to, if we were to follow parent pointers.
Definition: SBL.h:159
bool valid
Flag indicating whether this motion has been checked for validity.
Definition: SBL.h:168
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
Definition: SBL.cpp:66
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
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:274
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SBL.cpp:350
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
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
Definition: SBL.cpp:330
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SBL.cpp:55
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
double getRange() const
Get the range the planner is using.
Definition: SBL.h:123
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: SBL.h:117
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
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
Definition: SBL.cpp:268
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition: SBL.cpp:43
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
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_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
Definition of a cell in this grid.
Definition: Grid.h:59
_T data
The data we store in the cell.
Definition: Grid.h:61
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
Representation of a search tree. Two instances will be used. One for start and one for goal.
Definition: SBL.h:208
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Definition: SBL.h:218
unsigned int size
The number of motions (in total) from the tree.
Definition: SBL.h:215
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
Definition: SBL.h:212