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 }
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
unsigned int size() const
Number of cells.
Definition: PDST.h:342
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:314
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:280
Definition of an abstract control.
Definition: Control.h:111
A shared pointer wrapper for ompl::base::SpaceInformation.
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:184
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:124
base::State * endState_
The state reached by this motion.
Definition: PDST.h:283
void addMotion(Motion *motion, Cell *bsp, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:187
Definition of an abstract state.
Definition: State.h:113
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
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:473
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:289
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:384
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:397
void clear()
Clear the heap.
Definition: BinaryHeap.h:176
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:259
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:387
base::State * startState_
The starting point of this motion.
Definition: PDST.h:281
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:401
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:393
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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:271
Cell * cell_
Pointer to the cell that contains this path.
Definition: PDST.h:293
Cell * stab(const base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:327
A class to store the exit status of Planner::solve()
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:399
Cell is a Binary Space Partition.
Definition: PDST.h:302
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:403
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:295
Class representing the tree of motions exploring the state space.
Definition: PDST.h:241
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
Abstract definition of goals.
Definition: Goal.h:126
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:335
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:329
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
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:204
@ EXACT_SOLUTION
The planner found an exact solution.
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:282
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
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:361
void setGoalBias(double goalBias)
In the process of randomly selecting states in the state space to attempt to go towards,...
Definition: PDST.h:212
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:476
double getGoalBias() const
Get the goal bias the planner is using *‍/.
Definition: PDST.h:217
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:470
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...
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 updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition: PDST.h:369
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:285
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:250
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:395
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...
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 a goal region that can be sampled.
double uniform01()
Generate a random real between 0 and 1.
The exception type for ompl.
Definition: Exception.h:78
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
@ INVALID_START
Invalid start state or no start state specified.
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:368
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:291
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:265
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:391
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
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
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:287