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 base::PlannerStatus(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 }
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:278
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:173
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:193
Definition of an abstract control.
Definition: Control.h:47
Class representing the tree of motions exploring the state space.
Definition: PDST.h:145
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...
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...
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:50
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:199
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
Cell * stab(const Eigen::Ref< Eigen::VectorXd > projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:231
#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
The planner found an exact solution.
Definition: PlannerStatus.h:66
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
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
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:244
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:189
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:385
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...
*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
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:240
void addMotion(Motion *motion, Cell *cell, base::State *, base::State *, Eigen::Ref< Eigen::VectorXd >, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cells, splitting the motion as necessary. motion is assumed t...
Definition: PDST.cpp:200
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:312
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:327
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