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