All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/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);
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         // 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                 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         /* 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);
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                 /* extract the motions and put them in solution vector */
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     /* construct the solution path */
00245     while (motion != NULL)
00246     {
00247         mpath.push_back(motion);
00248         motion = motion->parent;
00249     }
00250 
00251     /* check the path */
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     /* remove from grid */
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     /* remove self from parent list */
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     /* remove children */
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                 // The edges in the goal tree are reversed so that they are in the same direction as start tree
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines