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/geometric/planners/sbl/SBL.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <limits>
00041 #include <cassert>
00042
00043 ompl::geometric::SBL::SBL(const base::SpaceInformationPtr &si) : base::Planner(si, "SBL")
00044 {
00045 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00046 maxDistance_ = 0.0;
00047
00048 Planner::declareParam<double>("range", this, &SBL::setRange, &SBL::getRange);
00049 }
00050
00051 ompl::geometric::SBL::~SBL(void)
00052 {
00053 freeMemory();
00054 }
00055
00056 void ompl::geometric::SBL::setup(void)
00057 {
00058 Planner::setup();
00059 tools::SelfConfig sc(si_, getName());
00060 sc.configureProjectionEvaluator(projectionEvaluator_);
00061 sc.configurePlannerRange(maxDistance_);
00062
00063 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
00064 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
00065 }
00066
00067 void ompl::geometric::SBL::freeGridMotions(Grid<MotionInfo> &grid)
00068 {
00069 for (Grid<MotionInfo>::iterator it = grid.begin(); it != grid.end() ; ++it)
00070 {
00071 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00072 {
00073 if (it->second->data[i]->state)
00074 si_->freeState(it->second->data[i]->state);
00075 delete it->second->data[i];
00076 }
00077 }
00078 }
00079
00080 bool ompl::geometric::SBL::solve(const base::PlannerTerminationCondition &ptc)
00081 {
00082 checkValidity();
00083 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00084
00085 if (!goal)
00086 {
00087 msg_.error("Unknown type of goal (or goal undefined)");
00088 return false;
00089 }
00090
00091 while (const base::State *st = pis_.nextStart())
00092 {
00093 Motion *motion = new Motion(si_);
00094 si_->copyState(motion->state, st);
00095 motion->valid = true;
00096 motion->root = motion->state;
00097 addMotion(tStart_, motion);
00098 }
00099
00100 if (tStart_.size == 0)
00101 {
00102 msg_.error("Motion planning start tree could not be initialized!");
00103 return false;
00104 }
00105
00106 if (!goal->couldSample())
00107 {
00108 msg_.error("Insufficient states in sampleable goal region");
00109 return false;
00110 }
00111
00112 if (!sampler_)
00113 sampler_ = si_->allocValidStateSampler();
00114
00115 msg_.inform("Starting with %d states", (int)(tStart_.size + tGoal_.size));
00116
00117 std::vector<Motion*> solution;
00118 base::State *xstate = si_->allocState();
00119
00120 bool startTree = true;
00121 bool solved = false;
00122
00123 while (ptc() == false)
00124 {
00125 TreeData &tree = startTree ? tStart_ : tGoal_;
00126 startTree = !startTree;
00127 TreeData &otherTree = startTree ? tStart_ : tGoal_;
00128
00129
00130 if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
00131 {
00132 const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00133 if (st)
00134 {
00135 Motion* motion = new Motion(si_);
00136 si_->copyState(motion->state, st);
00137 motion->root = motion->state;
00138 motion->valid = true;
00139 addMotion(tGoal_, motion);
00140 }
00141 if (tGoal_.size == 0)
00142 {
00143 msg_.error("Unable to sample any valid states for goal tree");
00144 break;
00145 }
00146 }
00147
00148 Motion *existing = selectMotion(tree);
00149 assert(existing);
00150 if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
00151 continue;
00152
00153
00154 Motion *motion = new Motion(si_);
00155 si_->copyState(motion->state, xstate);
00156 motion->parent = existing;
00157 motion->root = existing->root;
00158 existing->children.push_back(motion);
00159
00160 addMotion(tree, motion);
00161
00162 if (checkSolution(!startTree, tree, otherTree, motion, solution))
00163 {
00164 PathGeometric *path = new PathGeometric(si_);
00165 for (unsigned int i = 0 ; i < solution.size() ; ++i)
00166 path->append(solution[i]->state);
00167
00168 goal->addSolutionPath(base::PathPtr(path), false, 0.0);
00169 solved = true;
00170 break;
00171 }
00172 }
00173
00174 si_->freeState(xstate);
00175
00176 msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
00177 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
00178
00179 return solved;
00180 }
00181
00182 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
00183 {
00184 Grid<MotionInfo>::Coord coord;
00185 projectionEvaluator_->computeCoordinates(motion->state, coord);
00186 Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord);
00187
00188 if (cell && !cell->data.empty())
00189 {
00190 Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)];
00191
00192 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
00193 {
00194 Motion *connect = new Motion(si_);
00195
00196 si_->copyState(connect->state, connectOther->state);
00197 connect->parent = motion;
00198 connect->root = motion->root;
00199 motion->children.push_back(connect);
00200 addMotion(tree, connect);
00201
00202 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
00203 {
00204
00205
00206 std::vector<Motion*> mpath1;
00207 while (motion != NULL)
00208 {
00209 mpath1.push_back(motion);
00210 motion = motion->parent;
00211 }
00212
00213 std::vector<Motion*> mpath2;
00214 while (connectOther != NULL)
00215 {
00216 mpath2.push_back(connectOther);
00217 connectOther = connectOther->parent;
00218 }
00219
00220 if (!start)
00221 mpath1.swap(mpath2);
00222
00223 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00224 solution.push_back(mpath1[i]);
00225 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
00226
00227 return true;
00228 }
00229 }
00230 }
00231 return false;
00232 }
00233
00234 bool ompl::geometric::SBL::isPathValid(TreeData &tree, Motion *motion)
00235 {
00236 std::vector<Motion*> mpath;
00237
00238
00239 while (motion != NULL)
00240 {
00241 mpath.push_back(motion);
00242 motion = motion->parent;
00243 }
00244
00245
00246 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00247 if (!mpath[i]->valid)
00248 {
00249 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00250 mpath[i]->valid = true;
00251 else
00252 {
00253 removeMotion(tree, mpath[i]);
00254 return false;
00255 }
00256 }
00257 return true;
00258 }
00259
00260 ompl::geometric::SBL::Motion* ompl::geometric::SBL::selectMotion(TreeData &tree)
00261 {
00262 GridCell* cell = tree.pdf.sample(rng_.uniform01());
00263 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00264 }
00265
00266 void ompl::geometric::SBL::removeMotion(TreeData &tree, Motion *motion)
00267 {
00268
00269
00270 Grid<MotionInfo>::Coord coord;
00271 projectionEvaluator_->computeCoordinates(motion->state, coord);
00272 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00273 if (cell)
00274 {
00275 for (unsigned int i = 0 ; i < cell->data.size(); ++i)
00276 {
00277 if (cell->data[i] == motion)
00278 {
00279 cell->data.erase(cell->data.begin() + i);
00280 tree.size--;
00281 break;
00282 }
00283 }
00284 if (cell->data.empty())
00285 {
00286 tree.pdf.remove(cell->data.elem_);
00287 tree.grid.remove(cell);
00288 tree.grid.destroyCell(cell);
00289 }
00290 else
00291 {
00292 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00293 }
00294 }
00295
00296
00297
00298 if (motion->parent)
00299 {
00300 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00301 {
00302 if (motion->parent->children[i] == motion)
00303 {
00304 motion->parent->children.erase(motion->parent->children.begin() + i);
00305 break;
00306 }
00307 }
00308 }
00309
00310
00311 for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00312 {
00313 motion->children[i]->parent = NULL;
00314 removeMotion(tree, motion->children[i]);
00315 }
00316
00317 if (motion->state)
00318 si_->freeState(motion->state);
00319 delete motion;
00320 }
00321
00322 void ompl::geometric::SBL::addMotion(TreeData &tree, Motion *motion)
00323 {
00324 Grid<MotionInfo>::Coord coord;
00325 projectionEvaluator_->computeCoordinates(motion->state, coord);
00326 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
00327 if (cell)
00328 {
00329 cell->data.push_back(motion);
00330 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
00331 }
00332 else
00333 {
00334 cell = tree.grid.createCell(coord);
00335 cell->data.push_back(motion);
00336 tree.grid.add(cell);
00337 cell->data.elem_ = tree.pdf.add(cell, 1.0);
00338 }
00339 tree.size++;
00340 }
00341
00342 void ompl::geometric::SBL::clear(void)
00343 {
00344 Planner::clear();
00345
00346 sampler_.reset();
00347
00348 freeMemory();
00349
00350 tStart_.grid.clear();
00351 tStart_.size = 0;
00352 tStart_.pdf.clear();
00353
00354 tGoal_.grid.clear();
00355 tGoal_.size = 0;
00356 tGoal_.pdf.clear();
00357 }
00358
00359 void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
00360 {
00361 Planner::getPlannerData(data);
00362
00363 std::vector<MotionInfo> motions;
00364 tStart_.grid.getContent(motions);
00365
00366 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00367 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00368 {
00369 data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00370 data.tagState(motions[i][j]->state, 1);
00371 }
00372
00373
00374 motions.clear();
00375 tGoal_.grid.getContent(motions);
00376
00377 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00378 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00379 {
00380 data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00381 data.tagState(motions[i][j]->state, 2);
00382 }
00383
00384 }