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(
"There are no valid initial states!");
120 sampler_ = si_->allocValidStateSampler();
121 if (!controlSampler_)
122 controlSampler_ = siC_->allocDirectedControlSampler();
124 OMPL_INFORM(
"Starting with %u states", 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,
153 duration = siC_->propagateWhileValid(existing->
state, rmotion->
control, duration, rmotion->
state);
156 if (duration >= siC_->getMinControlDuration())
160 si_->copyState(motion->
state, rmotion->
state);
162 motion->
steps = duration;
163 motion->
parent = existing;
177 if (dist < approxdif)
185 bool approximate =
false;
186 if (solution == NULL)
188 solution = approxsol;
193 if (solution != NULL)
195 lastGoalMotion_ = solution;
197 std::vector<Motion*> mpath;
198 while (solution != NULL)
200 mpath.push_back(solution);
201 solution = solution->
parent;
205 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
206 if (mpath[i]->parent)
207 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
209 path->
append(mpath[i]->state);
211 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
216 si_->freeState(rmotion->
state);
218 siC_->freeControl(rmotion->
control);
221 OMPL_INFORM(
"Created %u states in %u cells", tree_.size, tree_.grid.size());
228 GridCell* cell = pdf_.sample(rng_.uniform01());
229 return cell && !cell->
data.empty() ? cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)] : NULL;
235 projectionEvaluator_->computeCoordinates(motion->
state, coord);
236 GridCell* cell = tree_.grid.getCell(coord);
239 cell->
data.push_back(motion);
240 pdf_.update(cell->
data.elem_, 1.0/cell->
data.size());
244 cell = tree_.grid.createCell(coord);
245 cell->
data.push_back(motion);
246 tree_.grid.add(cell);
247 cell->
data.elem_ = pdf_.add(cell, 1.0);
254 Planner::getPlannerData(data);
256 std::vector<MotionInfo> motions;
257 tree_.grid.getContent(motions);
259 double stepSize = siC_->getPropagationStepSize();
264 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
265 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
267 if (motions[i][j]->parent)