PDST.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
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 Rice University 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: Jonathan Sobieski, Mark Moll */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
38#define OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
39
40#include <utility>
41
42#include "ompl/base/Planner.h"
43#include "ompl/base/goals/GoalSampleableRegion.h"
44#include "ompl/geometric/PathGeometric.h"
45#include "ompl/base/PlannerData.h"
46#include "ompl/datastructures/BinaryHeap.h"
47
48namespace ompl
49{
50 namespace geometric
51 {
77 class PDST : public base::Planner
78 {
79 public:
80 PDST(const base::SpaceInformationPtr &si);
81
82 ~PDST() override;
83
85 void clear() override;
86 void setup() override;
87
89 void getPlannerData(base::PlannerData &data) const override;
90
92 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
93 {
94 projectionEvaluator_ = projectionEvaluator;
95 }
96
98 void setProjectionEvaluator(const std::string &name)
99 {
100 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
101 }
102
104 const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
105 {
107 }
108
116 void setGoalBias(double goalBias)
117 {
118 goalBias_ = goalBias;
119 }
121 double getGoalBias() const
122 {
123 return goalBias_;
124 }
125
126 protected:
127 struct Cell;
128 struct Motion;
129
132 {
134 bool operator()(Motion *p1, Motion *p2) const
135 {
136 // lowest priority means highest score
137 return p1->score() < p2->score();
138 }
139 };
140
142 struct Motion
143 {
144 public:
145 Motion(base::State *startState, base::State *endState, double priority, Motion *parent)
146 : startState_(startState)
147 , endState_(endState)
148 , priority_(priority)
149 , parent_(parent)
150 , cell_(nullptr)
151 , heapElement_(nullptr)
152 , isSplit_(false)
153 {
154 }
157 : startState_(state)
158 , endState_(state)
159 , priority_(0.)
160 , parent_(nullptr)
161 , cell_(nullptr)
162 , heapElement_(nullptr)
163 , isSplit_(false)
164 {
165 }
167 double score() const
168 {
169 return priority_ / cell_->volume_;
170 }
171 void updatePriority()
172 {
173 priority_ = priority_ * 2.0 + 1.0;
174 }
175 Motion *ancestor() const
176 {
177 auto *m = const_cast<Motion *>(this);
178 while ((m->parent_ != nullptr) && m->parent_->endState_ == m->startState_)
179 m = m->parent_;
180 return m;
181 }
182
188 double priority_;
198 };
199
201 struct Cell
202 {
203 Cell(double volume, ompl::base::RealVectorBounds bounds, unsigned int splitDimension = 0)
204 : volume_(volume)
205 , splitDimension_(splitDimension)
206 , splitValue_(0.0)
207 , left_(nullptr)
208 , right_(nullptr)
209 , bounds_(std::move(bounds))
210 {
211 }
212
213 ~Cell()
214 {
215 if (left_ != nullptr)
216 {
217 delete left_;
218 delete right_;
219 }
220 }
221
223 void subdivide(unsigned int spaceDimension);
224
226 Cell *stab(const Eigen::Ref<Eigen::VectorXd> &projection) const
227 {
228 auto *containingCell = const_cast<Cell *>(this);
229 while (containingCell->left_ != nullptr)
230 {
231 if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
232 containingCell = containingCell->left_;
233 else
234 containingCell = containingCell->right_;
235 }
236 return containingCell;
237 }
239 void addMotion(Motion *motion)
240 {
241 motions_.push_back(motion);
242 motion->cell_ = this;
243 }
244
246 unsigned int size() const
247 {
248 unsigned int sz = 1;
249 if (left_ != nullptr)
250 sz += left_->size() + right_->size();
251 return sz;
252 }
253
255 double volume_;
257 unsigned int splitDimension_;
267 std::vector<Motion *> motions_;
268 };
269
271 void addMotion(Motion *motion, Cell *bsp, base::State * /*state*/, Eigen::Ref<Eigen::VectorXd> /*proj*/);
274 {
275 if (motion->heapElement_ != nullptr)
277 else
278 motion->heapElement_ = priorityQueue_.insert(motion);
279 }
283 Motion *propagateFrom(Motion *motion, base::State * /*start*/, base::State * /*rnd*/);
284
285 void freeMemory();
286
289 // Random number generator
290 RNG rng_;
293 std::vector<Motion *> startMotions_;
297 Cell *bsp_{nullptr};
301 double goalBias_{0.05};
305 unsigned int iteration_{1};
308 };
309 } // namespace geometric
310} // namespace ompl
311
312#endif
When an element is added to the heap, an instance of Element* is created. This instance contains the ...
Definition: BinaryHeap.h:60
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Abstract definition of a goal region that can be sampled.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::base::StateSampler.
Definition of an abstract state.
Definition: State.h:50
Path-Directed Subdivision Tree.
Definition: PDST.h:78
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:299
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:273
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:297
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: PDST.h:98
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: PDST.h:104
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:301
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:267
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:282
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:295
Motion * propagateFrom(Motion *motion, base::State *, base::State *)
Select a state along motion and propagate a new motion from there. Return nullptr if no valid motion ...
Definition: PDST.cpp:170
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: PDST.cpp:50
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:237
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state.
Definition: PDST.h:92
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: PDST.h:116
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:305
void addMotion(Motion *motion, Cell *bsp, base::State *, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:186
std::vector< Motion * > startMotions_
Vector holding all of the start states supplied for the problem Each start motion is the root of its ...
Definition: PDST.h:293
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:288
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:303
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:307
double getGoalBias() const
Get the goal bias the planner is using *‍/.
Definition: PDST.h:121
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Cell is a Binary Space Partition.
Definition: PDST.h:202
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:263
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:239
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:261
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:226
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:259
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:257
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:265
unsigned int size() const
Number of cells.
Definition: PDST.h:246
double volume_
Volume of the cell.
Definition: PDST.h:255
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:310
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:267
Comparator used to order motions in the priority queue.
Definition: PDST.h:132
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:134
Class representing the tree of motions exploring the state space.
Definition: PDST.h:143
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:188
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:194
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:184
Motion(base::State *state)
constructor for start states
Definition: PDST.h:156
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:190
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:192
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:186
double score() const
The score is used to order motions in a priority queue.
Definition: PDST.h:167