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/control/planners/pdst/PDST.h"
39
40ompl::control::PDST::PDST(const SpaceInformationPtr &si) : base::Planner(si, "PDST"), siC_(si.get())
41{
42 Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
43}
44
45ompl::control::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 // depending on how the planning problem is set up, this may be necessary
59 bsp_->bounds_ = projectionEvaluator_->getBounds();
60 base::Goal *goal = pdef_->getGoal().get();
61 goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
62
63 // Ensure that we have a state sampler AND a control sampler
64 if (!sampler_)
65 sampler_ = si_->allocStateSampler();
66 if (!controlSampler_)
67 controlSampler_ = siC_->allocDirectedControlSampler();
68
69 // Initialize to correct values depending on whether or not previous calls to solve
70 // generated an approximate or exact solution. If solve is being called for the first
71 // time then initializes hasSolution to false and isApproximate to true.
72 double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
73 bool hasSolution = lastGoalMotion_ != nullptr;
74 bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
75 unsigned int ndim = projectionEvaluator_->getDimension();
76
77 // If an exact solution has already been found, do not run for another iteration.
78 if (hasSolution && !isApproximate)
80
81 // Initialize tree with start state(s)
82 while (const base::State *st = pis_.nextStart())
83 {
84 auto *startMotion = new Motion(si_->cloneState(st));
85 bsp_->addMotion(startMotion);
86 startMotion->heapElement_ = priorityQueue_.insert(startMotion);
87 }
88
89 if (priorityQueue_.empty())
90 {
91 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
93 }
94
95 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
96 priorityQueue_.size());
97
98 base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
99 Eigen::VectorXd tmpProj1(ndim), tmpProj2(ndim);
100 while (!ptc)
101 {
102 // Get the top priority path.
103 Motion *motionSelected = priorityQueue_.top()->data;
104 motionSelected->updatePriority();
105 priorityQueue_.update(motionSelected->heapElement_);
106
107 Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
108 if (newMotion == nullptr)
109 continue;
110
111 addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
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 else 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, tmpState2, tmpProj1, tmpProj2);
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 Motion *m;
146 std::vector<unsigned int> durations(
147 1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
148 std::vector<Motion *> mpath(1, m);
149
150 while (m->parent_)
151 {
152 durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
153 mpath.push_back(m);
154 }
155
156 auto path(std::make_shared<PathControl>(si_));
157 double dt = siC_->getPropagationStepSize();
158 path->append(mpath.back()->endState_);
159 for (int i = (int)mpath.size() - 2; i > 0; i--)
160 path->append(mpath[i - 1]->startState_, mpath[i]->control_, durations[i] * dt);
161 path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
162 pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
163 }
164
165 si_->freeState(tmpState1);
166 si_->freeState(tmpState2);
167
168 OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
169
170 return {hasSolution, isApproximate};
171}
172
174{
175 // sample a point along the trajectory given by motion
176 unsigned int prevDuration = motion->controlDuration_;
177 if (motion->controlDuration_ > 1)
178 prevDuration = rng_.uniformInt(1, motion->controlDuration_);
179 if (prevDuration == motion->controlDuration_)
180 start = motion->endState_;
181 else
182 siC_->propagate(motion->startState_, motion->control_, prevDuration, start);
183 // generate a random state
184 if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
185 goalSampler_->sampleGoal(rnd);
186 else
187 sampler_->sampleUniform(rnd);
188 // generate a random control
189 Control *control = siC_->allocControl();
190 unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd);
191 // return new motion if duration is large enough
192 if (duration < siC_->getMinControlDuration())
193 {
194 siC_->freeControl(control);
195 return nullptr;
196 }
197 return new Motion(si_->cloneState(start), si_->cloneState(rnd), control, duration, ++iteration_, motion);
198}
199
200void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state,
201 Eigen::Ref<Eigen::VectorXd> prevProj, Eigen::Ref<Eigen::VectorXd> proj)
202{
203 // If the motion is at most 1 step, then it cannot be split across cell bounds.
204 if (motion->controlDuration_ <= 1)
205 {
206 projectionEvaluator_->project(motion->endState_, proj);
207 bsp->stab(proj)->addMotion(motion);
208 updateHeapElement(motion);
209 return;
210 }
211
212 Cell *cell = nullptr, *prevCell = nullptr;
213 si_->copyState(prevState, motion->startState_);
214 // propagate the motion, check for cell boundary crossings, and split as necessary
215 for (unsigned int i = 0, duration = 0; i < motion->controlDuration_ - 1; ++i, ++duration)
216 {
217 siC_->propagate(prevState, motion->control_, 1, state);
218 projectionEvaluator_->project(state, proj);
219 cell = bsp->stab(proj);
220 if (duration > 0 && cell != prevCell)
221 {
222 auto *newMotion = new Motion(motion->startState_, si_->cloneState(prevState), motion->control_, duration,
223 motion->priority_, motion->parent_);
224 newMotion->isSplit_ = true;
225 prevCell->addMotion(newMotion);
226 updateHeapElement(newMotion);
227 motion->startState_ = newMotion->endState_;
228 motion->controlDuration_ -= duration;
229 motion->parent_ = newMotion;
230 duration = 0;
231 }
232 std::swap(state, prevState);
233 std::swap(cell, prevCell);
234 proj.swap(prevProj);
235 }
236 prevCell->addMotion(motion);
237 updateHeapElement(motion);
238}
239
241 Motion *&ancestor) const
242{
243 const double eps = std::numeric_limits<float>::epsilon();
244 unsigned int duration;
245 ancestor = motion;
246 if (state == motion->endState_ || motion->controlDuration_ == 0 || si_->distance(motion->endState_, state) < eps)
247 duration = motion->controlDuration_;
248 else if (motion->controlDuration_ > 0 && si_->distance(motion->startState_, state) < eps)
249 duration = 0;
250 else
251 {
252 duration = 1;
253 si_->copyState(scratch, motion->startState_);
254 for (; duration <= motion->controlDuration_; ++duration)
255 {
256 siC_->propagate(scratch, motion->control_, 1, scratch);
257 if (si_->distance(scratch, state) < eps)
258 break;
259 }
260 }
261 if (duration <= motion->controlDuration_)
262 {
263 // ancestor points to the start of a split motion; duration is computed
264 // relative to start state of ancestor motion
265 while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
266 {
267 ancestor = ancestor->parent_;
268 duration += ancestor->controlDuration_;
269 }
270 return duration;
271 }
272 // motion is no longer the parent of the motion starting at
273 // state because it has been split afterwards, so look for
274 // the starting point in the parent of motion.
275 return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
276}
277
279{
280 Planner::clear();
281 sampler_.reset();
282 controlSampler_.reset();
283 iteration_ = 1;
284 lastGoalMotion_ = nullptr;
285 freeMemory();
286 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
287}
288
289void ompl::control::PDST::freeMemory()
290{
291 // Iterate over the elements in the priority queue and clear it
292 std::vector<Motion *> motions;
293 motions.reserve(priorityQueue_.size());
294 priorityQueue_.getContent(motions);
295 for (auto &motion : motions)
296 {
297 if (motion->startState_ != motion->endState_)
298 si_->freeState(motion->startState_);
299 if (!motion->isSplit_)
300 {
301 si_->freeState(motion->endState_);
302 if (motion->control_)
303 siC_->freeControl(motion->control_);
304 }
305 delete motion;
306 }
307 priorityQueue_.clear(); // clears the Element objects in the priority queue
308 delete bsp_;
309 bsp_ = nullptr;
310}
311
313{
314 Planner::setup();
315 tools::SelfConfig sc(si_, getName());
316 sc.configureProjectionEvaluator(projectionEvaluator_);
317 if (!projectionEvaluator_->hasBounds())
318 projectionEvaluator_->inferBounds();
319 if (!projectionEvaluator_->hasBounds())
320 throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
321 if (bsp_)
322 delete bsp_;
323 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
324 lastGoalMotion_ = nullptr;
325}
326
328{
330
331 double dt = siC_->getPropagationStepSize();
332 base::State *scratch = si_->allocState();
333 std::vector<Motion *> motions;
334 motions.reserve(priorityQueue_.size());
335 priorityQueue_.getContent(motions);
336
337 // Add goal vertex
338 if (lastGoalMotion_ != nullptr)
339 data.addGoalVertex(lastGoalMotion_->endState_);
340
341 for (auto &motion : motions)
342 if (!motion->isSplit_)
343 {
344 // We only add one edge for each motion that has been split into smaller segments
345 Motion *cur = motion, *ancestor;
346 unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
347
348 if (cur->parent_ == nullptr)
350 else if (data.hasControls())
351 {
352 data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_),
353 PlannerDataEdgeControl(cur->control_, duration * dt));
354 if (ancestor->parent_)
355 {
356 // Include an edge between start state of parent ancestor motion and
357 // the start state of the ancestor motion, which lies somewhere on
358 // the parent ancestor motion.
359 cur = ancestor;
360 duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
361 data.addEdge(base::PlannerDataVertex(ancestor->startState_),
363 PlannerDataEdgeControl(ancestor->control_, duration * dt));
364 }
365 }
366 else
367 {
368 data.addEdge(base::PlannerDataVertex(ancestor->startState_), base::PlannerDataVertex(cur->endState_));
369 if (ancestor->parent_)
370 {
371 // Include an edge between start state of parent ancestor motion and
372 // the start state of the ancestor motion, which lies somewhere on
373 // the parent ancestor motion.
374 cur = ancestor;
375 findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
376 data.addEdge(base::PlannerDataVertex(ancestor->startState_),
378 }
379 }
380 }
381
382 si_->freeState(scratch);
383}
384
385void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
386{
387 double childVolume = .5 * volume_;
388 unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
389 splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
390
391 left_ = new Cell(childVolume, bounds_, nextSplitDimension);
392 left_->bounds_.high[splitDimension_] = splitValue_;
393 left_->motions_.reserve(motions_.size());
394 right_ = new Cell(childVolume, bounds_, nextSplitDimension);
395 right_->bounds_.low[splitDimension_] = splitValue_;
396 right_->motions_.reserve(motions_.size());
397}
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,...
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...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
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
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
Definition of an abstract control.
Definition: Control.h:48
unsigned int findDurationAndAncestor(Motion *motion, base::State *state, base::State *scratch, Motion *&ancestor) const
Find the max. duration that the control_ in motion can be applied s.t. the trajectory passes through ...
Definition: PDST.cpp:240
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PDST.cpp:312
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
void getPlannerData(base::PlannerData &data) const override
Extracts the planner data from the priority queue into data.
Definition: PDST.cpp:327
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PDST.cpp:278
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 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
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:61
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:207
Cell * stab(const Eigen::Ref< Eigen::VectorXd > &projection) const
Locates the cell that this motion begins in.
Definition: PDST.h:231
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Definition: PDST.cpp:385
void addMotion(Motion *motion)
Add a motion.
Definition: PDST.h:244
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
Definition: PDST.h:272
Class representing the tree of motions exploring the state space.
Definition: PDST.h:146
Motion * parent_
Parent motion from which this one started.
Definition: PDST.h:195
base::State * startState_
The starting point of this motion.
Definition: PDST.h:185
base::State * endState_
The state reached by this motion.
Definition: PDST.h:187
Cell * cell_
Pointer to the cell that contains this path.
Definition: PDST.h:197
double priority_
Priority for selecting this path to extend from in the future.
Definition: PDST.h:193
control::Control * control_
The control that was applied to arrive at this state from the parent.
Definition: PDST.h:189
BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
Definition: PDST.h:199
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Definition: PDST.h:191