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 
48 namespace ompl
49 {
50  namespace geometric
51  {
76  class PDST : public base::Planner
78  {
79  public:
80  PDST(const base::SpaceInformationPtr &si);
81 
82  ~PDST() override;
83 
84  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
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 
105  {
106  return projectionEvaluator_;
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  Motion *m = const_cast<Motion *>(this);
178  while (m->parent_ && m->parent_->endState_ == m->startState_)
179  m = m->parent_;
180  return m;
181  }
182 
188  double priority_;
197  bool isSplit_;
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_)
216  {
217  delete left_;
218  delete right_;
219  }
220  }
221 
223  void subdivide(unsigned int spaceDimension);
224 
226  Cell *stab(const ompl::base::EuclideanProjection &projection) const
227  {
228  Cell *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_)
250  sz += left_->size() + right_->size();
251  return sz;
252  }
253 
255  double volume_;
257  unsigned int splitDimension_;
259  double splitValue_;
267  std::vector<Motion *> motions_;
268  };
269 
271  void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &);
273  void updateHeapElement(Motion *motion)
274  {
275  if (motion->heapElement_)
277  else
278  motion->heapElement_ = priorityQueue_.insert(motion);
279  }
284 
285  void freeMemory();
286 
289  // Random number generator
290  RNG rng_;
293  std::vector<Motion *> startMotions_;
301  double goalBias_;
305  unsigned int iteration_;
308  };
309  }
310 }
311 
312 #endif
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:186
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:295
Comparator used to order motions in the priority queue.
Definition: PDST.h:131
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:307
Class representing the tree of motions exploring the state space.
Definition: PDST.h:142
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:265
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:190
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
A shared pointer wrapper for ompl::base::StateSampler.
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:299
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:301
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:184
When an element is added to the heap, an instance of Element* is created. This instance contains the ...
Definition: BinaryHeap.h:58
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:263
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:261
void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:192
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:267
Cell is a Binary Space Partition.
Definition: PDST.h:201
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:243
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:185
double volume_
Volume of the cell.
Definition: PDST.h:255
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
double getGoalBias() const
Get the goal bias the planner is using */.
Definition: PDST.h:121
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:257
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:259
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:287
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:49
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:239
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:303
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:176
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
double score() const
The score is used to order motions in a priority queue.
Definition: PDST.h:167
void updateHeapElement(Motion *motion)
Either update heap after motion&#39;s priority has changed or insert motion into heap.
Definition: PDST.h:273
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:272
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:297
The lower and upper bounds for an Rn space.
Motion(base::State *state)
constructor for start states
Definition: PDST.h:156
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: PDST.h:98
*void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.
Definition: PDST.h:116
unsigned int size() const
Number of cells.
Definition: PDST.h:246
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:288
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: PDST.h:104
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:194
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:134
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:192
Cell * stab(const ompl::base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:226
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:56
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:305
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:188
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:139