ompl/geometric/planners/sbl/src/SBL.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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, "0.:1.:10000."); 00050 } 00051 00052 ompl::geometric::SBL::~SBL() 00053 { 00054 freeMemory(); 00055 } 00056 00057 void ompl::geometric::SBL::setup() 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 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 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 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); 00104 return base::PlannerStatus::INVALID_START; 00105 } 00106 00107 if (!goal->couldSample()) 00108 { 00109 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); 00110 return base::PlannerStatus::INVALID_GOAL; 00111 } 00112 00113 if (!sampler_) 00114 sampler_ = si_->allocValidStateSampler(); 00115 00116 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (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 // if we have not sampled too many goals already 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 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str()); 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 /* create a motion */ 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, getName()); 00170 solved = true; 00171 break; 00172 } 00173 } 00174 00175 si_->freeState(xstate); 00176 00177 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", 00178 getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, 00179 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size()); 00180 00181 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00182 } 00183 00184 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution) 00185 { 00186 Grid<MotionInfo>::Coord coord; 00187 projectionEvaluator_->computeCoordinates(motion->state, coord); 00188 Grid<MotionInfo>::Cell* cell = otherTree.grid.getCell(coord); 00189 00190 if (cell && !cell->data.empty()) 00191 { 00192 Motion *connectOther = cell->data[rng_.uniformInt(0, cell->data.size() - 1)]; 00193 00194 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root)) 00195 { 00196 Motion *connect = new Motion(si_); 00197 00198 si_->copyState(connect->state, connectOther->state); 00199 connect->parent = motion; 00200 connect->root = motion->root; 00201 motion->children.push_back(connect); 00202 addMotion(tree, connect); 00203 00204 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther)) 00205 { 00206 if (start) 00207 connectionPoint_ = std::make_pair(motion->state, connectOther->state); 00208 else 00209 connectionPoint_ = std::make_pair(connectOther->state, motion->state); 00210 00211 /* extract the motions and put them in solution vector */ 00212 00213 std::vector<Motion*> mpath1; 00214 while (motion != NULL) 00215 { 00216 mpath1.push_back(motion); 00217 motion = motion->parent; 00218 } 00219 00220 std::vector<Motion*> mpath2; 00221 while (connectOther != NULL) 00222 { 00223 mpath2.push_back(connectOther); 00224 connectOther = connectOther->parent; 00225 } 00226 00227 if (!start) 00228 mpath1.swap(mpath2); 00229 00230 for (int i = mpath1.size() - 1 ; i >= 0 ; --i) 00231 solution.push_back(mpath1[i]); 00232 solution.insert(solution.end(), mpath2.begin(), mpath2.end()); 00233 00234 return true; 00235 } 00236 } 00237 } 00238 return false; 00239 } 00240 00241 bool ompl::geometric::SBL::isPathValid(TreeData &tree, Motion *motion) 00242 { 00243 std::vector<Motion*> mpath; 00244 00245 /* construct the solution path */ 00246 while (motion != NULL) 00247 { 00248 mpath.push_back(motion); 00249 motion = motion->parent; 00250 } 00251 00252 /* check the path */ 00253 for (int i = mpath.size() - 1 ; i >= 0 ; --i) 00254 if (!mpath[i]->valid) 00255 { 00256 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state)) 00257 mpath[i]->valid = true; 00258 else 00259 { 00260 removeMotion(tree, mpath[i]); 00261 return false; 00262 } 00263 } 00264 return true; 00265 } 00266 00267 ompl::geometric::SBL::Motion* ompl::geometric::SBL::selectMotion(TreeData &tree) 00268 { 00269 GridCell* cell = tree.pdf.sample(rng_.uniform01()); 00270 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL; 00271 } 00272 00273 void ompl::geometric::SBL::removeMotion(TreeData &tree, Motion *motion) 00274 { 00275 /* remove from grid */ 00276 00277 Grid<MotionInfo>::Coord coord; 00278 projectionEvaluator_->computeCoordinates(motion->state, coord); 00279 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord); 00280 if (cell) 00281 { 00282 for (unsigned int i = 0 ; i < cell->data.size(); ++i) 00283 { 00284 if (cell->data[i] == motion) 00285 { 00286 cell->data.erase(cell->data.begin() + i); 00287 tree.size--; 00288 break; 00289 } 00290 } 00291 if (cell->data.empty()) 00292 { 00293 tree.pdf.remove(cell->data.elem_); 00294 tree.grid.remove(cell); 00295 tree.grid.destroyCell(cell); 00296 } 00297 else 00298 { 00299 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size()); 00300 } 00301 } 00302 00303 /* remove self from parent list */ 00304 00305 if (motion->parent) 00306 { 00307 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i) 00308 { 00309 if (motion->parent->children[i] == motion) 00310 { 00311 motion->parent->children.erase(motion->parent->children.begin() + i); 00312 break; 00313 } 00314 } 00315 } 00316 00317 /* remove children */ 00318 for (unsigned int i = 0 ; i < motion->children.size() ; ++i) 00319 { 00320 motion->children[i]->parent = NULL; 00321 removeMotion(tree, motion->children[i]); 00322 } 00323 00324 if (motion->state) 00325 si_->freeState(motion->state); 00326 delete motion; 00327 } 00328 00329 void ompl::geometric::SBL::addMotion(TreeData &tree, Motion *motion) 00330 { 00331 Grid<MotionInfo>::Coord coord; 00332 projectionEvaluator_->computeCoordinates(motion->state, coord); 00333 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord); 00334 if (cell) 00335 { 00336 cell->data.push_back(motion); 00337 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size()); 00338 } 00339 else 00340 { 00341 cell = tree.grid.createCell(coord); 00342 cell->data.push_back(motion); 00343 tree.grid.add(cell); 00344 cell->data.elem_ = tree.pdf.add(cell, 1.0); 00345 } 00346 tree.size++; 00347 } 00348 00349 void ompl::geometric::SBL::clear() 00350 { 00351 Planner::clear(); 00352 00353 sampler_.reset(); 00354 00355 freeMemory(); 00356 00357 tStart_.grid.clear(); 00358 tStart_.size = 0; 00359 tStart_.pdf.clear(); 00360 00361 tGoal_.grid.clear(); 00362 tGoal_.size = 0; 00363 tGoal_.pdf.clear(); 00364 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL); 00365 } 00366 00367 void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const 00368 { 00369 Planner::getPlannerData(data); 00370 00371 std::vector<MotionInfo> motions; 00372 tStart_.grid.getContent(motions); 00373 00374 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00375 for (unsigned int j = 0 ; j < motions[i].size() ; ++j) 00376 if (motions[i][j]->parent == NULL) 00377 data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1)); 00378 else 00379 data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1), 00380 base::PlannerDataVertex(motions[i][j]->state, 1)); 00381 00382 motions.clear(); 00383 tGoal_.grid.getContent(motions); 00384 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00385 for (unsigned int j = 0 ; j < motions[i].size() ; ++j) 00386 if (motions[i][j]->parent == NULL) 00387 data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2)); 00388 else 00389 // The edges in the goal tree are reversed so that they are in the same direction as start tree 00390 data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2), 00391 base::PlannerDataVertex(motions[i][j]->parent->state, 2)); 00392 00393 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second)); 00394 }