All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
EST.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
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 Rice University 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: Ryan Luna */
00036 
00037 #include "ompl/control/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 ompl::control::EST::EST(const SpaceInformationPtr &si) : base::Planner(si, "EST")
00044 {
00045     specs_.approximateSolutions = true;
00046     goalBias_ = 0.05;
00047     maxDistance_ = 0.0;
00048     siC_ = si.get();
00049 
00050     Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange);
00051     Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias);
00052 }
00053 
00054 ompl::control::EST::~EST(void)
00055 {
00056     freeMemory();
00057 }
00058 
00059 void ompl::control::EST::setup(void)
00060 {
00061     Planner::setup();
00062     tools::SelfConfig sc(si_, getName());
00063     sc.configureProjectionEvaluator(projectionEvaluator_);
00064     sc.configurePlannerRange(maxDistance_);
00065 
00066     tree_.grid.setDimension(projectionEvaluator_->getDimension());
00067 }
00068 
00069 void ompl::control::EST::clear(void)
00070 {
00071     Planner::clear();
00072     sampler_.reset();
00073     controlSampler_.reset();
00074     freeMemory();
00075     tree_.grid.clear();
00076     tree_.size = 0;
00077     pdf_.clear ();
00078 }
00079 
00080 void ompl::control::EST::freeMemory(void)
00081 {
00082     for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00083     {
00084         for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00085         {
00086             if (it->second->data[i]->state)
00087                 si_->freeState(it->second->data[i]->state);
00088             if (it->second->data[i]->control)
00089                 siC_->freeControl(it->second->data[i]->control);
00090             delete it->second->data[i];
00091         }
00092     }
00093 }
00094 
00095 bool ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc)
00096 {
00097     checkValidity();
00098     base::Goal                   *goal = pdef_->getGoal().get();
00099     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00100 
00101     // Initializing tree with start state(s)
00102     while (const base::State *st = pis_.nextStart())
00103     {
00104         Motion *motion = new Motion(siC_);
00105         si_->copyState(motion->state, st);
00106         siC_->nullControl(motion->control);
00107         addMotion(motion);
00108     }
00109 
00110     if (tree_.grid.size() == 0)
00111     {
00112         msg_.error("There are no valid initial states!");
00113         return false;
00114     }
00115 
00116     // Ensure that we have a state sampler AND a control sampler
00117     if (!sampler_)
00118         sampler_ = si_->allocValidStateSampler();
00119     if (!controlSampler_)
00120         controlSampler_ = siC_->allocDirectedControlSampler();
00121 
00122     msg_.inform("Starting with %u states", tree_.size);
00123 
00124     Motion *solution = NULL;
00125     double   slndist = std::numeric_limits<double>::infinity();
00126     Motion  *rmotion = new Motion(siC_);
00127     bool      solved = false;
00128 
00129     while (!ptc())
00130     {
00131         // Select a state to expand the tree from
00132         Motion *existing = selectMotion();
00133         assert (existing);
00134 
00135         // sample a random state (with goal biasing) near the state selected for expansion
00136         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00137             goal_s->sampleGoal(rmotion->state);
00138         else
00139         {
00140             if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
00141                 continue;
00142         }
00143 
00144         // Extend a motion toward the state we just sampled
00145         unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
00146                                                           existing->state, rmotion->state);
00147 
00148         // Propagate the system from the state selected for expansion using the control we
00149         // just sampled for the given duration.  Save the resulting state into rmotion->state.
00150         duration = siC_->propagateWhileValid(existing->state, rmotion->control, duration, rmotion->state);
00151 
00152         // If the system was propagated for a meaningful amount of time, save into the tree
00153         if (duration >= siC_->getMinControlDuration())
00154         {
00155             // create a motion to the resulting state
00156             Motion *motion = new Motion(siC_);
00157             si_->copyState(motion->state, rmotion->state);
00158             siC_->copyControl(motion->control, rmotion->control);
00159             motion->steps = duration;
00160             motion->parent = existing;
00161 
00162             // save the state
00163             addMotion(motion);
00164 
00165             // Check if this state is the goal state, or improves the best solution so far
00166             double dist = 0.0;
00167             solved = goal->isSatisfied(motion->state, &dist);
00168             if (solved || dist < slndist)
00169             {
00170                 slndist = dist;
00171                 solution = motion;
00172 
00173                 if (solved)
00174                     break;
00175             }
00176         }
00177     }
00178 
00179     bool addedSolution = false;
00180 
00181     // Constructing the solution path
00182     if (solution != NULL)
00183     {
00184         std::vector<Motion*> mpath;
00185         while (solution != NULL)
00186         {
00187             mpath.push_back(solution);
00188             solution = solution->parent;
00189         }
00190 
00191         PathControl *path = new PathControl(si_);
00192         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00193             if (mpath[i]->parent)
00194                 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00195             else
00196                 path->append(mpath[i]->state);
00197         addedSolution = true;
00198         goal->addSolutionPath(base::PathPtr(path), !solved, slndist);
00199     }
00200 
00201     // Cleaning up memory
00202     if (rmotion->state)
00203         si_->freeState(rmotion->state);
00204     if (rmotion->control)
00205         siC_->freeControl(rmotion->control);
00206     delete rmotion;
00207 
00208     msg_.inform("Created %u states in %u cells", tree_.size, tree_.grid.size());
00209 
00210     return addedSolution;
00211 }
00212 
00213 ompl::control::EST::Motion* ompl::control::EST::selectMotion(void)
00214 {
00215     GridCell* cell = pdf_.sample(rng_.uniform01());
00216     return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00217 }
00218 
00219 void ompl::control::EST::addMotion(Motion *motion)
00220 {
00221     Grid<MotionInfo>::Coord coord;
00222     projectionEvaluator_->computeCoordinates(motion->state, coord);
00223     GridCell* cell = tree_.grid.getCell(coord);
00224     if (cell)
00225     {
00226         cell->data.push_back(motion);
00227         pdf_.update(cell->data.elem_, 1.0/cell->data.size());
00228     }
00229     else
00230     {
00231         cell = tree_.grid.createCell(coord);
00232         cell->data.push_back(motion);
00233         tree_.grid.add(cell);
00234         cell->data.elem_ = pdf_.add(cell, 1.0);
00235     }
00236     tree_.size++;
00237 }
00238 
00239 void ompl::control::EST::getPlannerData(base::PlannerData &data) const
00240 {
00241     Planner::getPlannerData(data);
00242 
00243     std::vector<MotionInfo> motions;
00244     tree_.grid.getContent(motions);
00245 
00246     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00247         for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00248             data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00249 }