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  , bsp_(nullptr)
44  , goalBias_(0.05)
45  , goalSampler_(nullptr)
46  , iteration_(1)
47  , lastGoalMotion_(nullptr)
48 {
49  Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
50 }
51 
52 ompl::control::PDST::~PDST()
53 {
54  freeMemory();
55 }
56 
58 {
59  // Make sure the planner is configured correctly.
60  // ompl::base::Planner::checkValidity checks that there is at least one
61  // start state and an ompl::base::Goal object specified and throws an
62  // exception if this is not the case.
63  checkValidity();
64 
65  // depending on how the planning problem is set up, this may be necessary
66  bsp_->bounds_ = projectionEvaluator_->getBounds();
67  base::Goal *goal = pdef_->getGoal().get();
68  goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
69 
70  // Ensure that we have a state sampler AND a control sampler
71  if (!sampler_)
72  sampler_ = si_->allocStateSampler();
73  if (!controlSampler_)
74  controlSampler_ = siC_->allocDirectedControlSampler();
75 
76  // Initialize to correct values depending on whether or not previous calls to solve
77  // generated an approximate or exact solution. If solve is being called for the first
78  // time then initializes hasSolution to false and isApproximate to true.
79  double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
80  bool hasSolution = lastGoalMotion_ != nullptr;
81  bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
82  unsigned int ndim = projectionEvaluator_->getDimension();
83 
84  // If an exact solution has already been found, do not run for another iteration.
85  if (hasSolution && !isApproximate)
87 
88  // Initialize tree with start state(s)
89  while (const base::State *st = pis_.nextStart())
90  {
91  auto *startMotion = new Motion(si_->cloneState(st));
92  bsp_->addMotion(startMotion);
93  startMotion->heapElement_ = priorityQueue_.insert(startMotion);
94  }
95 
96  if (priorityQueue_.empty())
97  {
98  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
100  }
101 
102  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
103  priorityQueue_.size());
104 
105  base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
106  base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
107  while (!ptc)
108  {
109  // Get the top priority path.
110  Motion *motionSelected = priorityQueue_.top()->data;
111  motionSelected->updatePriority();
112  priorityQueue_.update(motionSelected->heapElement_);
113 
114  Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
115  if (newMotion == nullptr)
116  continue;
117 
118  addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
119 
120  // Check if the newMotion reached the goal.
121  hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
122  if (hasSolution)
123  {
124  closestDistanceToGoal = distanceToGoal;
125  lastGoalMotion_ = newMotion;
126  isApproximate = false;
127  break;
128  }
129  else if (distanceToGoal < closestDistanceToGoal)
130  {
131  closestDistanceToGoal = distanceToGoal;
132  lastGoalMotion_ = newMotion;
133  }
134 
135  // subdivide cell that contained selected motion, put motions of that
136  // cell in subcells and split motions so that they contained within
137  // one subcell
138  Cell *cellSelected = motionSelected->cell_;
139  std::vector<Motion *> motions;
140  cellSelected->subdivide(ndim);
141  motions.swap(cellSelected->motions_);
142  for (auto &motion : motions)
143  addMotion(motion, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
144  }
145 
146  if (lastGoalMotion_ != nullptr)
147  hasSolution = true;
148 
149  // If a solution path has been computed, save it in the problem definition object.
150  if (hasSolution)
151  {
152  Motion *m;
153  std::vector<unsigned int> durations(
154  1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
155  std::vector<Motion *> mpath(1, m);
156 
157  while (m->parent_)
158  {
159  durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
160  mpath.push_back(m);
161  }
162 
163  auto path(std::make_shared<PathControl>(si_));
164  double dt = siC_->getPropagationStepSize();
165  path->append(mpath.back()->endState_);
166  for (int i = (int)mpath.size() - 2; i > 0; i--)
167  path->append(mpath[i - 1]->startState_, mpath[i]->control_, durations[i] * dt);
168  path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
169  pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
170  }
171 
172  si_->freeState(tmpState1);
173  si_->freeState(tmpState2);
174 
175  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
176 
177  return base::PlannerStatus(hasSolution, isApproximate);
178 }
179 
181 {
182  // sample a point along the trajectory given by motion
183  unsigned int prevDuration = motion->controlDuration_;
184  if (motion->controlDuration_ > 1)
185  prevDuration = rng_.uniformInt(1, motion->controlDuration_);
186  if (prevDuration == motion->controlDuration_)
187  start = motion->endState_;
188  else
189  siC_->propagate(motion->startState_, motion->control_, prevDuration, start);
190  // generate a random state
191  if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
192  goalSampler_->sampleGoal(rnd);
193  else
194  sampler_->sampleUniform(rnd);
195  // generate a random control
196  Control *control = siC_->allocControl();
197  unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd);
198  // return new motion if duration is large enough
199  if (duration < siC_->getMinControlDuration())
200  {
201  siC_->freeControl(control);
202  return nullptr;
203  }
204  return new Motion(si_->cloneState(start), si_->cloneState(rnd), control, duration, ++iteration_, motion);
205 }
206 
207 void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state,
209 {
210  // If the motion is at most 1 step, then it cannot be split across cell bounds.
211  if (motion->controlDuration_ <= 1)
212  {
213  projectionEvaluator_->project(motion->endState_, proj);
214  bsp->stab(proj)->addMotion(motion);
215  updateHeapElement(motion);
216  return;
217  }
218 
219  Cell *cell = nullptr, *prevCell = nullptr;
220  si_->copyState(prevState, motion->startState_);
221  // propagate the motion, check for cell boundary crossings, and split as necessary
222  for (unsigned int i = 0, duration = 0; i < motion->controlDuration_ - 1; ++i, ++duration)
223  {
224  siC_->propagate(prevState, motion->control_, 1, state);
225  projectionEvaluator_->project(state, proj);
226  cell = bsp->stab(proj);
227  if (duration > 0 && cell != prevCell)
228  {
229  auto *newMotion = new Motion(motion->startState_, si_->cloneState(prevState), motion->control_, duration,
230  motion->priority_, motion->parent_);
231  newMotion->isSplit_ = true;
232  prevCell->addMotion(newMotion);
233  updateHeapElement(newMotion);
234  motion->startState_ = newMotion->endState_;
235  motion->controlDuration_ -= duration;
236  motion->parent_ = newMotion;
237  duration = 0;
238  }
239  std::swap(state, prevState);
240  std::swap(cell, prevCell);
241  proj.swap(prevProj);
242  }
243  prevCell->addMotion(motion);
244  updateHeapElement(motion);
245 }
246 
248  Motion *&ancestor) const
249 {
250  const double eps = std::numeric_limits<float>::epsilon();
251  unsigned int duration;
252  ancestor = motion;
253  if (state == motion->endState_ || motion->controlDuration_ == 0 || si_->distance(motion->endState_, state) < eps)
254  duration = motion->controlDuration_;
255  else if (motion->controlDuration_ > 0 && si_->distance(motion->startState_, state) < eps)
256  duration = 0;
257  else
258  {
259  duration = 1;
260  si_->copyState(scratch, motion->startState_);
261  for (; duration <= motion->controlDuration_; ++duration)
262  {
263  siC_->propagate(scratch, motion->control_, 1, scratch);
264  if (si_->distance(scratch, state) < eps)
265  break;
266  }
267  }
268  if (duration <= motion->controlDuration_)
269  {
270  // ancestor points to the start of a split motion; duration is computed
271  // relative to start state of ancestor motion
272  while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
273  {
274  ancestor = ancestor->parent_;
275  duration += ancestor->controlDuration_;
276  }
277  return duration;
278  }
279  // motion is no longer the parent of the motion starting at
280  // state because it has been split afterwards, so look for
281  // the starting point in the parent of motion.
282  return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
283 }
284 
286 {
287  Planner::clear();
288  sampler_.reset();
289  controlSampler_.reset();
290  iteration_ = 1;
291  lastGoalMotion_ = nullptr;
292  freeMemory();
293  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
294 }
295 
296 void ompl::control::PDST::freeMemory()
297 {
298  // Iterate over the elements in the priority queue and clear it
299  std::vector<Motion *> motions;
300  motions.reserve(priorityQueue_.size());
301  priorityQueue_.getContent(motions);
302  for (auto &motion : motions)
303  {
304  if (motion->startState_ != motion->endState_)
305  si_->freeState(motion->startState_);
306  if (!motion->isSplit_)
307  {
308  si_->freeState(motion->endState_);
309  if (motion->control_)
310  siC_->freeControl(motion->control_);
311  }
312  delete motion;
313  }
314  priorityQueue_.clear(); // clears the Element objects in the priority queue
315  delete bsp_;
316  bsp_ = nullptr;
317 }
318 
320 {
321  Planner::setup();
324  if (!projectionEvaluator_->hasBounds())
325  projectionEvaluator_->inferBounds();
326  if (!projectionEvaluator_->hasBounds())
327  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
328  if (bsp_)
329  delete bsp_;
330  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
331  lastGoalMotion_ = nullptr;
332 }
333 
335 {
337 
338  double dt = siC_->getPropagationStepSize();
339  base::State *scratch = si_->allocState();
340  std::vector<Motion *> motions;
341  motions.reserve(priorityQueue_.size());
342  priorityQueue_.getContent(motions);
343 
344  // Add goal vertex
345  if (lastGoalMotion_ != nullptr)
347 
348  for (auto &motion : motions)
349  if (!motion->isSplit_)
350  {
351  // We only add one edge for each motion that has been split into smaller segments
352  Motion *cur = motion, *ancestor;
353  unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
354 
355  if (cur->parent_ == nullptr)
357  else if (data.hasControls())
358  {
359  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_),
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_), base::PlannerDataVertex(cur->endState_));
376  if (ancestor->parent_)
377  {
378  // Include an edge between start state of parent ancestor motion and
379  // the start state of the ancestor motion, which lies somewhere on
380  // the parent ancestor motion.
381  cur = ancestor;
382  findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
383  data.addEdge(base::PlannerDataVertex(ancestor->startState_),
385  }
386  }
387  }
388 
389  si_->freeState(scratch);
390 }
391 
392 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
393 {
394  double childVolume = .5 * volume_;
395  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
396  splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
397 
398  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
399  left_->bounds_.high[splitDimension_] = splitValue_;
400  left_->motions_.reserve(motions_.size());
401  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
402  right_->bounds_.low[splitDimension_] = splitValue_;
403  right_->motions_.reserve(motions_.size());
404 }
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:285
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:180
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:111
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:57
void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:192
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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:194
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:185
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:207
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:421
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:176
const State * nextStart()
Return the next valid start state or nullptr 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:206
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:392
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:119
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:247
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:288
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:319
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:334
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:200
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:139
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