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")
42  , siC_(si.get())
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_ != nullptr;
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  auto *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(),
99 
100  base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
101  base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
102  while (!ptc)
103  {
104  // Get the top priority path.
105  Motion *motionSelected = priorityQueue_.top()->data;
106  motionSelected->updatePriority();
107  priorityQueue_.update(motionSelected->heapElement_);
108 
109  Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
110  if (newMotion == nullptr)
111  continue;
112 
113  addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
114 
115  // Check if the newMotion reached the goal.
116  hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
117  if (hasSolution)
118  {
119  closestDistanceToGoal = distanceToGoal;
120  lastGoalMotion_ = newMotion;
121  isApproximate = false;
122  break;
123  }
124  else if (distanceToGoal < closestDistanceToGoal)
125  {
126  closestDistanceToGoal = distanceToGoal;
127  lastGoalMotion_ = newMotion;
128  }
129 
130  // subdivide cell that contained selected motion, put motions of that
131  // cell in subcells and split motions so that they contained within
132  // one subcell
133  Cell *cellSelected = motionSelected->cell_;
134  std::vector<Motion *> motions;
135  cellSelected->subdivide(ndim);
136  motions.swap(cellSelected->motions_);
137  for (auto &motion : motions)
138  addMotion(motion, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
139  }
140 
141  if (lastGoalMotion_ != nullptr)
142  hasSolution = true;
143 
144  // If a solution path has been computed, save it in the problem definition object.
145  if (hasSolution)
146  {
147  Motion *m;
148  std::vector<unsigned int> durations(
149  1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
150  std::vector<Motion *> mpath(1, m);
151 
152  while (m->parent_)
153  {
154  durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
155  mpath.push_back(m);
156  }
157 
158  auto path(std::make_shared<PathControl>(si_));
159  double dt = siC_->getPropagationStepSize();
160  path->append(mpath.back()->endState_);
161  for (int i = (int)mpath.size() - 2; i > 0; i--)
162  path->append(mpath[i - 1]->startState_, mpath[i]->control_, durations[i] * dt);
163  path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
164  pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
165  }
166 
167  si_->freeState(tmpState1);
168  si_->freeState(tmpState2);
169 
170  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
171 
172  return base::PlannerStatus(hasSolution, isApproximate);
173 }
174 
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 nullptr;
198  }
199  return new Motion(si_->cloneState(start), si_->cloneState(rnd), control, duration, ++iteration_, motion);
200 }
201 
202 void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state,
204 {
205  // If the motion is at most 1 step, then it cannot be split across cell bounds.
206  if (motion->controlDuration_ <= 1)
207  {
208  projectionEvaluator_->project(motion->endState_, proj);
209  bsp->stab(proj)->addMotion(motion);
210  updateHeapElement(motion);
211  return;
212  }
213 
214  Cell *cell = nullptr, *prevCell = nullptr;
215  si_->copyState(prevState, motion->startState_);
216  // propagate the motion, check for cell boundary crossings, and split as necessary
217  for (unsigned int i = 0, duration = 0; i < motion->controlDuration_ - 1; ++i, ++duration)
218  {
219  siC_->propagate(prevState, motion->control_, 1, state);
220  projectionEvaluator_->project(state, proj);
221  cell = bsp->stab(proj);
222  if (duration > 0 && cell != prevCell)
223  {
224  auto *newMotion = new Motion(motion->startState_, si_->cloneState(prevState), motion->control_, duration,
225  motion->priority_, motion->parent_);
226  newMotion->isSplit_ = true;
227  prevCell->addMotion(newMotion);
228  updateHeapElement(newMotion);
229  motion->startState_ = newMotion->endState_;
230  motion->controlDuration_ -= duration;
231  motion->parent_ = newMotion;
232  duration = 0;
233  }
234  std::swap(state, prevState);
235  std::swap(cell, prevCell);
236  proj.swap(prevProj);
237  }
238  prevCell->addMotion(motion);
239  updateHeapElement(motion);
240 }
241 
243  Motion *&ancestor) const
244 {
245  const double eps = std::numeric_limits<float>::epsilon();
246  unsigned int duration;
247  ancestor = motion;
248  if (state == motion->endState_ || motion->controlDuration_ == 0 || si_->distance(motion->endState_, state) < eps)
249  duration = motion->controlDuration_;
250  else if (motion->controlDuration_ > 0 && si_->distance(motion->startState_, state) < eps)
251  duration = 0;
252  else
253  {
254  duration = 1;
255  si_->copyState(scratch, motion->startState_);
256  for (; duration <= motion->controlDuration_; ++duration)
257  {
258  siC_->propagate(scratch, motion->control_, 1, scratch);
259  if (si_->distance(scratch, state) < eps)
260  break;
261  }
262  }
263  if (duration <= motion->controlDuration_)
264  {
265  // ancestor points to the start of a split motion; duration is computed
266  // relative to start state of ancestor motion
267  while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
268  {
269  ancestor = ancestor->parent_;
270  duration += ancestor->controlDuration_;
271  }
272  return duration;
273  }
274  // motion is no longer the parent of the motion starting at
275  // state because it has been split afterwards, so look for
276  // the starting point in the parent of motion.
277  return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
278 }
279 
281 {
282  Planner::clear();
283  sampler_.reset();
284  controlSampler_.reset();
285  iteration_ = 1;
286  lastGoalMotion_ = nullptr;
287  freeMemory();
288  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
289 }
290 
291 void ompl::control::PDST::freeMemory()
292 {
293  // Iterate over the elements in the priority queue and clear it
294  std::vector<Motion *> motions;
295  motions.reserve(priorityQueue_.size());
296  priorityQueue_.getContent(motions);
297  for (auto &motion : motions)
298  {
299  if (motion->startState_ != motion->endState_)
300  si_->freeState(motion->startState_);
301  if (!motion->isSplit_)
302  {
303  si_->freeState(motion->endState_);
304  if (motion->control_)
305  siC_->freeControl(motion->control_);
306  }
307  delete motion;
308  }
309  priorityQueue_.clear(); // clears the Element objects in the priority queue
310  delete bsp_;
311  bsp_ = nullptr;
312 }
313 
315 {
316  Planner::setup();
319  if (!projectionEvaluator_->hasBounds())
320  projectionEvaluator_->inferBounds();
321  if (!projectionEvaluator_->hasBounds())
322  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
323  if (bsp_)
324  delete bsp_;
325  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
326  lastGoalMotion_ = nullptr;
327 }
328 
330 {
332 
333  double dt = siC_->getPropagationStepSize();
334  base::State *scratch = si_->allocState();
335  std::vector<Motion *> motions;
336  motions.reserve(priorityQueue_.size());
337  priorityQueue_.getContent(motions);
338 
339  // Add goal vertex
340  if (lastGoalMotion_ != nullptr)
342 
343  for (auto &motion : motions)
344  if (!motion->isSplit_)
345  {
346  // We only add one edge for each motion that has been split into smaller segments
347  Motion *cur = motion, *ancestor;
348  unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
349 
350  if (cur->parent_ == nullptr)
352  else if (data.hasControls())
353  {
354  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_),
355  PlannerDataEdgeControl(cur->control_, duration * dt));
356  if (ancestor->parent_)
357  {
358  // Include an edge between start state of parent ancestor motion and
359  // the start state of the ancestor motion, which lies somewhere on
360  // the parent ancestor motion.
361  cur = ancestor;
362  duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
363  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
365  PlannerDataEdgeControl(ancestor->control_, duration * dt));
366  }
367  }
368  else
369  {
370  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
371  if (ancestor->parent_)
372  {
373  // Include an edge between start state of parent ancestor motion and
374  // the start state of the ancestor motion, which lies somewhere on
375  // the parent ancestor motion.
376  cur = ancestor;
377  findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
378  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
380  }
381  }
382  }
383 
384  si_->freeState(scratch);
385 }
386 
387 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
388 {
389  double childVolume = .5 * volume_;
390  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
391  splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
392 
393  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
394  left_->bounds_.high[splitDimension_] = splitValue_;
395  left_->motions_.reserve(motions_.size());
396  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
397  right_->bounds_.low[splitDimension_] = splitValue_;
398  right_->motions_.reserve(motions_.size());
399 }
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:186
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:295
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:191
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:280
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:307
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:193
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:265
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
Definition of an abstract control.
Definition: Control.h:47
Class representing the tree of motions exploring the state space.
Definition: PDST.h:145
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:299
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:301
Abstract definition of goals.
Definition: Goal.h:62
base::State * startState_
The starting point of this motion.
Definition: PDST.h:185
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:231
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
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
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:199
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
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:119
double getGoalBias() const
Get the goal bias the planner is using */.
Definition: PDST.h:121
#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:197
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
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 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...
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.
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:49
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:101
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:239
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:412
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:303
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:189
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:171
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition: BinaryHeap.h:207
The exception type for ompl.
Definition: Exception.h:46
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:387
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
void updateHeapElement(Motion *motion)
Either update heap after motion&#39;s priority has changed or insert motion into heap.
Definition: PDST.h:273
Cell is a Binary Space Partition.
Definition: PDST.h:206
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:297
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
*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:116
unsigned int size() const
Number of cells.
Definition: PDST.h:246
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
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
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:288
void addMotion(Motion *motion, Cell *bsp, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:187
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
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:81
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:201
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:305
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
base::State * endState_
The state reached by this motion.
Definition: PDST.h:187
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68