All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
38 
39 #ifndef OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
40 #define OMPL_GEOMETRIC_PLANNERS_PDST_PDST_
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 
49 namespace ompl
50 {
51 
52  namespace geometric
53  {
54 
79  class PDST : public base::Planner
81  {
82  public:
83 
84  PDST(const base::SpaceInformationPtr &si);
85 
86  virtual ~PDST(void);
87 
88  virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
89  virtual void clear(void);
90  virtual void setup(void);
91 
93  virtual void getPlannerData(base::PlannerData &data) const;
94 
96  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
97  {
98  projectionEvaluator_ = projectionEvaluator;
99  }
100 
102  void setProjectionEvaluator(const std::string &name)
103  {
104  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
105  }
106 
109  {
110  return projectionEvaluator_;
111  }
112 
120  void setGoalBias(double goalBias)
121  {
122  goalBias_ = goalBias;
123  }
125  double getGoalBias(void) const
126  {
127  return goalBias_;
128  }
129 
130  protected:
131  struct Cell;
132  struct Motion;
133 
136  {
138  bool operator() (Motion* p1, Motion* p2) const
139  {
140  // lowest priority means highest score
141  return p1->score() < p2->score();
142  }
143  };
144 
146  struct Motion
147  {
148  public:
149  Motion(base::State *startState, base::State *endState, double priority, Motion* parent)
150  : startState_(startState), endState_(endState), priority_(priority),
151  parent_(parent), cell_(NULL), heapElement_(NULL), isSplit_(false)
152  {
153  }
156  : startState_(state), endState_(state), priority_(0.),
157  parent_(NULL), cell_(NULL), heapElement_(NULL), isSplit_(false)
158  {
159  }
161  double score(void) const
162  {
163  return priority_ / cell_->volume_;
164  }
165  void updatePriority(void)
166  {
167  priority_ = priority_ * 2.0 + 1.0;
168  }
169  Motion* ancestor() const
170  {
171  Motion* m = const_cast<Motion*>(this);
172  while (m->parent_ && m->parent_->endState_ == m->startState_)
173  m = m->parent_;
174  return m;
175  }
176 
182  double priority_;
191  bool isSplit_;
192  };
193 
195  struct Cell
196  {
197  Cell(double volume, const ompl::base::RealVectorBounds& bounds,
198  unsigned int splitDimension = 0)
199  : volume_(volume), splitDimension_(splitDimension), splitValue_(0.0),
200  left_(NULL), right_(NULL), bounds_(bounds)
201  {
202  }
203 
204  ~Cell()
205  {
206  if (left_)
207  {
208  delete left_;
209  delete right_;
210  }
211  }
212 
214  void subdivide(unsigned int spaceDimension);
215 
217  Cell* stab(const ompl::base::EuclideanProjection& projection) const
218  {
219  Cell *containingCell = const_cast<Cell*>(this);
220  while (containingCell->left_ != NULL)
221  {
222  if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
223  containingCell = containingCell->left_;
224  else
225  containingCell = containingCell->right_;
226  }
227  return containingCell;
228  }
230  void addMotion(Motion *motion)
231  {
232  motions_.push_back(motion);
233  motion->cell_ = this;
234  }
235 
237  unsigned int size() const
238  {
239  unsigned int sz = 1;
240  if (left_)
241  sz += left_->size() + right_->size();
242  return sz;
243  }
244 
246  double volume_;
248  unsigned int splitDimension_;
250  double splitValue_;
258  std::vector<Motion*> motions_;
259  };
260 
261 
263  void addMotion(Motion *motion, Cell *cell, base::State*, base::EuclideanProjection&);
265  void updateHeapElement(Motion* motion)
266  {
267  if (motion->heapElement_)
268  priorityQueue_.update(motion->heapElement_);
269  else
270  motion->heapElement_ = priorityQueue_.insert(motion);
271  }
275  Motion* propagateFrom(Motion* motion, base::State*, base::State*);
276 
277  void freeMemory(void);
278 
281  // Random number generator
282  RNG rng_;
285  std::vector<Motion*> startMotions_;
293  double goalBias_;
297  unsigned int iteration_;
300  };
301  }
302 }
303 
304 #endif
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:180
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: PDST.cpp:52
Comparator used to order motions in the priority queue.
Definition: PDST.h:135
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:299
Class representing the tree of motions exploring the state space.
Definition: PDST.h:146
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:267
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:256
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:184
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:285
A boost shared pointer wrapper for ompl::base::StateSampler.
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:291
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:293
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:178
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:258
When an element is added to the heap, an instance of Element* is created. This instance contains the ...
Definition: BinaryHeap.h:61
Cell * right_
The right child cell (NULL for a leaf cell)
Definition: PDST.h:254
Cell * left_
The left child cell (NULL for a leaf cell)
Definition: PDST.h:252
void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:187
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:238
Cell is a Binary Space Partition.
Definition: PDST.h:195
const base::ProjectionEvaluatorPtr & getProjectionEvaluator(void) const
Get the projection evaluator.
Definition: PDST.h:108
double volume_
Volume of the cell.
Definition: PDST.h:246
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome...
Definition: BinaryHeap.h:53
Abstract definition of a goal region that can be sampled.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:248
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:250
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:287
A boost 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:50
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:311
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:230
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
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 NULL if no valid motion cou...
Definition: PDST.cpp:171
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:96
unsigned int size() const
Number of cells.
Definition: PDST.h:237
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:265
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:289
The lower and upper bounds for an Rn space.
Motion(base::State *state)
constructor for start states
Definition: PDST.h:155
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:138
double score(void) const
The score is used to order motions in a priority queue.
Definition: PDST.h:161
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: PDST.h:102
*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:120
virtual void getPlannerData(base::PlannerData &data) const
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:282
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:280
Cell * stab(const ompl::base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:217
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:392
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:188
double getGoalBias(void) const
Get the goal bias the planner is using */.
Definition: PDST.h:125
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:186
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:297
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:182