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_CONTROL_PLANNERS_PDST_PDST_
38 #define OMPL_CONTROL_PLANNERS_PDST_PDST_
39 
40 #include <utility>
41 
42 #include "ompl/base/Planner.h"
43 #include "ompl/base/goals/GoalSampleableRegion.h"
44 #include "ompl/control/PathControl.h"
45 #include "ompl/control/PlannerData.h"
46 #include "ompl/datastructures/BinaryHeap.h"
47 
48 namespace ompl
49 {
50  namespace control
51  {
79  class PDST : public base::Planner
81  {
82  public:
83  PDST(const SpaceInformationPtr &si);
84 
85  ~PDST() override;
86 
87  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
88  void clear() override;
89  void setup() override;
90 
92  void getPlannerData(base::PlannerData &data) const override;
93 
95  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
96  {
97  projectionEvaluator_ = projectionEvaluator;
98  }
99 
101  void setProjectionEvaluator(const std::string &name)
102  {
103  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
104  }
105 
108  {
109  return projectionEvaluator_;
110  }
111 
119  void setGoalBias(double goalBias)
120  {
121  goalBias_ = goalBias;
122  }
124  double getGoalBias() const
125  {
126  return goalBias_;
127  }
128 
129  protected:
130  struct Cell;
131  struct Motion;
132 
135  {
137  bool operator()(Motion *p1, Motion *p2) const
138  {
139  // lowest priority means highest score
140  return p1->score() < p2->score();
141  }
142  };
143 
145  struct Motion
146  {
147  public:
148  Motion(base::State *startState, base::State *endState, Control *control, unsigned int controlDuration,
149  double priority, Motion *parent)
150  : startState_(startState)
151  , endState_(endState)
152  , control_(control)
153  , controlDuration_(controlDuration)
154  , priority_(priority)
155  , parent_(parent)
156  , cell_(nullptr)
157  , heapElement_(nullptr)
158  , isSplit_(false)
159  {
160  }
163  : startState_(state)
164  , endState_(state)
165  , control_(nullptr)
166  , controlDuration_(0)
167  , priority_(0.)
168  , parent_(nullptr)
169  , cell_(nullptr)
170  , heapElement_(nullptr)
171  , isSplit_(false)
172  {
173  }
175  double score() const
176  {
177  return priority_ / cell_->volume_;
178  }
179  void updatePriority()
180  {
181  priority_ = priority_ * 2. + 1.;
182  }
183 
191  unsigned int controlDuration_;
193  double priority_;
202  bool isSplit_;
203  };
204 
206  struct Cell
207  {
208  Cell(double volume, base::RealVectorBounds bounds, unsigned int splitDimension = 0)
209  : volume_(volume)
210  , splitDimension_(splitDimension)
211  , splitValue_(0.0)
212  , left_(nullptr)
213  , right_(nullptr)
214  , bounds_(std::move(bounds))
215  {
216  }
217 
218  ~Cell()
219  {
220  if (left_ != nullptr)
221  {
222  delete left_;
223  delete right_;
224  }
225  }
226 
228  void subdivide(unsigned int spaceDimension);
229 
231  Cell *stab(const base::EuclideanProjection &projection) const
232  {
233  auto *containingCell = const_cast<Cell *>(this);
234  while (containingCell->left_ != nullptr)
235  {
236  if (projection[containingCell->splitDimension_] <= containingCell->splitValue_)
237  containingCell = containingCell->left_;
238  else
239  containingCell = containingCell->right_;
240  }
241  return containingCell;
242  }
244  void addMotion(Motion *motion)
245  {
246  motions_.push_back(motion);
247  motion->cell_ = this;
248  }
249 
251  unsigned int size() const
252  {
253  unsigned int sz = 1;
254  if (left_ != nullptr)
255  sz += left_->size() + right_->size();
256  return sz;
257  }
258 
260  double volume_;
262  unsigned int splitDimension_;
264  double splitValue_;
272  std::vector<Motion *> motions_;
273  };
274 
280  void updateHeapElement(Motion *motion)
281  {
282  if (motion->heapElement_ != nullptr)
284  else
285  motion->heapElement_ = priorityQueue_.insert(motion);
286  }
290  Motion *propagateFrom(Motion *motion, base::State *, base::State *);
297  unsigned int findDurationAndAncestor(Motion *motion, base::State *state, base::State *scratch,
298  Motion *&ancestor) const;
299 
300  void freeMemory();
301 
308  // Random number generator
309  RNG rng_;
312  std::vector<Motion *> startMotions_;
316  Cell *bsp_{nullptr};
320  double goalBias_{0.05};
324  unsigned int iteration_{1};
327  };
328  }
329 }
330 
331 #endif
base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:270
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:191
base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:303
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:280
unsigned int size() const
Number of cells.
Definition: PDST.h:251
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:175
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:324
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:193
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:95
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:316
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition: PDST.h:305
Definition of an abstract control.
Definition: Control.h:47
const SpaceInformation * siC_
SpaceInformation convenience pointer.
Definition: PDST.h:307
A shared pointer wrapper for ompl::base::StateSampler.
BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:314
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: PDST.h:101
Class representing the tree of motions exploring the state space.
Definition: PDST.h:145
base::State * startState_
The starting point of this motion.
Definition: PDST.h:185
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:264
Cell * stab(const base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:231
base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:322
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:52
double getGoalBias() const
Get the goal bias the planner is using */.
Definition: PDST.h:124
Motion(base::State *state)
constructor for start states
Definition: PDST.h:162
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:199
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:326
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:52
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:268
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
double volume_
Volume of the cell.
Definition: PDST.h:260
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Cell * cell_
Pointer to the cell that contains this path.
Definition: PDST.h:197
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:280
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: PDST.h:107
void addMotion(Motion *motion, Cell *cell, base::State *, base::State *, base::EuclideanProjection &, base::EuclideanProjection &)
Inserts the motion into the appropriate cells, splitting the motion as necessary. motion is assumed t...
Definition: PDST.cpp:202
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:272
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:244
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:137
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:189
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:387
Cell is a Binary Space Partition.
Definition: PDST.h:206
The lower and upper bounds for an Rn space.
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:312
unsigned int findDurationAndAncestor(Motion *motion, base::State *state, base::State *scratch, Motion *&ancestor) const
Find the max. duration that the control_ in motion can be applied s.t. the trajectory passes through ...
Definition: PDST.cpp:242
Comparator used to order motions in the priority queue.
Definition: PDST.h:134
base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:318
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:314
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:329
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:266
Space information containing necessary information for planning with controls. setup() needs to be ca...
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:262
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:195
double score() const
The score is used to order motions in a priority queue.
Definition: PDST.h:175
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:320
base::State * endState_
The state reached by this motion.
Definition: PDST.h:187
*void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: PDST.h:119