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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines