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 {
43  Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
44 }
45 
46 ompl::geometric::PDST::~PDST()
47 {
48  freeMemory();
49 }
50 
52 {
53  // Make sure the planner is configured correctly.
54  // ompl::base::Planner::checkValidity checks that there is at least one
55  // start state and an ompl::base::Goal object specified and throws an
56  // exception if this is not the case.
57  checkValidity();
58 
59  if (bsp_ == nullptr)
60  throw Exception("PDST was not set up.");
61 
62  // depending on how the planning problem is set up, this may be necessary
63  bsp_->bounds_ = projectionEvaluator_->getBounds();
64 
65  base::Goal *goal = pdef_->getGoal().get();
66  goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
67 
68  // Ensure that we have a state sampler
69  if (!sampler_)
70  sampler_ = si_->allocStateSampler();
71 
72  // Initialize to correct values depending on whether or not previous calls to solve
73  // generated an approximate or exact solution. If solve is being called for the first
74  // time then initializes hasSolution to false and isApproximate to true.
75  double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
76  bool hasSolution = lastGoalMotion_ != nullptr;
77  bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
78  unsigned ndim = projectionEvaluator_->getDimension();
79 
80  // If an exact solution has already been found, do not run for another iteration.
81  if (hasSolution && !isApproximate)
83 
84  // Initialize tree with start state(s)
85  while (const base::State *st = pis_.nextStart())
86  {
87  auto *startMotion = new Motion(si_->cloneState(st));
88  bsp_->addMotion(startMotion);
89  startMotion->heapElement_ = priorityQueue_.insert(startMotion);
90  }
91 
92  if (priorityQueue_.empty())
93  {
94  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
96  }
97 
98  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
100 
101  base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
102  base::EuclideanProjection tmpProj(ndim);
103  while (!ptc)
104  {
105  // Get the top priority path.
106  Motion *motionSelected = priorityQueue_.top()->data;
107  motionSelected->updatePriority();
108  priorityQueue_.update(motionSelected->heapElement_);
109 
110  Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
111  addMotion(newMotion, bsp_, tmpState1, tmpProj);
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  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, tmpProj);
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  auto path(std::make_shared<PathGeometric>(si_));
146 
147  // Compute the path by going up the tree of motions.
148  std::vector<base::State *> spath(1, lastGoalMotion_->endState_);
149  Motion *m = lastGoalMotion_;
150  while (m != nullptr)
151  {
152  m = m->ancestor();
153  spath.push_back(m->startState_);
154  m = m->parent_;
155  }
156 
157  // Add the solution path in order from the start state to the goal.
158  for (auto rIt = spath.rbegin(); rIt != spath.rend(); ++rIt)
159  path->append((*rIt));
160  pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
161  }
162 
163  si_->freeState(tmpState1);
164  si_->freeState(tmpState2);
165 
166  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
167 
168  return base::PlannerStatus(hasSolution, isApproximate);
169 }
170 
172  base::State *rnd)
173 {
174  // sample a point along the trajectory given by motion
175  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, rng_.uniform01(), start);
176  // generate a random state
177  if ((goalSampler_ != nullptr) && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
178  goalSampler_->sampleGoal(rnd);
179  else
180  sampler_->sampleUniform(rnd);
181  // compute longest valid segment from start towards rnd
182  std::pair<base::State *, double> lastValid = std::make_pair(rnd, 0.);
183  si_->checkMotion(start, rnd, lastValid);
184  return new Motion(si_->cloneState(start), si_->cloneState(rnd), ++iteration_, motion);
185 }
186 
188 {
189  projectionEvaluator_->project(motion->endState_, proj);
190  bsp->stab(proj)->addMotion(motion);
191  updateHeapElement(motion);
192 
193  // If the motion corresponds to a start state, then it cannot be split across cell
194  // bounds. So we're done.
195  if (motion->parent_ == nullptr)
196  return;
197 
198  // Otherwise, split into smaller segments s.t. endpoints project into same cell
199  int numSegments = si_->getStateSpace()->validSegmentCount(motion->startState_, motion->endState_);
200  Cell *startCell;
201  projectionEvaluator_->project(motion->startState_, proj);
202  startCell = bsp->stab(proj);
203 
204  while (startCell != motion->cell_ && numSegments > 1)
205  {
206  Cell *nextStartCell = motion->cell_, *cell;
207  int i = 0, j = numSegments, k = 1;
208  // Find the largest k such that the interpolated state at k/numSegments is
209  // still in startCell. The variables i and j bracket the range that k
210  // can be in.
211  while (j - i > 1)
212  {
213  k = (i + j) / 2;
214  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, (double)k / (double)numSegments,
215  state);
216  projectionEvaluator_->project(state, proj);
217  cell = bsp->stab(proj);
218  if (cell == startCell)
219  i = k;
220  else
221  {
222  j = k;
223  nextStartCell = cell;
224  }
225  }
226 
227  auto *m = new Motion(motion->startState_, si_->cloneState(state), motion->priority_, motion->parent_);
228  startCell->addMotion(m);
229  m->heapElement_ = priorityQueue_.insert(m);
230  m->isSplit_ = true;
231  motion->startState_ = m->endState_;
232  motion->parent_ = m;
233  numSegments -= k;
234  startCell = nextStartCell;
235  }
236 }
237 
239 {
240  Planner::clear();
241  sampler_.reset();
242  iteration_ = 1;
243  lastGoalMotion_ = nullptr;
244  freeMemory();
245  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
246 }
247 
248 void ompl::geometric::PDST::freeMemory()
249 {
250  // Iterate over the elements in the priority queue and clear it
251  std::vector<Motion *> motions;
252  motions.reserve(priorityQueue_.size());
253  priorityQueue_.getContent(motions);
254  for (auto it = motions.begin(); it < motions.end(); ++it)
255  {
256  if ((*it)->startState_ != (*it)->endState_)
257  si_->freeState((*it)->startState_);
258  if (!(*it)->isSplit_)
259  si_->freeState((*it)->endState_);
260  delete *it;
261  }
262  priorityQueue_.clear(); // clears the Element objects in the priority queue
263  delete bsp_;
264  bsp_ = nullptr;
265 }
266 
268 {
269  Planner::setup();
272  if (!projectionEvaluator_->hasBounds())
273  projectionEvaluator_->inferBounds();
274  if (!projectionEvaluator_->hasBounds())
275  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
276  if (bsp_ != nullptr)
277  delete bsp_;
278  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
279  lastGoalMotion_ = nullptr;
280 }
281 
283 {
285 
286  std::vector<Motion *> motions;
287  priorityQueue_.getContent(motions);
288 
289  // Add goal vertex
290  if (lastGoalMotion_ != nullptr)
292 
293  for (auto it = motions.begin(); it < motions.end(); ++it)
294  if (!(*it)->isSplit_)
295  {
296  Motion *cur = *it, *ancestor = cur->ancestor();
297  if (cur->parent_ == nullptr)
299  else
300  {
301  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
302  // add edge connecting start state of ancestor's parent to start state of ancestor
303  if (ancestor->parent_ != nullptr)
304  data.addEdge(base::PlannerDataVertex(ancestor->parent_->ancestor()->startState_),
305  base::PlannerDataVertex(ancestor->startState_));
306  }
307  }
308 }
309 
310 void ompl::geometric::PDST::Cell::subdivide(unsigned int spaceDimension)
311 {
312  double childVolume = .5 * volume_;
313  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
315 
316  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
318  left_->motions_.reserve(motions_.size());
319  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
321  right_->motions_.reserve(motions_.size());
322 }
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:175
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:112
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
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:409
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:238
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
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:282
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:310
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:412
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
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:207
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:120
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:267
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
void addMotion(Motion *motion, Cell *bsp, base::State *, base::EuclideanProjection &)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:187
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:406
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:51
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:201
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:140
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