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