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