ompl/control/planners/kpiece/src/KPIECE1.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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, "0.:.05:1."); 00058 Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction, "0.:0.05:1."); 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() 00065 { 00066 freeMemory(); 00067 } 00068 00069 void ompl::control::KPIECE1::setup() 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() 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() 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 // if the sample we're considering is closer to the goal than the worst sample in the 00132 // set of close samples, we include it 00133 if (samples.rbegin()->distance > distance) 00134 { 00135 // if the inclusion would go above the maximum allowed size, 00136 // remove the last element 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 // this is the factor by which distances are inflated when considered for addition to closest samples 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 // average the highest & lowest distances and multiply by CLOSE_MOTION_DISTANCE_INFLATION_FACTOR 00160 // (make the distance appear artificially longer) 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 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00194 return base::PlannerStatus::INVALID_START; 00195 } 00196 00197 if (!controlSampler_) 00198 controlSampler_ = siC_->allocControlSampler(); 00199 00200 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), 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 // samples that were found to be the best, so far 00216 CloseSamples closeSamples(nCloseSamples_); 00217 00218 while (ptc == false) 00219 { 00220 tree_.iteration++; 00221 00222 /* Decide on a state to expand from */ 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 /* sample a random control */ 00236 controlSampler_->sampleNext(rctrl, existing->control, existing->state); 00237 00238 /* propagate */ 00239 unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration()); 00240 cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false); 00241 00242 /* if we have enough steps */ 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 // split the motion into smaller ones, so we do not cross cell boundaries 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 // new parent will be the newly created motion 00293 existing = motion; 00294 index = nextIndex + 1; 00295 } 00296 00297 if (solution) 00298 break; 00299 } 00300 00301 // update cell score 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 /* construct the solution path */ 00323 std::vector<Motion*> mpath; 00324 while (solution != NULL) 00325 { 00326 mpath.push_back(solution); 00327 solution = solution->parent; 00328 } 00329 00330 /* set the solution path */ 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, getName()); 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 OMPL_INFORM("%s: Created %u states in %u cells (%u internal + %u external)", 00347 getName().c_str(), tree_.size, tree_.grid.size(), 00348 tree_.grid.countInternal(), tree_.grid.countExternal()); 00349 00350 return base::PlannerStatus(solved, approximate); 00351 } 00352 00353 bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell) 00354 { 00355 scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ? 00356 tree_.grid.topExternal() : tree_.grid.topInternal(); 00357 00358 // We are running on finite precision, so our update scheme will end up 00359 // with 0 values for the score. This is where we fix the problem 00360 if (scell->data->score < std::numeric_limits<double>::epsilon()) 00361 { 00362 OMPL_DEBUG("%s: Numerical precision limit reached. Resetting costs.", getName().c_str()); 00363 std::vector<CellData*> content; 00364 content.reserve(tree_.grid.size()); 00365 tree_.grid.getContent(content); 00366 for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it) 00367 (*it)->score += 1.0 + log((double)((*it)->iteration)); 00368 tree_.grid.updateAll(); 00369 } 00370 00371 if (scell && !scell->data->motions.empty()) 00372 { 00373 scell->data->selections++; 00374 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)]; 00375 return true; 00376 } 00377 else 00378 return false; 00379 } 00380 00382 // this is the offset added to estimated distances to the goal, so we avoid division by 0 00383 static const double DISTANCE_TO_GOAL_OFFSET = 1e-3; 00385 00386 ompl::control::KPIECE1::Grid::Cell* ompl::control::KPIECE1::addMotion(Motion *motion, double dist) 00387 { 00388 Grid::Coord coord; 00389 projectionEvaluator_->computeCoordinates(motion->state, coord); 00390 Grid::Cell* cell = tree_.grid.getCell(coord); 00391 if (cell) 00392 { 00393 cell->data->motions.push_back(motion); 00394 cell->data->coverage += motion->steps; 00395 tree_.grid.update(cell); 00396 } 00397 else 00398 { 00399 cell = tree_.grid.createCell(coord); 00400 cell->data = new CellData(); 00401 cell->data->motions.push_back(motion); 00402 cell->data->coverage = motion->steps; 00403 cell->data->iteration = tree_.iteration; 00404 cell->data->selections = 1; 00405 cell->data->score = (1.0 + log((double)(tree_.iteration))) / (DISTANCE_TO_GOAL_OFFSET + dist); 00406 tree_.grid.add(cell); 00407 } 00408 tree_.size++; 00409 return cell; 00410 } 00411 00412 void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const 00413 { 00414 Planner::getPlannerData(data); 00415 00416 Grid::CellArray cells; 00417 tree_.grid.getCells(cells); 00418 00419 double delta = siC_->getPropagationStepSize(); 00420 00421 if (lastGoalMotion_) 00422 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); 00423 00424 for (unsigned int i = 0 ; i < cells.size() ; ++i) 00425 { 00426 for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j) 00427 { 00428 const Motion *m = cells[i]->data->motions[j]; 00429 if (m->parent) 00430 { 00431 if (data.hasControls()) 00432 data.addEdge(base::PlannerDataVertex (m->parent->state), 00433 base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1), 00434 control::PlannerDataEdgeControl (m->control, m->steps * delta)); 00435 else 00436 data.addEdge(base::PlannerDataVertex (m->parent->state), 00437 base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1)); 00438 } 00439 else 00440 data.addStartVertex(base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1)); 00441 00442 // A state created as a parent first may have an improper tag variable 00443 data.tagState(m->state, cells[i]->border ? 2 : 1); 00444 } 00445 } 00446 }