37 #include "ompl/control/planners/kpiece/KPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/util/Exception.h"
64 ompl::control::KPIECE1::~KPIECE1(
void)
75 if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
76 throw Exception(
"Bad cell score factor must be in the range (0,1]");
77 if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
78 throw Exception(
"Good cell score factor must be in the range (0,1]");
79 if (selectBorderFraction_ < std::numeric_limits<double>::epsilon() || selectBorderFraction_ > 1.0)
80 throw Exception(
"The fraction of time spent selecting border cells must be in the range (0,1]");
82 tree_.grid.setDimension(projectionEvaluator_->getDimension());
88 controlSampler_.reset();
93 lastGoalMotion_ = NULL;
98 freeGridMotions(tree_.grid);
104 freeCellData(it->second->data);
109 for (
unsigned int i = 0 ; i < cdata->
motions.size() ; ++i)
117 si_->freeState(motion->
state);
119 siC_->freeControl(motion->
control);
133 if (samples.rbegin()->distance > distance)
137 if (samples.size() >= maxSize)
138 samples.erase(--samples.end());
150 static const double CLOSE_MOTION_DISTANCE_INFLATION_FACTOR = 1.1;
155 if (samples.size() > 0)
157 scell = samples.begin()->cell;
158 smotion = samples.begin()->motion;
161 double d = (samples.begin()->distance + samples.rbegin()->distance) * (CLOSE_MOTION_DISTANCE_INFLATION_FACTOR / 2.0);
162 samples.erase(samples.begin());
163 consider(scell, smotion, d);
171 for (
unsigned int i = index + 1 ; i < count ; ++i)
172 if (coords[i] != coords[index])
186 si_->copyState(motion->
state, st);
187 siC_->nullControl(motion->
control);
188 addMotion(motion, 1.0);
191 if (tree_.grid.size() == 0)
193 OMPL_ERROR(
"There are no valid initial states!");
197 if (!controlSampler_)
198 controlSampler_ = siC_->allocControlSampler();
200 OMPL_INFORM(
"Starting with %u states", tree_.size);
204 double approxdif = std::numeric_limits<double>::infinity();
206 Control *rctrl = siC_->allocControl();
208 std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
209 std::vector<Grid::Coord> coords(states.size());
210 std::vector<Grid::Cell*> cells(coords.size());
212 for (
unsigned int i = 0 ; i < states.size() ; ++i)
213 states[i] = si_->allocState();
226 if (closeSamples.
canSample() && rng_.uniform01() < goalBias_)
229 selectMotion(existing, ecell);
232 selectMotion(existing, ecell);
236 controlSampler_->sampleNext(rctrl, existing->
control, existing->
state);
239 unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
240 cd = siC_->propagateWhileValid(existing->
state, rctrl, cd, states,
false);
243 if (cd >= siC_->getMinControlDuration())
245 std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
246 bool interestingMotion =
false;
249 for (
unsigned int i = 0 ; i < cd ; ++i)
251 projectionEvaluator_->computeCoordinates(states[i], coords[i]);
252 cells[i] = tree_.grid.getCell(coords[i]);
254 interestingMotion =
true;
257 if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
258 interestingMotion =
true;
262 if (interestingMotion || rng_.uniform01() < 0.05)
264 unsigned int index = 0;
267 unsigned int nextIndex = findNextMotion(coords, index, cd);
269 si_->copyState(motion->
state, states[nextIndex]);
270 siC_->copyControl(motion->
control, rctrl);
271 motion->
steps = nextIndex - index + 1;
272 motion->
parent = existing;
284 if (dist < approxdif)
290 closeSamples.
consider(toCell, motion, dist);
294 index = nextIndex + 1;
302 ecell->data->score *= goodScoreFactor_;
305 ecell->data->score *= badScoreFactor_;
307 tree_.grid.update(ecell);
311 bool approximate =
false;
312 if (solution == NULL)
314 solution = approxsol;
318 if (solution != NULL)
320 lastGoalMotion_ = solution;
323 std::vector<Motion*> mpath;
324 while (solution != NULL)
326 mpath.push_back(solution);
327 solution = solution->
parent;
332 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
333 if (mpath[i]->parent)
334 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
336 path->
append(mpath[i]->state);
338 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
342 siC_->freeControl(rctrl);
343 for (
unsigned int i = 0 ; i < states.size() ; ++i)
344 si_->freeState(states[i]);
346 OMPL_INFORM(
"Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(),
347 tree_.grid.countInternal(), tree_.grid.countExternal());
354 scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
355 tree_.grid.topExternal() : tree_.grid.topInternal();
359 if (scell->data->score < std::numeric_limits<double>::epsilon())
361 OMPL_DEBUG(
"Numerical precision limit reached. Resetting costs.");
362 std::vector<CellData*> content;
363 content.reserve(tree_.grid.size());
364 tree_.grid.getContent(content);
365 for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
366 (*it)->score += 1.0 +
log((
double)((*it)->iteration));
367 tree_.grid.updateAll();
370 if (scell && !scell->data->motions.empty())
372 scell->data->selections++;
373 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
382 static const double DISTANCE_TO_GOAL_OFFSET = 1e-3;
388 projectionEvaluator_->computeCoordinates(motion->
state, coord);
392 cell->data->motions.push_back(motion);
393 cell->data->coverage += motion->
steps;
394 tree_.grid.update(cell);
398 cell = tree_.grid.createCell(coord);
400 cell->data->motions.push_back(motion);
401 cell->data->coverage = motion->
steps;
402 cell->data->iteration = tree_.iteration;
403 cell->data->selections = 1;
404 cell->data->score = (1.0 +
log((
double)(tree_.iteration))) / (DISTANCE_TO_GOAL_OFFSET + dist);
405 tree_.grid.add(cell);
413 Planner::getPlannerData(data);
416 tree_.grid.getCells(cells);
418 double delta = siC_->getPropagationStepSize();
423 for (
unsigned int i = 0 ; i < cells.size() ; ++i)
425 for (
unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
427 const Motion* m = cells[i]->data->motions[j];