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