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