37 #include "ompl/tools/config/SelfConfig.h"
38 #include "ompl/control/planners/pdst/PDST.h"
40 ompl::control::PDST::PDST(
const SpaceInformationPtr &si)
41 : base::Planner(si,
"PDST"), siC_(si.get()), bsp_(NULL), goalBias_(0.05),
42 goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL)
47 ompl::control::PDST::~PDST(
void)
61 bsp_->bounds_ = projectionEvaluator_->getBounds();
67 sampler_ = si_->allocStateSampler();
69 controlSampler_ = siC_->allocDirectedControlSampler();
74 double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
75 bool hasSolution = lastGoalMotion_ != NULL;
76 bool isApproximate = !hasSolution || !goal->
isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
77 unsigned int ndim = projectionEvaluator_->getDimension();
80 if (hasSolution && !isApproximate)
87 bsp_->addMotion(startMotion);
88 startMotion->
heapElement_ = priorityQueue_.insert(startMotion);
91 if (priorityQueue_.empty())
93 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
97 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), priorityQueue_.size());
99 base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
104 Motion *motionSelected = priorityQueue_.top()->data;
105 motionSelected->updatePriority();
108 Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
109 if (newMotion == NULL)
112 addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
118 closestDistanceToGoal = distanceToGoal;
119 lastGoalMotion_ = newMotion;
120 isApproximate =
false;
123 else if (distanceToGoal < closestDistanceToGoal)
125 closestDistanceToGoal = distanceToGoal;
126 lastGoalMotion_ = newMotion;
132 Cell *cellSelected = motionSelected->
cell_;
133 std::vector<Motion*> motions;
135 motions.swap(cellSelected->
motions_);
136 for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m)
137 addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
140 if (lastGoalMotion_ != NULL)
147 std::vector<unsigned int> durations(1,
148 findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
149 std::vector<Motion*> mpath(1, m);
153 durations.push_back(findDurationAndAncestor(m->
parent_, m->
startState_, tmpState1, m));
158 double dt = siC_->getPropagationStepSize();
159 path->
append(mpath.back()->endState_);
160 for (
int i = (
int) mpath.size() - 2; i > 0; i--)
161 path->
append(mpath[i-1]->startState_, mpath[i]->control_, durations[i] * dt);
162 path->
append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
163 pdef_->addSolutionPath(
base::PathPtr(path), isApproximate, closestDistanceToGoal);
166 si_->freeState(tmpState1);
167 si_->freeState(tmpState2);
169 OMPL_INFORM(
"%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
186 if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
187 goalSampler_->sampleGoal(rnd);
189 sampler_->sampleUniform(rnd);
191 Control *control = siC_->allocControl();
192 unsigned int duration = controlSampler_->sampleTo(control, motion->
control_, start, rnd);
194 if (duration < siC_->getMinControlDuration())
196 siC_->freeControl(control);
199 return new Motion(si_->cloneState(start), si_->cloneState(rnd),
200 control, duration, ++iteration_, motion);
209 projectionEvaluator_->project(motion->
endState_, proj);
211 updateHeapElement(motion);
215 Cell *cell = NULL, *prevCell = NULL;
218 for (
unsigned int i = 0, duration = 0 ; i < motion->
controlDuration_ - 1 ; ++i, ++duration)
220 siC_->propagate(prevState, motion->
control_, 1, state);
221 projectionEvaluator_->project(state, proj);
222 cell = bsp->
stab(proj);
223 if (duration > 0 && cell != prevCell)
228 prevCell->addMotion(newMotion);
229 updateHeapElement(newMotion);
235 std::swap(state, prevState);
236 std::swap(cell, prevCell);
239 prevCell->addMotion(motion);
240 updateHeapElement(motion);
247 const double eps = std::numeric_limits<float>::epsilon();
248 unsigned int duration;
251 si_->distance(motion->
endState_, state) < eps)
262 siC_->propagate(scratch, motion->
control_, 1, scratch);
263 if (si_->distance(scratch, state) < eps)
267 if (duration <= motion->controlDuration_)
281 return findDurationAndAncestor(motion->
parent_, state, scratch, ancestor);
288 controlSampler_.reset();
290 lastGoalMotion_ = NULL;
292 bsp_ =
new Cell(1., projectionEvaluator_->getBounds(), 0);
295 void ompl::control::PDST::freeMemory(
void)
298 std::vector<Motion*> motions;
299 motions.reserve(priorityQueue_.size());
300 priorityQueue_.getContent(motions);
301 for (std::vector<Motion*>::iterator it = motions.begin() ; it < motions.end() ; ++it)
303 if ((*it)->startState_ != (*it)->endState_)
304 si_->freeState((*it)->startState_);
305 if (!(*it)->isSplit_)
307 si_->freeState((*it)->endState_);
309 siC_->freeControl((*it)->control_);
313 priorityQueue_.clear();
323 if (!projectionEvaluator_->hasBounds())
324 projectionEvaluator_->inferBounds();
325 if (!projectionEvaluator_->hasBounds())
326 throw Exception(
"PDST requires a projection evaluator that specifies bounds for the projected space");
329 bsp_ =
new Cell(1., projectionEvaluator_->getBounds(), 0);
330 lastGoalMotion_ = NULL;
337 double dt = siC_->getPropagationStepSize();
339 std::vector<Motion*> motions;
340 motions.reserve(priorityQueue_.size());
341 priorityQueue_.getContent(motions);
344 if (lastGoalMotion_ != NULL)
347 for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
348 if (!(*it)->isSplit_)
351 Motion *cur = *it, *ancestor;
352 unsigned int duration = findDurationAndAncestor(cur, cur->
endState_, scratch, ancestor);
361 if (ancestor->parent_)
367 duration = findDurationAndAncestor(cur->
parent_, cur->
startState_, scratch, ancestor);
377 if (ancestor->parent_)
390 si_->freeState(scratch);
395 double childVolume = .5 * volume_;
396 unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
397 splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
399 left_ =
new Cell(childVolume, bounds_, nextSplitDimension);
400 left_->bounds_.high[splitDimension_] = splitValue_;
401 left_->motions_.reserve(motions_.size());
402 right_ =
new Cell(childVolume, bounds_, nextSplitDimension);
403 right_->bounds_.low[splitDimension_] = splitValue_;
404 right_->motions_.reserve(motions_.size());
unsigned int controlDuration_
The duration that the control was applied to arrive at this state from the parent.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Motion * propagateFrom(Motion *motion, base::State *, base::State *)
Select a state along motion and propagate a new motion from there. Return NULL if no valid motion cou...
double priority_
Priority for selecting this path to extend from in the future.
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition of an abstract control.
Class representing the tree of motions exploring the state space.
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...
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition of a control path.
virtual void getPlannerData(base::PlannerData &data) const
Extracts the planner data from the priority queue into data.
ompl::base::State * startState_
The starting point of this motion.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Cell * cell_
Pointer to the cell that contains this path.
The planner found an exact solution.
ompl::control::Control * control_
The control that was applied to arrive at this state from the parent.
void addMotion(Motion *motion, Cell *cell, base::State *, base::State *, base::EuclideanProjection &, base::EuclideanProjection &)
Inserts the motion into the appropriate cells, splitting the motion as necessary. motion is assumed t...
A class to store the exit status of Planner::solve()
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...
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.
void addMotion(Motion *motion)
Add a motion.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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 ...
The exception type for ompl.
std::vector< Motion * > motions_
The motions contained in this cell. Motions are stored only in leaf nodes.
void subdivide(unsigned int spaceDimension)
Subdivides this cell.
Cell is a Binary Space Partition.
*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.
ompl::BinaryHeap< Motion *, MotionCompare >::Element * heapElement_
Handle to the element of the priority queue for this Motion.
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
Cell * stab(const ompl::base::EuclideanProjection &projection) const
Locates the cell that this motion begins in.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
double getGoalBias(void) const
Get the goal bias the planner is using */.
ompl::base::State * endState_
The state reached by this motion.
A boost shared pointer wrapper for ompl::base::Path.
Motion * parent_
Parent motion from which this one started.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.