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