Loading...
Searching...
No Matches
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
40ompl::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
45ompl::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.
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_);
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
186void 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
248void 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();
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
310void 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);
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
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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,...
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...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
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:106
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
ompl::base::ProjectionEvaluatorPtr projectionEvaluator_
Projection evaluator for the problem.
Definition PDST.h:299
void updateHeapElement(Motion *motion)
Either update heap after motion's priority has changed or insert motion into heap.
Definition PDST.h:273
Cell * bsp_
Binary Space Partition.
Definition PDST.h:297
double goalBias_
Number between 0 and 1 specifying the probability with which the goal should be sampled.
Definition PDST.h:301
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
ompl::BinaryHeap< Motion *, MotionCompare > priorityQueue_
Priority queue of motions.
Definition PDST.h:295
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
unsigned int iteration_
Iteration number and priority of the next Motion that will be generated.
Definition PDST.h:305
void addMotion(Motion *motion, Cell *bsp, base::State *, Eigen::Ref< Eigen::VectorXd >)
Inserts the motion into the appropriate cell.
Definition PDST.cpp:186
ompl::base::StateSamplerPtr sampler_
State sampler.
Definition PDST.h:288
ompl::base::GoalSampleableRegion * goalSampler_
Objected used to sample the goal.
Definition PDST.h:303
Motion * lastGoalMotion_
Closest motion to the goal.
Definition PDST.h:307
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...
#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()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
Cell is a Binary Space Partition.
Definition PDST.h:202
Cell * right_
The right child cell (nullptr for a leaf cell)
Definition PDST.h:263
void addMotion(Motion *motion)
Add a motion.
Definition PDST.h:239
Cell * left_
The left child cell (nullptr for a leaf cell)
Definition PDST.h:261
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition PDST.h:226
double splitValue_
The midpoint between the bounds_ at the splitDimension_.
Definition PDST.h:259
unsigned int splitDimension_
Dimension along which the cell is split into smaller cells.
Definition PDST.h:257
ompl::base::RealVectorBounds bounds_
A bounding box for this cell.
Definition PDST.h:265
double volume_
Volume of the cell.
Definition PDST.h:255
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::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition PDST.h:194
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::base::State * endState_
The state reached by this motion.
Definition PDST.h:186