All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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/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         // if we have not sampled too many goals already
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         /* create a motion */
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                 /* extract the motions and put them in solution vector */
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     /* construct the solution path */
00225     while (motion != NULL)
00226     {
00227         mpath.push_back(motion);
00228         motion = motion->parent;
00229     }
00230 
00231     /* check the path */
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     /* remove from grid */
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     /* remove self from parent list */
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     /* remove children */
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 }