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 logError(
"There are no valid initial states!");
120 sampler_ = si_->allocValidStateSampler();
121 if (!controlSampler_)
122 controlSampler_ = siC_->allocDirectedControlSampler();
124 logInform(
"Starting with %u states", tree_.size);
127 double slndist = std::numeric_limits<double>::infinity();
134 Motion *existing = selectMotion();
138 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
142 if (!sampler_->sampleNear(rmotion->
state, existing->
state, maxDistance_))
147 unsigned int duration = controlSampler_->sampleTo(rmotion->
control, existing->
control,
152 duration = siC_->propagateWhileValid(existing->
state, rmotion->
control, duration, rmotion->
state);
155 if (duration >= siC_->getMinControlDuration())
159 si_->copyState(motion->
state, rmotion->
state);
161 motion->
steps = duration;
162 motion->
parent = existing;
170 if (solved || dist < slndist)
181 bool addedSolution =
false;
184 if (solution != NULL)
186 lastGoalMotion_ = solution;
188 std::vector<Motion*> mpath;
189 while (solution != NULL)
191 mpath.push_back(solution);
192 solution = solution->
parent;
196 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
197 if (mpath[i]->parent)
198 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
200 path->
append(mpath[i]->state);
201 addedSolution =
true;
202 pdef_->addSolutionPath(
base::PathPtr(path), !solved, slndist);
207 si_->freeState(rmotion->
state);
209 siC_->freeControl(rmotion->
control);
212 logInform(
"Created %u states in %u cells", tree_.size, tree_.grid.size());
219 GridCell* cell = pdf_.sample(rng_.uniform01());
220 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
226 projectionEvaluator_->computeCoordinates(motion->
state, coord);
230 cell->
data.push_back(motion);
231 pdf_.update(cell->data.elem_, 1.0/cell->data.size());
235 cell = tree_.grid.createCell(coord);
236 cell->data.push_back(motion);
237 tree_.grid.add(cell);
238 cell->data.elem_ = pdf_.add(cell, 1.0);
245 Planner::getPlannerData(data);
247 std::vector<MotionInfo> motions;
248 tree_.grid.getContent(motions);
250 double stepSize = siC_->getPropagationStepSize();
255 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
256 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
258 if (motions[i][j]->parent)
260 if (data.hasControls())