PDST.cpp
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 #include "ompl/tools/config/SelfConfig.h"
38 #include "ompl/control/planners/pdst/PDST.h"
39 
40 ompl::control::PDST::PDST(const SpaceInformationPtr &si)
41  : base::Planner(si, "PDST"), siC_(si.get()), bsp_(NULL), goalBias_(0.05),
42  goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
43 {
44  Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
45 }
46 
47 ompl::control::PDST::~PDST()
48 {
49  freeMemory();
50 }
51 
53 {
54  // Make sure the planner is configured correctly.
55  // ompl::base::Planner::checkValidity checks that there is at least one
56  // start state and an ompl::base::Goal object specified and throws an
57  // exception if this is not the case.
58  checkValidity();
59 
60  // depending on how the planning problem is set up, this may be necessary
61  bsp_->bounds_ = projectionEvaluator_->getBounds();
62  base::Goal *goal = pdef_->getGoal().get();
63  goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal);
64 
65  // Ensure that we have a state sampler AND a control sampler
66  if (!sampler_)
67  sampler_ = si_->allocStateSampler();
68  if (!controlSampler_)
69  controlSampler_ = siC_->allocDirectedControlSampler();
70 
71  // Initialize to correct values depending on whether or not previous calls to solve
72  // generated an approximate or exact solution. If solve is being called for the first
73  // time then initializes hasSolution to false and isApproximate to true.
74  double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
75  bool hasSolution = lastGoalMotion_ != NULL;
76  bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
77  unsigned int ndim = projectionEvaluator_->getDimension();
78 
79  // If an exact solution has already been found, do not run for another iteration.
80  if (hasSolution && !isApproximate)
82 
83  // Initialize tree with start state(s)
84  while (const base::State *st = pis_.nextStart())
85  {
86  Motion *startMotion = new Motion(si_->cloneState(st));
87  bsp_->addMotion(startMotion);
88  startMotion->heapElement_ = priorityQueue_.insert(startMotion);
89  }
90 
91  if (priorityQueue_.empty())
92  {
93  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
95  }
96 
97  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), priorityQueue_.size());
98 
99  base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
100  base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
101  while (!ptc)
102  {
103  // Get the top priority path.
104  Motion *motionSelected = priorityQueue_.top()->data;
105  motionSelected->updatePriority();
106  priorityQueue_.update(motionSelected->heapElement_);
107 
108  Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
109  if (newMotion == NULL)
110  continue;
111 
112  addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
113 
114  // Check if the newMotion reached the goal.
115  hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
116  if (hasSolution)
117  {
118  closestDistanceToGoal = distanceToGoal;
119  lastGoalMotion_ = newMotion;
120  isApproximate = false;
121  break;
122  }
123  else if (distanceToGoal < closestDistanceToGoal)
124  {
125  closestDistanceToGoal = distanceToGoal;
126  lastGoalMotion_ = newMotion;
127  }
128 
129  // subdivide cell that contained selected motion, put motions of that
130  // cell in subcells and split motions so that they contained within
131  // one subcell
132  Cell *cellSelected = motionSelected->cell_;
133  std::vector<Motion*> motions;
134  cellSelected->subdivide(ndim);
135  motions.swap(cellSelected->motions_);
136  for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m)
137  addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
138  }
139 
140  if (lastGoalMotion_ != NULL)
141  hasSolution = true;
142 
143  // If a solution path has been computed, save it in the problem definition object.
144  if (hasSolution)
145  {
146  Motion *m;
147  std::vector<unsigned int> durations(1,
148  findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
149  std::vector<Motion*> mpath(1, m);
150 
151  while (m->parent_)
152  {
153  durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
154  mpath.push_back(m);
155  }
156 
157  PathControl *path = new PathControl(si_);
158  double dt = siC_->getPropagationStepSize();
159  path->append(mpath.back()->endState_);
160  for (int i = (int) mpath.size() - 2; i > 0; i--)
161  path->append(mpath[i-1]->startState_, mpath[i]->control_, durations[i] * dt);
162  path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
163  pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal, getName());
164  }
165 
166  si_->freeState(tmpState1);
167  si_->freeState(tmpState2);
168 
169  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
170 
171  return base::PlannerStatus(hasSolution, isApproximate);
172 }
173 
175  Motion *motion, base::State *start, base::State *rnd)
176 {
177  // sample a point along the trajectory given by motion
178  unsigned int prevDuration = motion->controlDuration_;
179  if (motion->controlDuration_ > 1)
180  prevDuration = rng_.uniformInt(1, motion->controlDuration_);
181  if (prevDuration == motion->controlDuration_)
182  start = motion->endState_;
183  else
184  siC_->propagate(motion->startState_, motion->control_, prevDuration, start);
185  // generate a random state
186  if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
187  goalSampler_->sampleGoal(rnd);
188  else
189  sampler_->sampleUniform(rnd);
190  // generate a random control
191  Control *control = siC_->allocControl();
192  unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd);
193  // return new motion if duration is large enough
194  if (duration < siC_->getMinControlDuration())
195  {
196  siC_->freeControl(control);
197  return NULL;
198  }
199  return new Motion(si_->cloneState(start), si_->cloneState(rnd),
200  control, duration, ++iteration_, motion);
201 }
202 
203 void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state,
205 {
206  // If the motion is at most 1 step, then it cannot be split across cell bounds.
207  if (motion->controlDuration_ <= 1)
208  {
209  projectionEvaluator_->project(motion->endState_, proj);
210  bsp->stab(proj)->addMotion(motion);
211  updateHeapElement(motion);
212  return;
213  }
214 
215  Cell *cell = NULL, *prevCell = NULL;
216  si_->copyState(prevState, motion->startState_);
217  // propagate the motion, check for cell boundary crossings, and split as necessary
218  for (unsigned int i = 0, duration = 0 ; i < motion->controlDuration_ - 1 ; ++i, ++duration)
219  {
220  siC_->propagate(prevState, motion->control_, 1, state);
221  projectionEvaluator_->project(state, proj);
222  cell = bsp->stab(proj);
223  if (duration > 0 && cell != prevCell)
224  {
225  Motion *newMotion = new Motion(motion->startState_, si_->cloneState(prevState),
226  motion->control_, duration, motion->priority_, motion->parent_);
227  newMotion->isSplit_ = true;
228  prevCell->addMotion(newMotion);
229  updateHeapElement(newMotion);
230  motion->startState_ = newMotion->endState_;
231  motion->controlDuration_ -= duration;
232  motion->parent_ = newMotion;
233  duration = 0;
234  }
235  std::swap(state, prevState);
236  std::swap(cell, prevCell);
237  proj.swap(prevProj);
238  }
239  prevCell->addMotion(motion);
240  updateHeapElement(motion);
241 }
242 
243 
245  base::State *scratch, Motion*& ancestor) const
246 {
247  const double eps = std::numeric_limits<float>::epsilon();
248  unsigned int duration;
249  ancestor = motion;
250  if (state == motion->endState_ || motion->controlDuration_ == 0 ||
251  si_->distance(motion->endState_, state) < eps)
252  duration = motion->controlDuration_;
253  else if (motion->controlDuration_ > 0 &&
254  si_->distance(motion->startState_, state) < eps)
255  duration = 0;
256  else
257  {
258  duration = 1;
259  si_->copyState(scratch, motion->startState_);
260  for (; duration <= motion->controlDuration_; ++duration)
261  {
262  siC_->propagate(scratch, motion->control_, 1, scratch);
263  if (si_->distance(scratch, state) < eps)
264  break;
265  }
266  }
267  if (duration <= motion->controlDuration_)
268  {
269  // ancestor points to the start of a split motion; duration is computed
270  // relative to start state of ancestor motion
271  while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
272  {
273  ancestor = ancestor->parent_;
274  duration += ancestor->controlDuration_;
275  }
276  return duration;
277  }
278  // motion is no longer the parent of the motion starting at
279  // state because it has been split afterwards, so look for
280  // the starting point in the parent of motion.
281  return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
282 }
283 
285 {
286  Planner::clear();
287  sampler_.reset();
288  controlSampler_.reset();
289  iteration_ = 1;
290  lastGoalMotion_ = NULL;
291  freeMemory();
292  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
293 }
294 
295 void ompl::control::PDST::freeMemory()
296 {
297  // Iterate over the elements in the priority queue and clear it
298  std::vector<Motion*> motions;
299  motions.reserve(priorityQueue_.size());
300  priorityQueue_.getContent(motions);
301  for (std::vector<Motion*>::iterator it = motions.begin() ; it < motions.end() ; ++it)
302  {
303  if ((*it)->startState_ != (*it)->endState_)
304  si_->freeState((*it)->startState_);
305  if (!(*it)->isSplit_)
306  {
307  si_->freeState((*it)->endState_);
308  if ((*it)->control_)
309  siC_->freeControl((*it)->control_);
310  }
311  delete *it;
312  }
313  priorityQueue_.clear(); // clears the Element objects in the priority queue
314  delete bsp_;
315  bsp_ = NULL;
316 }
317 
319 {
320  Planner::setup();
323  if (!projectionEvaluator_->hasBounds())
324  projectionEvaluator_->inferBounds();
325  if (!projectionEvaluator_->hasBounds())
326  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
327  if (bsp_)
328  delete bsp_;
329  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
330  lastGoalMotion_ = NULL;
331 }
332 
334 {
336 
337  double dt = siC_->getPropagationStepSize();
338  base::State *scratch = si_->allocState();
339  std::vector<Motion*> motions;
340  motions.reserve(priorityQueue_.size());
341  priorityQueue_.getContent(motions);
342 
343  // Add goal vertex
344  if (lastGoalMotion_ != NULL)
346 
347  for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
348  if (!(*it)->isSplit_)
349  {
350  // We only add one edge for each motion that has been split into smaller segments
351  Motion *cur = *it, *ancestor;
352  unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
353 
354  if (cur->parent_ == NULL)
356  else if (data.hasControls())
357  {
358  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
360  PlannerDataEdgeControl(cur->control_, duration * dt));
361  if (ancestor->parent_)
362  {
363  // Include an edge between start state of parent ancestor motion and
364  // the start state of the ancestor motion, which lies somewhere on
365  // the parent ancestor motion.
366  cur = ancestor;
367  duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
368  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
370  PlannerDataEdgeControl(ancestor->control_, duration * dt));
371  }
372  }
373  else
374  {
375  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
377  if (ancestor->parent_)
378  {
379  // Include an edge between start state of parent ancestor motion and
380  // the start state of the ancestor motion, which lies somewhere on
381  // the parent ancestor motion.
382  cur = ancestor;
383  findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
384  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
386  }
387  }
388  }
389 
390  si_->freeState(scratch);
391 }
392 
393 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
394 {
395  double childVolume = .5 * volume_;
396  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
397  splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
398 
399  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
400  left_->bounds_.high[splitDimension_] = splitValue_;
401  left_->motions_.reserve(motions_.size());
402  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
403  right_->bounds_.low[splitDimension_] = splitValue_;
404  right_->motions_.reserve(motions_.size());
405 }
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:180
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:181
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:318
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:174
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:299
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:183
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:256
void clear()
Clear the heap.
Definition: BinaryHeap.h:106
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
Definition: Control.h:48
virtual void getPlannerData(base::PlannerData &data) const
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:333
Class representing the tree of motions exploring the state space.
Definition: PDST.h:148
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:291
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:293
Abstract definition of goals.
Definition: Goal.h:63
base::State * startState_
The starting point of this motion.
Definition: PDST.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Cell * stab(const base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:218
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:187
Definition of a control path.
Definition: PathControl.h:60
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:190
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:181
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:189
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:118
double getGoalBias() const
Get the goal bias the planner is using */.
Definition: PDST.h:125
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Cell * cell_
Pointer to the cell that contains this path.
Definition: PDST.h:187
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
The planner found an exact solution.
Definition: PlannerStatus.h:66
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:284
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:287
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:203
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:230
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:295
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:179
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
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition: BinaryHeap.h:202
The exception type for ompl.
Definition: Exception.h:47
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:259
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:238
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:393
Element * top() const
Return the top element. NULL for an empty heap.
Definition: BinaryHeap.h:115
void updateHeapElement(Motion *motion)
Either update heap after motion&#39;s priority has changed or insert motion into heap.
Definition: PDST.h:265
Cell is a Binary Space Partition.
Definition: PDST.h:196
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:289
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
*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
unsigned int size() const
Number of cells.
Definition: PDST.h:237
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
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:244
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:280
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
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
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:75
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:196
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:297
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:135
A boost shared pointer wrapper for ompl::base::Path.
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:185
base::State * endState_
The state reached by this motion.
Definition: PDST.h:177
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68