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 
107  const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
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 
134  struct MotionCompare
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  }
162  Motion(base::State *state)
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 
185  base::State *startState_;
187  base::State *endState_;
189  control::Control *control_;
191  unsigned int controlDuration_;
193  double priority_;
195  Motion *parent_;
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 Eigen::Ref<Eigen::VectorXd>& 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_;
266  Cell *left_;
268  Cell *right_;
272  std::vector<Motion *> motions_;
273  };
274 
277  void addMotion(Motion *motion, Cell *cell, base::State *, base::State *, Eigen::Ref<Eigen::VectorXd>,
278  Eigen::Ref<Eigen::VectorXd>);
280  void updateHeapElement(Motion *motion)
281  {
282  if (motion->heapElement_ != nullptr)
283  priorityQueue_.update(motion->heapElement_);
284  else
285  motion->heapElement_ = priorityQueue_.insert(motion);
286  }
297  unsigned int findDurationAndAncestor(Motion *motion, base::State *state, base::State *scratch,
298  Motion *&ancestor) const;
299 
300  void freeMemory();
301 
303  base::StateSamplerPtr sampler_;
307  const SpaceInformation *siC_;
308  // Random number generator
309  RNG rng_;
312  std::vector<Motion *> startMotions_;
316  Cell *bsp_{nullptr};
318  base::ProjectionEvaluatorPtr projectionEvaluator_;
320  double goalBias_{0.05};
324  unsigned int iteration_{1};
326  Motion *lastGoalMotion_{nullptr};
327  };
328  } // namespace control
329 } // namespace ompl
330 
331 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:422
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:364
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:191
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:312
BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:410
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:278
A shared pointer wrapper for ompl::base::SpaceInformation.
base::State * endState_
The state reached by this motion.
Definition: PDST.h:283
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:420
base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:414
const SpaceInformation * siC_
SpaceInformation convenience pointer.
Definition: PDST.h:403
Definition of an abstract state.
Definition: State.h:113
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
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:416
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:289
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:362
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:408
double volume_
Volume of the cell.
Definition: PDST.h:356
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:385
base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:418
unsigned int size() const
Number of cells.
Definition: PDST.h:347
base::State * startState_
The starting point of this motion.
Definition: PDST.h:281
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:84
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:376
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition: PDST.h:233
Cell * cell_
Pointer to the cell that contains this path.
Definition: PDST.h:293
double getGoalBias() const
Get the goal bias the planner is using *‍/.
Definition: PDST.h:220
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition: PDST.h:401
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:360
Cell is a Binary Space Partition.
Definition: PDST.h:302
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:295
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:327
Class representing the tree of motions exploring the state space.
Definition: PDST.h:241
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:173
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:327
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:204
double score() const
The score is used to order motions in a priority queue.
Definition: PDST.h:271
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:340
Space information containing necessary information for planning with controls. setup() needs to be ca...
A shared pointer wrapper for ompl::control::DirectedControlSampler.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
void addMotion(Motion *motion, Cell *cell, base::State *, base::State *, Eigen::Ref< Eigen::VectorXd >, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cells, splitting the motion as necessary. motion is assumed t...
Definition: PDST.cpp:200
base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:399
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:358
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:285
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:250
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:412
Abstract definition of a goal region that can be sampled.
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:368
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:291
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: PDST.h:215
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: PDST.h:203
base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:366
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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:240
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:287
The lower and upper bounds for an Rn space.