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