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/geometric/planners/pdst/PDST.h"
39 
40 ompl::geometric::PDST::PDST(const base::SpaceInformationPtr &si)
41  : base::Planner(si, "PDST")
42  , bsp_(nullptr)
43  , goalBias_(0.05)
44  , goalSampler_(nullptr)
45  , iteration_(1)
46  , lastGoalMotion_(nullptr)
47 {
48  Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
49 }
50 
51 ompl::geometric::PDST::~PDST()
52 {
53  freeMemory();
54 }
55 
57 {
58  // Make sure the planner is configured correctly.
59  // ompl::base::Planner::checkValidity checks that there is at least one
60  // start state and an ompl::base::Goal object specified and throws an
61  // exception if this is not the case.
62  checkValidity();
63 
64  if (!bsp_)
65  throw Exception("PDST was not set up.");
66 
67  // depending on how the planning problem is set up, this may be necessary
68  bsp_->bounds_ = projectionEvaluator_->getBounds();
69 
70  base::Goal *goal = pdef_->getGoal().get();
71  goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
72 
73  // Ensure that we have a state sampler
74  if (!sampler_)
75  sampler_ = si_->allocStateSampler();
76 
77  // Initialize to correct values depending on whether or not previous calls to solve
78  // generated an approximate or exact solution. If solve is being called for the first
79  // time then initializes hasSolution to false and isApproximate to true.
80  double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
81  bool hasSolution = lastGoalMotion_ != nullptr;
82  bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
83  unsigned ndim = projectionEvaluator_->getDimension();
84 
85  // If an exact solution has already been found, do not run for another iteration.
86  if (hasSolution && !isApproximate)
88 
89  // Initialize tree with start state(s)
90  while (const base::State *st = pis_.nextStart())
91  {
92  auto *startMotion = new Motion(si_->cloneState(st));
93  bsp_->addMotion(startMotion);
94  startMotion->heapElement_ = priorityQueue_.insert(startMotion);
95  }
96 
97  if (priorityQueue_.empty())
98  {
99  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
101  }
102 
103  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
104  priorityQueue_.size());
105 
106  base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
107  base::EuclideanProjection tmpProj(ndim);
108  while (!ptc)
109  {
110  // Get the top priority path.
111  Motion *motionSelected = priorityQueue_.top()->data;
112  motionSelected->updatePriority();
113  priorityQueue_.update(motionSelected->heapElement_);
114 
115  Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
116  addMotion(newMotion, bsp_, tmpState1, tmpProj);
117 
118  // Check if the newMotion reached the goal.
119  hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
120  if (hasSolution)
121  {
122  closestDistanceToGoal = distanceToGoal;
123  lastGoalMotion_ = newMotion;
124  isApproximate = false;
125  break;
126  }
127  else if (distanceToGoal < closestDistanceToGoal)
128  {
129  closestDistanceToGoal = distanceToGoal;
130  lastGoalMotion_ = newMotion;
131  }
132 
133  // subdivide cell that contained selected motion, put motions of that
134  // cell in subcells and split motions so that they contained within
135  // one subcell
136  Cell *cellSelected = motionSelected->cell_;
137  std::vector<Motion *> motions;
138  cellSelected->subdivide(ndim);
139  motions.swap(cellSelected->motions_);
140  for (auto &motion : motions)
141  addMotion(motion, cellSelected, tmpState1, tmpProj);
142  }
143 
144  if (lastGoalMotion_ != nullptr)
145  hasSolution = true;
146 
147  // If a solution path has been computed, save it in the problem definition object.
148  if (hasSolution)
149  {
150  auto path(std::make_shared<PathGeometric>(si_));
151 
152  // Compute the path by going up the tree of motions.
153  std::vector<base::State *> spath(1, lastGoalMotion_->endState_);
154  Motion *m = lastGoalMotion_;
155  while (m)
156  {
157  m = m->ancestor();
158  spath.push_back(m->startState_);
159  m = m->parent_;
160  }
161 
162  // Add the solution path in order from the start state to the goal.
163  for (auto rIt = spath.rbegin(); rIt != spath.rend(); ++rIt)
164  path->append((*rIt));
165  pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
166  }
167 
168  si_->freeState(tmpState1);
169  si_->freeState(tmpState2);
170 
171  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
172 
173  return base::PlannerStatus(hasSolution, isApproximate);
174 }
175 
177  base::State *rnd)
178 {
179  // sample a point along the trajectory given by motion
180  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, rng_.uniform01(), start);
181  // generate a random state
182  if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
183  goalSampler_->sampleGoal(rnd);
184  else
185  sampler_->sampleUniform(rnd);
186  // compute longest valid segment from start towards rnd
187  std::pair<base::State *, double> lastValid = std::make_pair(rnd, 0.);
188  si_->checkMotion(start, rnd, lastValid);
189  return new Motion(si_->cloneState(start), si_->cloneState(rnd), ++iteration_, motion);
190 }
191 
193 {
194  projectionEvaluator_->project(motion->endState_, proj);
195  bsp->stab(proj)->addMotion(motion);
196  updateHeapElement(motion);
197 
198  // If the motion corresponds to a start state, then it cannot be split across cell
199  // bounds. So we're done.
200  if (!motion->parent_)
201  return;
202 
203  // Otherwise, split into smaller segments s.t. endpoints project into same cell
204  int numSegments = si_->getStateSpace()->validSegmentCount(motion->startState_, motion->endState_);
205  Cell *startCell;
206  projectionEvaluator_->project(motion->startState_, proj);
207  startCell = bsp->stab(proj);
208 
209  while (startCell != motion->cell_ && numSegments > 1)
210  {
211  Cell *nextStartCell = motion->cell_, *cell;
212  int i = 0, j = numSegments, k = 1;
213  // Find the largest k such that the interpolated state at k/numSegments is
214  // still in startCell. The variables i and j bracket the range that k
215  // can be in.
216  while (j - i > 1)
217  {
218  k = (i + j) / 2;
219  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, (double)k / (double)numSegments,
220  state);
221  projectionEvaluator_->project(state, proj);
222  cell = bsp->stab(proj);
223  if (cell == startCell)
224  i = k;
225  else
226  {
227  j = k;
228  nextStartCell = cell;
229  }
230  }
231 
232  auto *m = new Motion(motion->startState_, si_->cloneState(state), motion->priority_, motion->parent_);
233  startCell->addMotion(m);
234  m->heapElement_ = priorityQueue_.insert(m);
235  m->isSplit_ = true;
236  motion->startState_ = m->endState_;
237  motion->parent_ = m;
238  numSegments -= k;
239  startCell = nextStartCell;
240  }
241 }
242 
244 {
245  Planner::clear();
246  sampler_.reset();
247  iteration_ = 1;
248  lastGoalMotion_ = nullptr;
249  freeMemory();
250  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
251 }
252 
253 void ompl::geometric::PDST::freeMemory()
254 {
255  // Iterate over the elements in the priority queue and clear it
256  std::vector<Motion *> motions;
257  motions.reserve(priorityQueue_.size());
258  priorityQueue_.getContent(motions);
259  for (auto it = motions.begin(); it < motions.end(); ++it)
260  {
261  if ((*it)->startState_ != (*it)->endState_)
262  si_->freeState((*it)->startState_);
263  if (!(*it)->isSplit_)
264  si_->freeState((*it)->endState_);
265  delete *it;
266  }
267  priorityQueue_.clear(); // clears the Element objects in the priority queue
268  delete bsp_;
269  bsp_ = nullptr;
270 }
271 
273 {
274  Planner::setup();
277  if (!projectionEvaluator_->hasBounds())
278  projectionEvaluator_->inferBounds();
279  if (!projectionEvaluator_->hasBounds())
280  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
281  if (bsp_)
282  delete bsp_;
283  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
284  lastGoalMotion_ = nullptr;
285 }
286 
288 {
290 
291  std::vector<Motion *> motions;
292  priorityQueue_.getContent(motions);
293 
294  // Add goal vertex
295  if (lastGoalMotion_ != nullptr)
297 
298  for (auto it = motions.begin(); it < motions.end(); ++it)
299  if (!(*it)->isSplit_)
300  {
301  Motion *cur = *it, *ancestor = cur->ancestor();
302  if (!cur->parent_)
304  else
305  {
306  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
307  // add edge connecting start state of ancestor's parent to start state of ancestor
308  if (ancestor->parent_)
309  data.addEdge(base::PlannerDataVertex(ancestor->parent_->ancestor()->startState_),
310  base::PlannerDataVertex(ancestor->startState_));
311  }
312  }
313 }
314 
315 void ompl::geometric::PDST::Cell::subdivide(unsigned int spaceDimension)
316 {
317  double childVolume = .5 * volume_;
318  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
320 
321  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
323  left_->motions_.reserve(motions_.size());
324  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
326  right_->motions_.reserve(motions_.size());
327 }
base::RealVectorBounds bounds_
A bounding box for this cell.
Definition: PDST.h:270
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:186
base::StateSamplerPtr sampler_
State sampler.
Definition: PDST.h:303
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
unsigned int size() const
Number of cells.
Definition: PDST.h:251
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
Class representing the tree of motions exploring the state space.
Definition: PDST.h:142
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition: PDST.h:324
Cell * bsp_
Binary Space Partition.
Definition: PDST.h:316
std::vector< double > low
Lower bound.
void clear()
Clear the heap.
Definition: BinaryHeap.h:111
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:190
BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition: PDST.h:314
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...
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:184
Abstract definition of goals.
Definition: Goal.h:62
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition: PDST.h:264
base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition: PDST.h:322
void addMotion(Motion *motion, Cell *cell, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:192
double getGoalBias() const
Get the goal bias the planner is using */.
Definition: PDST.h:124
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:267
Cell is a Binary Space Partition.
Definition: PDST.h:201
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.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:243
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:194
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
Motion * lastGoalMotion_
Closest motion to the goal.
Definition: PDST.h:326
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
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition: PDST.h:268
double volume_
Volume of the cell.
Definition: PDST.h:260
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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 getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:287
std::vector< double > high
Upper bound.
void updateHeapElement(Motion *motion)
Either update heap after motion&#39;s priority has changed or insert motion into heap.
Definition: PDST.h:280
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
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:244
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 subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:315
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
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
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:119
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:272
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition: PDST.h:318
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:194
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition: PDST.h:266
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:192
Cell * stab(const ompl::base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:226
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:56
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition: PDST.h:262
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:200
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:188
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:139
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition: PDST.h:320
base::State * endState_
The state reached by this motion.
Definition: PDST.h:187
*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:119
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68