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) : base::Planner(si, "PDST")
41 {
42  Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
43 }
44 
45 ompl::geometric::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  if (bsp_ == nullptr)
59  throw Exception("PDST was not set up.");
60 
61  // depending on how the planning problem is set up, this may be necessary
62  bsp_->bounds_ = projectionEvaluator_->getBounds();
63 
64  base::Goal *goal = pdef_->getGoal().get();
65  goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
66 
67  // Ensure that we have a state sampler
68  if (!sampler_)
69  sampler_ = si_->allocStateSampler();
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 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  Eigen::VectorXd tmpProj(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  addMotion(newMotion, bsp_, tmpState1, tmpProj);
111 
112  // Check if the newMotion reached the goal.
113  hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
114  if (hasSolution)
115  {
116  closestDistanceToGoal = distanceToGoal;
117  lastGoalMotion_ = newMotion;
118  isApproximate = false;
119  break;
120  }
121  if (distanceToGoal < closestDistanceToGoal)
122  {
123  closestDistanceToGoal = distanceToGoal;
124  lastGoalMotion_ = newMotion;
125  }
126 
127  // subdivide cell that contained selected motion, put motions of that
128  // cell in subcells and split motions so that they contained within
129  // one subcell
130  Cell *cellSelected = motionSelected->cell_;
131  std::vector<Motion *> motions;
132  cellSelected->subdivide(ndim);
133  motions.swap(cellSelected->motions_);
134  for (auto &motion : motions)
135  addMotion(motion, cellSelected, tmpState1, tmpProj);
136  }
137 
138  if (lastGoalMotion_ != nullptr)
139  hasSolution = true;
140 
141  // If a solution path has been computed, save it in the problem definition object.
142  if (hasSolution)
143  {
144  auto path(std::make_shared<PathGeometric>(si_));
145 
146  // Compute the path by going up the tree of motions.
147  std::vector<base::State *> spath(1, lastGoalMotion_->endState_);
148  Motion *m = lastGoalMotion_;
149  while (m != nullptr)
150  {
151  m = m->ancestor();
152  spath.push_back(m->startState_);
153  m = m->parent_;
154  }
155 
156  // Add the solution path in order from the start state to the goal.
157  for (auto rIt = spath.rbegin(); rIt != spath.rend(); ++rIt)
158  path->append((*rIt));
159  pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
160  }
161 
162  si_->freeState(tmpState1);
163  si_->freeState(tmpState2);
164 
165  OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
166 
167  return base::PlannerStatus(hasSolution, isApproximate);
168 }
169 
171  base::State *rnd)
172 {
173  // sample a point along the trajectory given by motion
174  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, rng_.uniform01(), start);
175  // generate a random state
176  if ((goalSampler_ != nullptr) && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
177  goalSampler_->sampleGoal(rnd);
178  else
179  sampler_->sampleUniform(rnd);
180  // compute longest valid segment from start towards rnd
181  std::pair<base::State *, double> lastValid = std::make_pair(rnd, 0.);
182  si_->checkMotion(start, rnd, lastValid);
183  return new Motion(si_->cloneState(start), si_->cloneState(rnd), ++iteration_, motion);
184 }
185 
186 void ompl::geometric::PDST::addMotion(Motion *motion, Cell *bsp, base::State *state, Eigen::Ref<Eigen::VectorXd> proj)
187 {
188  projectionEvaluator_->project(motion->endState_, proj);
189  bsp->stab(proj)->addMotion(motion);
190  updateHeapElement(motion);
191 
192  // If the motion corresponds to a start state, then it cannot be split across cell
193  // bounds. So we're done.
194  if (motion->parent_ == nullptr)
195  return;
196 
197  // Otherwise, split into smaller segments s.t. endpoints project into same cell
198  int numSegments = si_->getStateSpace()->validSegmentCount(motion->startState_, motion->endState_);
199  Cell *startCell;
200  projectionEvaluator_->project(motion->startState_, proj);
201  startCell = bsp->stab(proj);
202 
203  while (startCell != motion->cell_ && numSegments > 1)
204  {
205  Cell *nextStartCell = motion->cell_, *cell;
206  int i = 0, j = numSegments, k = 1;
207  // Find the largest k such that the interpolated state at k/numSegments is
208  // still in startCell. The variables i and j bracket the range that k
209  // can be in.
210  while (j - i > 1)
211  {
212  k = (i + j) / 2;
213  si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, (double)k / (double)numSegments,
214  state);
215  projectionEvaluator_->project(state, proj);
216  cell = bsp->stab(proj);
217  if (cell == startCell)
218  i = k;
219  else
220  {
221  j = k;
222  nextStartCell = cell;
223  }
224  }
225 
226  auto *m = new Motion(motion->startState_, si_->cloneState(state), motion->priority_, motion->parent_);
227  startCell->addMotion(m);
228  m->heapElement_ = priorityQueue_.insert(m);
229  m->isSplit_ = true;
230  motion->startState_ = m->endState_;
231  motion->parent_ = m;
232  numSegments -= k;
233  startCell = nextStartCell;
234  }
235 }
236 
238 {
239  Planner::clear();
240  sampler_.reset();
241  iteration_ = 1;
242  lastGoalMotion_ = nullptr;
243  freeMemory();
244  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
245 }
246 
247 void ompl::geometric::PDST::freeMemory()
248 {
249  // Iterate over the elements in the priority queue and clear it
250  std::vector<Motion *> motions;
251  motions.reserve(priorityQueue_.size());
252  priorityQueue_.getContent(motions);
253  for (auto &motion : motions)
254  {
255  if (motion->startState_ != motion->endState_)
256  si_->freeState(motion->startState_);
257  if (!motion->isSplit_)
258  si_->freeState(motion->endState_);
259  delete motion;
260  }
261  priorityQueue_.clear(); // clears the Element objects in the priority queue
262  delete bsp_;
263  bsp_ = nullptr;
264 }
265 
267 {
268  Planner::setup();
271  if (!projectionEvaluator_->hasBounds())
272  projectionEvaluator_->inferBounds();
273  if (!projectionEvaluator_->hasBounds())
274  throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
275  if (bsp_ != nullptr)
276  delete bsp_;
277  bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
278  lastGoalMotion_ = nullptr;
279 }
280 
282 {
284 
285  std::vector<Motion *> motions;
286  priorityQueue_.getContent(motions);
287 
288  // Add goal vertex
289  if (lastGoalMotion_ != nullptr)
291 
292  for (const auto &cur : motions)
293  if (!cur->isSplit_)
294  {
295  Motion *ancestor = cur->ancestor();
296  if (cur->parent_ == nullptr)
297  data.addStartVertex(base::PlannerDataVertex(cur->endState_));
298  else
299  {
300  data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
301  // add edge connecting start state of ancestor's parent to start state of ancestor
302  if (ancestor->parent_ != nullptr)
303  data.addEdge(base::PlannerDataVertex(ancestor->parent_->ancestor()->startState_),
305  }
306  }
307 }
308 
309 void ompl::geometric::PDST::Cell::subdivide(unsigned int spaceDimension)
310 {
311  double childVolume = .5 * volume_;
312  unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
313  splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
314 
315  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
316  left_->bounds_.high[splitDimension_] = splitValue_;
317  left_->motions_.reserve(motions_.size());
318  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
319  right_->bounds_.low[splitDimension_] = splitValue_;
320  right_->motions_.reserve(motions_.size());
321 }
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:173
Class representing the tree of motions exploring the state space.
Definition: PDST.h:142
void addMotion(Motion *motion, Cell *bsp, base::State *, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:186
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
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
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...
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:237
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:66
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
#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:281
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...
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:226
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:309
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:239
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:170
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:266
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 *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
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 * cell_
pointer to the cell that contains this path
Definition: PDST.h:192
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
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