All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
EST.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/est/EST.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::EST::setup(void)
00044 {
00045     Planner::setup();
00046     SelfConfig sc(si_, getName());
00047     sc.configureProjectionEvaluator(projectionEvaluator_);
00048     sc.configurePlannerRange(maxDistance_);
00049 
00050     tree_.grid.setDimension(projectionEvaluator_->getDimension());
00051 }
00052 
00053 void ompl::geometric::EST::clear(void)
00054 {
00055     Planner::clear();
00056     sampler_.reset();
00057     freeMemory();
00058     tree_.grid.clear();
00059     tree_.size = 0;
00060 }
00061 
00062 void ompl::geometric::EST::freeMemory(void)
00063 {
00064     for (Grid<MotionSet>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00065     {
00066         for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00067         {
00068             if (it->second->data[i]->state)
00069                 si_->freeState(it->second->data[i]->state);
00070             delete it->second->data[i];
00071         }
00072     }
00073 }
00074 
00075 bool ompl::geometric::EST::solve(const base::PlannerTerminationCondition &ptc)
00076 {
00077     checkValidity();
00078     base::Goal                   *goal = pdef_->getGoal().get();
00079     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00080 
00081     while (const base::State *st = pis_.nextStart())
00082     {
00083         Motion *motion = new Motion(si_);
00084         si_->copyState(motion->state, st);
00085         addMotion(motion);
00086     }
00087 
00088     if (tree_.grid.size() == 0)
00089     {
00090         msg_.error("There are no valid initial states!");
00091         return false;
00092     }
00093 
00094     if (!sampler_)
00095         sampler_ = si_->allocValidStateSampler();
00096 
00097     msg_.inform("Starting with %u states", tree_.size);
00098 
00099     Motion *solution  = NULL;
00100     Motion *approxsol = NULL;
00101     double  approxdif = std::numeric_limits<double>::infinity();
00102     base::State *xstate = si_->allocState();
00103 
00104     while (ptc() == false)
00105     {
00106         /* Decide on a state to expand from */
00107         Motion *existing = selectMotion();
00108         assert(existing);
00109 
00110         /* sample random state (with goal biasing) */
00111         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00112             goal_s->sampleGoal(xstate);
00113         else
00114             if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
00115                 continue;
00116 
00117         if (si_->checkMotion(existing->state, xstate))
00118         {
00119             /* create a motion */
00120             Motion *motion = new Motion(si_);
00121             si_->copyState(motion->state, xstate);
00122             motion->parent = existing;
00123 
00124             addMotion(motion);
00125             double dist = 0.0;
00126             bool solved = goal->isSatisfied(motion->state, &dist);
00127             if (solved)
00128             {
00129                 approxdif = dist;
00130                 solution = motion;
00131                 break;
00132             }
00133             if (dist < approxdif)
00134             {
00135                 approxdif = dist;
00136                 approxsol = motion;
00137             }
00138         }
00139     }
00140 
00141     bool approximate = false;
00142     if (solution == NULL)
00143     {
00144         solution = approxsol;
00145         approximate = true;
00146     }
00147 
00148     if (solution != NULL)
00149     {
00150         /* construct the solution path */
00151         std::vector<Motion*> mpath;
00152         while (solution != NULL)
00153         {
00154             mpath.push_back(solution);
00155             solution = solution->parent;
00156         }
00157 
00158         /* set the solution path */
00159         PathGeometric *path = new PathGeometric(si_);
00160            for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00161             path->states.push_back(si_->cloneState(mpath[i]->state));
00162         goal->setDifference(approxdif);
00163         goal->setSolutionPath(base::PathPtr(path), approximate);
00164 
00165         if (approximate)
00166             msg_.warn("Found approximate solution");
00167     }
00168 
00169     si_->freeState(xstate);
00170 
00171     msg_.inform("Created %u states in %u cells", tree_.size, tree_.grid.size());
00172 
00173     return goal->isAchieved();
00174 }
00175 
00176 ompl::geometric::EST::Motion* ompl::geometric::EST::selectMotion(void)
00177 {
00178     double sum  = 0.0;
00179     Grid<MotionSet>::Cell* cell = NULL;
00180     double prob = rng_.uniform01() * (tree_.grid.size() - 1);
00181     for (Grid<MotionSet>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00182     {
00183         sum += (double)(tree_.size - it->second->data.size()) / (double)tree_.size;
00184         if (prob < sum)
00185         {
00186             cell = it->second;
00187             break;
00188         }
00189     }
00190     if (!cell && tree_.grid.size() > 0)
00191         cell = tree_.grid.begin()->second;
00192     return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00193 }
00194 
00195 void ompl::geometric::EST::addMotion(Motion *motion)
00196 {
00197     Grid<MotionSet>::Coord coord;
00198     projectionEvaluator_->computeCoordinates(motion->state, coord);
00199     Grid<MotionSet>::Cell* cell = tree_.grid.getCell(coord);
00200     if (cell)
00201         cell->data.push_back(motion);
00202     else
00203     {
00204         cell = tree_.grid.createCell(coord);
00205         cell->data.push_back(motion);
00206         tree_.grid.add(cell);
00207     }
00208     tree_.size++;
00209 }
00210 
00211 void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
00212 {
00213     Planner::getPlannerData(data);
00214 
00215     std::vector<MotionSet> motions;
00216     tree_.grid.getContent(motions);
00217 
00218     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00219         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00220             data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00221 }