37 #include "ompl/control/planners/est/EST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
55 ompl::control::EST::~EST(
void)
67 tree_.grid.setDimension(projectionEvaluator_->getDimension());
74 controlSampler_.reset();
79 lastGoalMotion_ = NULL;
86 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
88 if (it->second->data[i]->state)
89 si_->freeState(it->second->data[i]->state);
90 if (it->second->data[i]->control)
91 siC_->freeControl(it->second->data[i]->control);
92 delete it->second->data[i];
107 si_->copyState(motion->
state, st);
108 siC_->nullControl(motion->
control);
112 if (tree_.grid.size() == 0)
114 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
120 sampler_ = si_->allocValidStateSampler();
121 if (!controlSampler_)
122 controlSampler_ = siC_->allocDirectedControlSampler();
124 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), tree_.size);
128 double approxdif = std::numeric_limits<double>::infinity();
135 Motion *existing = selectMotion();
139 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
143 if (!sampler_->sampleNear(rmotion->
state, existing->
state, maxDistance_))
148 unsigned int duration = controlSampler_->sampleTo(rmotion->
control, existing->
control,
152 if (duration >= siC_->getMinControlDuration())
156 si_->copyState(motion->
state, rmotion->
state);
158 motion->
steps = duration;
159 motion->
parent = existing;
173 if (dist < approxdif)
181 bool approximate =
false;
182 if (solution == NULL)
184 solution = approxsol;
189 if (solution != NULL)
191 lastGoalMotion_ = solution;
193 std::vector<Motion*> mpath;
194 while (solution != NULL)
196 mpath.push_back(solution);
197 solution = solution->
parent;
201 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
202 if (mpath[i]->parent)
203 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
205 path->
append(mpath[i]->state);
207 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
212 si_->freeState(rmotion->
state);
214 siC_->freeControl(rmotion->
control);
217 OMPL_INFORM(
"%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
224 GridCell* cell = pdf_.sample(rng_.uniform01());
225 return cell && !cell->
data.empty() ? cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)] : NULL;
231 projectionEvaluator_->computeCoordinates(motion->
state, coord);
232 GridCell* cell = tree_.grid.getCell(coord);
235 cell->
data.push_back(motion);
236 pdf_.update(cell->
data.elem_, 1.0/cell->
data.size());
240 cell = tree_.grid.createCell(coord);
241 cell->
data.push_back(motion);
242 tree_.grid.add(cell);
243 cell->
data.elem_ = pdf_.add(cell, 1.0);
250 Planner::getPlannerData(data);
252 std::vector<MotionInfo> motions;
253 tree_.grid.getContent(motions);
255 double stepSize = siC_->getPropagationStepSize();
260 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
261 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
263 if (motions[i][j]->parent)
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
double getGoalBias(void) const
Get the goal bias the planner is using.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
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...
void addMotion(Motion *motion)
Add a motion to the exploration tree.
Control * control
The control contained by the motion.
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...
std::vector< int > Coord
Definition of a coordinate within this grid.
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...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
_T data
The data we store in the cell.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
double getRange(void) const
Get the range the planner is using.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
void freeMemory(void)
Free the memory allocated by this planner.
Definition of a control path.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
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.
unsigned int steps
The number of steps the control is applied for.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Motion * parent
The parent motion in the exploration tree.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
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.
Representation of a motion.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
void setRange(double distance)
Set the range the planner is supposed to use.
base::State * state
The state contained by the motion.
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.
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
Motion * selectMotion(void)
Select a motion to continue the expansion of the tree from.
double maxDistance_
The maximum length of a motion to be added to a tree.
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
A boost shared pointer wrapper for ompl::base::Path.
CoordHash::const_iterator iterator
We only allow const iterators.
EST(const SpaceInformationPtr &si)
Constructor.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.