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