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 
104  const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
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 
131  struct MotionCompare
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  }
156  Motion(base::State *state)
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_;
190  Motion *parent_;
192  Cell *cell_;
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_ != 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_;
259  double splitValue_;
261  Cell *left_;
267  std::vector<Motion *> motions_;
268  };
269 
271  void addMotion(Motion *motion, Cell *bsp, base::State * /*state*/, Eigen::Ref<Eigen::VectorXd> /*proj*/);
273  void updateHeapElement(Motion *motion)
274  {
275  if (motion->heapElement_ != nullptr)
276  priorityQueue_.update(motion->heapElement_);
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};
307  Motion *lastGoalMotion_{nullptr};
308  };
309  } // namespace geometric
310 } // namespace ompl
311 
312 #endif
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
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
unsigned int size() const
Number of cells.
Definition: PDST.h:342
void addMotion(Motion *motion, Cell *bsp, base::State *, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:186
double volume_
Volume of the cell.
Definition: PDST.h:351
Definition of an abstract state.
Definition: State.h:113
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:363
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:359
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:290
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:355
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:384
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:397
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:230
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: PDST.h:200
Class representing the tree of motions exploring the state space.
Definition: PDST.h:238
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:401
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:393
Cell is a Binary Space Partition.
Definition: PDST.h:297
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:280
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:399
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:286
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:322
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:403
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:188
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:353
double score() const
The score is used to order motions in a priority queue.
Definition: PDST.h:263
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:284
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:288
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:335
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:310
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:204
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:282
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:361
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: PDST.h:212
When an element is added to the heap, an instance of Element* is created. This instance contains the ...
Definition: BinaryHeap.h:123
double getGoalBias() const
Get the goal bias the planner is using *‍/.
Definition: PDST.h:217
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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
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:389
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:369
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:237
A shared pointer wrapper for ompl::base::StateSampler.
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:250
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:395
Abstract definition of a goal region that can be sampled.
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:357
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:391
Main namespace. Contains everything in this library.
The lower and upper bounds for an Rn space.