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(),
98  priorityQueue_.size());
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 {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  if (projectionEvaluator_ && projectionEvaluator_->hasBounds())
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 &motion : motions)
255  {
256  if (motion->startState_ != motion->endState_)
257  si_->freeState(motion->startState_);
258  if (!motion->isSplit_)
259  si_->freeState(motion->endState_);
260  delete motion;
261  }
262  priorityQueue_.clear(); // clears the Element objects in the priority queue
263  delete bsp_;
264  bsp_ = nullptr;
265 }
266 
268 {
269  Planner::setup();
270  tools::SelfConfig sc(si_, getName());
271  sc.configureProjectionEvaluator(projectionEvaluator_);
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)
291  data.addGoalVertex(lastGoalMotion_->endState_);
292 
293  for (const auto &cur : motions)
294  if (!cur->isSplit_)
295  {
296  Motion *ancestor = cur->ancestor();
297  if (cur->parent_ == nullptr)
298  data.addStartVertex(base::PlannerDataVertex(cur->endState_));
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_),
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;
314  splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
315 
316  left_ = new Cell(childVolume, bounds_, nextSplitDimension);
317  left_->bounds_.high[splitDimension_] = splitValue_;
318  left_->motions_.reserve(motions_.size());
319  right_ = new Cell(childVolume, bounds_, nextSplitDimension);
320  right_->bounds_.low[splitDimension_] = splitValue_;
321  right_->motions_.reserve(motions_.size());
322 }
The exception type for ompl.
Definition: Exception.h:47
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition: Goal.h:63
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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:129
Definition of an abstract state.
Definition: State.h:50
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:267
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:282
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
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:237
void addMotion(Motion *motion, Cell *bsp, base::State *, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cell.
Definition: PDST.cpp:186
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
Cell is a Binary Space Partition.
Definition: PDST.h:202
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:239
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:226
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:310
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:267
Class representing the tree of motions exploring the state space.
Definition: PDST.h:143
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:188
ompl::base::State * startState_
The starting point of this motion.
Definition: PDST.h:184
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:190
Cell * cell_
pointer to the cell that contains this path
Definition: PDST.h:192
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:194
ompl::base::State * endState_
The state reached by this motion.
Definition: PDST.h:186