00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/planners/kpiece/KPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include "ompl/util/Exception.h"
00041 #include <limits>
00042 #include <cassert>
00043
00044 void ompl::control::KPIECE1::setup(void)
00045 {
00046 Planner::setup();
00047 SelfConfig sc(si_, getName());
00048 sc.configureProjectionEvaluator(projectionEvaluator_);
00049
00050 if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00051 throw Exception("Bad cell score factor must be in the range (0,1]");
00052 if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00053 throw Exception("Good cell score factor must be in the range (0,1]");
00054 if (selectBorderFraction_ < std::numeric_limits<double>::epsilon() || selectBorderFraction_ > 1.0)
00055 throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00056
00057 tree_.grid.setDimension(projectionEvaluator_->getDimension());
00058 }
00059
00060 void ompl::control::KPIECE1::clear(void)
00061 {
00062 Planner::clear();
00063 controlSampler_.reset();
00064 freeMemory();
00065 tree_.grid.clear();
00066 tree_.size = 0;
00067 tree_.iteration = 1;
00068 }
00069
00070 void ompl::control::KPIECE1::freeMemory(void)
00071 {
00072 freeGridMotions(tree_.grid);
00073 }
00074
00075 void ompl::control::KPIECE1::freeGridMotions(Grid &grid)
00076 {
00077 for (Grid::iterator it = grid.begin(); it != grid.end() ; ++it)
00078 freeCellData(it->second->data);
00079 }
00080
00081 void ompl::control::KPIECE1::freeCellData(CellData *cdata)
00082 {
00083 for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00084 freeMotion(cdata->motions[i]);
00085 delete cdata;
00086 }
00087
00088 void ompl::control::KPIECE1::freeMotion(Motion *motion)
00089 {
00090 if (motion->state)
00091 si_->freeState(motion->state);
00092 if (motion->control)
00093 siC_->freeControl(motion->control);
00094 delete motion;
00095 }
00096
00097 bool ompl::control::KPIECE1::CloseSamples::consider(Grid::Cell *cell, Motion *motion, double distance)
00098 {
00099 if (samples.empty())
00100 {
00101 CloseSample cs(cell, motion, distance);
00102 samples.insert(cs);
00103 return true;
00104 }
00105
00106
00107 if (samples.rbegin()->distance > distance)
00108 {
00109
00110
00111 if (samples.size() >= maxSize)
00112 samples.erase(--samples.end());
00113 CloseSample cs(cell, motion, distance);
00114 samples.insert(cs);
00115 return true;
00116 }
00117
00118 return false;
00119 }
00120
00121 bool ompl::control::KPIECE1::CloseSamples::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00122 {
00123 if (samples.size() > 0)
00124 {
00125 scell = samples.begin()->cell;
00126 smotion = samples.begin()->motion;
00127
00128
00129 double d = (samples.begin()->distance + samples.rbegin()->distance) * 0.55;
00130 samples.erase(samples.begin());
00131 consider(scell, smotion, d);
00132 return true;
00133 }
00134 return false;
00135 }
00136
00137 unsigned int ompl::control::KPIECE1::findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count)
00138 {
00139 for (unsigned int i = index + 1 ; i < count ; ++i)
00140 if (coords[i] != coords[index])
00141 return i - 1;
00142
00143 return count - 1;
00144 }
00145
00146 bool ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00147 {
00148 checkValidity();
00149 base::Goal *goal = pdef_->getGoal().get();
00150
00151 while (const base::State *st = pis_.nextStart())
00152 {
00153 Motion *motion = new Motion(siC_);
00154 si_->copyState(motion->state, st);
00155 siC_->nullControl(motion->control);
00156 addMotion(motion, 1.0);
00157 }
00158
00159 if (tree_.grid.size() == 0)
00160 {
00161 msg_.error("There are no valid initial states!");
00162 return false;
00163 }
00164
00165 if (!controlSampler_)
00166 controlSampler_ = siC_->allocControlSampler();
00167
00168 msg_.inform("Starting with %u states", tree_.size);
00169
00170 Motion *solution = NULL;
00171 Motion *approxsol = NULL;
00172 double approxdif = std::numeric_limits<double>::infinity();
00173
00174 Control *rctrl = siC_->allocControl();
00175
00176 std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
00177 std::vector<Grid::Coord> coords(states.size());
00178 std::vector<Grid::Cell*> cells(coords.size());
00179
00180 for (unsigned int i = 0 ; i < states.size() ; ++i)
00181 states[i] = si_->allocState();
00182
00183
00184 CloseSamples closeSamples(nCloseSamples_);
00185
00186 while (ptc() == false)
00187 {
00188 tree_.iteration++;
00189
00190
00191 Motion *existing = NULL;
00192 Grid::Cell *ecell = NULL;
00193
00194 if (closeSamples.canSample() && rng_.uniform01() < goalBias_)
00195 {
00196 if (!closeSamples.selectMotion(existing, ecell))
00197 selectMotion(existing, ecell);
00198 }
00199 else
00200 selectMotion(existing, ecell);
00201 assert(existing);
00202
00203
00204 controlSampler_->sampleNext(rctrl, existing->control, existing->state);
00205
00206
00207 unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00208 cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false);
00209
00210
00211 if (cd >= siC_->getMinControlDuration())
00212 {
00213 std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
00214 bool interestingMotion = false;
00215
00216
00217 for (unsigned int i = 0 ; i < cd ; ++i)
00218 {
00219 projectionEvaluator_->computeCoordinates(states[i], coords[i]);
00220 cells[i] = tree_.grid.getCell(coords[i]);
00221 if (!cells[i])
00222 interestingMotion = true;
00223 else
00224 {
00225 if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
00226 interestingMotion = true;
00227 }
00228 }
00229
00230 if (interestingMotion || rng_.uniform01() < 0.05)
00231 {
00232 unsigned int index = 0;
00233 while (index < cd)
00234 {
00235 unsigned int nextIndex = findNextMotion(coords, index, cd);
00236 Motion *motion = new Motion(siC_);
00237 si_->copyState(motion->state, states[nextIndex]);
00238 siC_->copyControl(motion->control, rctrl);
00239 motion->steps = nextIndex - index + 1;
00240 motion->parent = existing;
00241
00242 double dist = 0.0;
00243 bool solved = goal->isSatisfied(motion->state, &dist);
00244 Grid::Cell *toCell = addMotion(motion, dist);
00245
00246 if (solved)
00247 {
00248 approxdif = dist;
00249 solution = motion;
00250 break;
00251 }
00252 if (dist < approxdif)
00253 {
00254 approxdif = dist;
00255 approxsol = motion;
00256 }
00257
00258 closeSamples.consider(toCell, motion, dist);
00259
00260
00261 existing = motion;
00262 index = nextIndex + 1;
00263 }
00264
00265 if (solution)
00266 break;
00267 }
00268
00269
00270 ecell->data->score *= goodScoreFactor_;
00271 }
00272 else
00273 ecell->data->score *= badScoreFactor_;
00274
00275 tree_.grid.update(ecell);
00276 }
00277
00278 bool approximate = false;
00279 if (solution == NULL)
00280 {
00281 solution = approxsol;
00282 approximate = true;
00283 }
00284
00285 if (solution != NULL)
00286 {
00287
00288 std::vector<Motion*> mpath;
00289 while (solution != NULL)
00290 {
00291 mpath.push_back(solution);
00292 solution = solution->parent;
00293 }
00294
00295
00296 PathControl *path = new PathControl(si_);
00297 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00298 {
00299 path->states.push_back(si_->cloneState(mpath[i]->state));
00300 if (mpath[i]->parent)
00301 {
00302 path->controls.push_back(siC_->cloneControl(mpath[i]->control));
00303 path->controlDurations.push_back(mpath[i]->steps * siC_->getPropagationStepSize());
00304 }
00305 }
00306
00307 goal->setDifference(approxdif);
00308 goal->setSolutionPath(base::PathPtr(path), approximate);
00309
00310 if (approximate)
00311 msg_.warn("Found approximate solution");
00312 }
00313
00314 siC_->freeControl(rctrl);
00315 for (unsigned int i = 0 ; i < states.size() ; ++i)
00316 si_->freeState(states[i]);
00317
00318 msg_.inform("Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(),
00319 tree_.grid.countInternal(), tree_.grid.countExternal());
00320
00321 return goal->isAchieved();
00322 }
00323
00324 bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00325 {
00326 scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
00327 tree_.grid.topExternal() : tree_.grid.topInternal();
00328
00329
00330
00331 if (scell->data->score < std::numeric_limits<double>::epsilon())
00332 {
00333 msg_.debug("Numerical precision limit reached. Resetting costs.");
00334 std::vector<CellData*> content;
00335 content.reserve(tree_.grid.size());
00336 tree_.grid.getContent(content);
00337 for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00338 (*it)->score += 1.0 + log((double)((*it)->iteration));
00339 tree_.grid.updateAll();
00340 }
00341
00342 if (scell && !scell->data->motions.empty())
00343 {
00344 scell->data->selections++;
00345 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00346 return true;
00347 }
00348 else
00349 return false;
00350 }
00351
00352 ompl::control::KPIECE1::Grid::Cell* ompl::control::KPIECE1::addMotion(Motion *motion, double dist)
00353 {
00354 Grid::Coord coord;
00355 projectionEvaluator_->computeCoordinates(motion->state, coord);
00356 Grid::Cell* cell = tree_.grid.getCell(coord);
00357 if (cell)
00358 {
00359 cell->data->motions.push_back(motion);
00360 cell->data->coverage += motion->steps;
00361 tree_.grid.update(cell);
00362 }
00363 else
00364 {
00365 cell = tree_.grid.createCell(coord);
00366 cell->data = new CellData();
00367 cell->data->motions.push_back(motion);
00368 cell->data->coverage = motion->steps;
00369 cell->data->iteration = tree_.iteration;
00370 cell->data->selections = 1;
00371 cell->data->score = (1.0 + log((double)(tree_.iteration))) / (1e-3 + dist);
00372 tree_.grid.add(cell);
00373 }
00374 tree_.size++;
00375 return cell;
00376 }
00377
00378 void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
00379 {
00380 Planner::getPlannerData(data);
00381
00382 Grid::CellArray cells;
00383 tree_.grid.getCells(cells);
00384
00385 if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00386 {
00387 double delta = siC_->getPropagationStepSize();
00388
00389 for (unsigned int i = 0 ; i < cells.size() ; ++i)
00390 for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
00391 {
00392 const Motion* m = cells[i]->data->motions[j];
00393 if (m->parent)
00394 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00395 else
00396 cpd->recordEdge(NULL, m->state, NULL, 0.);
00397 cpd->tagState(m->state, cells[i]->border ? 2 : 1);
00398 }
00399 }
00400 else
00401 {
00402 for (unsigned int i = 0 ; i < cells.size() ; ++i)
00403 for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
00404 {
00405 const Motion* m = cells[i]->data->motions[j];
00406 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00407 data.tagState(m->state, cells[i]->border ? 2 : 1);
00408 }
00409 }
00410 }