Loading...
Searching...
No Matches
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
48namespace ompl
49{
50 namespace control
51 {
80 class PDST : public base::Planner
81 {
82 public:
83 PDST(const SpaceInformationPtr &si);
84
85 ~PDST() override;
86
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 {
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)
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_;
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_;
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>);
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
303 base::StateSamplerPtr sampler_;
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};
327 };
328 } // namespace control
329} // namespace ompl
330
331#endif
When an element is added to the heap, an instance of Element* is created. This instance contains the ...
Definition BinaryHeap.h:60
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition BinaryHeap.h:53
Element * insert(const _T &data)
Add a new element.
Definition BinaryHeap.h:140
void update(Element *element)
Update an element in the heap.
Definition BinaryHeap.h:186
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Abstract definition of a goal region that can be sampled.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:223
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:417
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Path-Directed Subdivision Tree.
Definition PDST.h:81
DirectedControlSamplerPtr controlSampler_
Directed control sampler.
Definition PDST.h:305
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition PDST.cpp:312
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition PDST.h:107
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 setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition PDST.h:101
BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition PDST.h:314
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition PDST.h:119
base::StateSamplerPtr sampler_
State sampler.
Definition PDST.h:303
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition PDST.h:324
base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition PDST.h:322
Motion * lastGoalMotion_
Closest motion to the goal.
Definition PDST.h:326
double getGoalBias() const
Get the goal bias the planner is using *‍/.
Definition PDST.h:124
const SpaceInformation * siC_
SpaceInformation convenience pointer.
Definition PDST.h:307
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition PDST.cpp:327
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition PDST.h:280
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition PDST.h:320
base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition PDST.h:318
Cell * bsp_
Binary Space Partition.
Definition PDST.h:316
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition PDST.cpp:278
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
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
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
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
Space information containing necessary information for planning with controls. setup() needs to be ca...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Cell is a Binary Space Partition.
Definition PDST.h:207
double volume_
Volume of the cell.
Definition PDST.h:260
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition PDST.h:262
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition PDST.h:266
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition PDST.h:231
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition PDST.cpp:385
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition PDST.h:268
void addMotion(Motion *motion)
Add a motion.
Definition PDST.h:244
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition PDST.h:272
base::RealVectorBounds bounds_
A bounding box for this cell.
Definition PDST.h:270
unsigned int size() const
Number of cells.
Definition PDST.h:251
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition PDST.h:264
Comparator used to order motions in the priority queue.
Definition PDST.h:135
bool operator()(Motion *p1, Motion *p2) const
returns true if m1 is lower priority than m2
Definition PDST.h:137
Class representing the tree of motions exploring the state space.
Definition PDST.h:146
Motion * parent_
Parent motion from which this one started.
Definition PDST.h:195
base::State * startState_
The starting point of this motion.
Definition PDST.h:185
base::State * endState_
The state reached by this motion.
Definition PDST.h:187
Cell * cell_
Pointer to the cell that contains this path.
Definition PDST.h:197
double priority_
Priority for selecting this path to extend from in the future.
Definition PDST.h:193
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition PDST.h:189
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition PDST.h:199
Motion(base::State *state)
constructor for start states
Definition PDST.h:162
double score() const
The score is used to order motions in a priority queue.
Definition PDST.h:175
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition PDST.h:191