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