All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
LazyRRT.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/rrt/LazyRRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <cassert>
00042 
00043 void ompl::geometric::LazyRRT::setup(void)
00044 {
00045     Planner::setup();
00046     SelfConfig sc(si_, getName());
00047     sc.configurePlannerRange(maxDistance_);
00048 
00049     if (!nn_)
00050         nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00051     nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
00052 }
00053 
00054 void ompl::geometric::LazyRRT::clear(void)
00055 {
00056     Planner::clear();
00057     sampler_.reset();
00058     freeMemory();
00059     if (nn_)
00060         nn_->clear();
00061 }
00062 
00063 void ompl::geometric::LazyRRT::freeMemory(void)
00064 {
00065     if (nn_)
00066     {
00067         std::vector<Motion*> motions;
00068         nn_->list(motions);
00069         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00070         {
00071             if (motions[i]->state)
00072                 si_->freeState(motions[i]->state);
00073             delete motions[i];
00074         }
00075     }
00076 }
00077 
00078 bool ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
00079 {
00080     checkValidity();
00081     base::Goal                 *goal   = pdef_->getGoal().get();
00082     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00083 
00084     while (const base::State *st = pis_.nextStart())
00085     {
00086         Motion *motion = new Motion(si_);
00087         si_->copyState(motion->state, st);
00088         motion->valid = true;
00089         nn_->add(motion);
00090     }
00091 
00092     if (nn_->size() == 0)
00093     {
00094         msg_.error("There are no valid initial states!");
00095         return false;
00096     }
00097 
00098     if (!sampler_)
00099         sampler_ = si_->allocStateSampler();
00100 
00101     msg_.inform("Starting with %u states", nn_->size());
00102 
00103     Motion *solution = NULL;
00104     double  distsol  = -1.0;
00105     Motion *rmotion  = new Motion(si_);
00106     base::State *rstate = rmotion->state;
00107     base::State *xstate = si_->allocState();
00108 
00109  RETRY:
00110 
00111     while (ptc() == false)
00112     {
00113         /* sample random state (with goal biasing) */
00114         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00115             goal_s->sampleGoal(rstate);
00116         else
00117             sampler_->sampleUniform(rstate);
00118 
00119         /* find closest state in the tree */
00120         Motion *nmotion = nn_->nearest(rmotion);
00121         assert(nmotion != rmotion);
00122         base::State *dstate = rstate;
00123 
00124         /* find state to add */
00125         double d = si_->distance(nmotion->state, rstate);
00126         if (d > maxDistance_)
00127         {
00128             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00129             dstate = xstate;
00130         }
00131 
00132         /* create a motion */
00133         Motion *motion = new Motion(si_);
00134         si_->copyState(motion->state, dstate);
00135         motion->parent = nmotion;
00136         nmotion->children.push_back(motion);
00137         nn_->add(motion);
00138 
00139         double dist = 0.0;
00140         if (goal->isSatisfied(motion->state, &dist))
00141         {
00142             distsol = dist;
00143             solution = motion;
00144             break;
00145         }
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         /* check the path */
00159         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00160             if (!mpath[i]->valid)
00161             {
00162                 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
00163                     mpath[i]->valid = true;
00164                 else
00165                 {
00166                     removeMotion(mpath[i]);
00167                     goto RETRY;
00168                 }
00169             }
00170 
00171         /* set the solution path */
00172         PathGeometric *path = new PathGeometric(si_);
00173         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00174             path->states.push_back(si_->cloneState(mpath[i]->state));
00175 
00176         goal->setDifference(distsol);
00177         goal->setSolutionPath(base::PathPtr(path));
00178 
00179     }
00180 
00181     si_->freeState(xstate);
00182     si_->freeState(rstate);
00183     delete rmotion;
00184 
00185     msg_.inform("Created %u states", nn_->size());
00186 
00187     return goal->isAchieved();
00188 }
00189 
00190 void ompl::geometric::LazyRRT::removeMotion(Motion *motion)
00191 {
00192     nn_->remove(motion);
00193 
00194     /* remove self from parent list */
00195 
00196     if (motion->parent)
00197     {
00198         for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00199             if (motion->parent->children[i] == motion)
00200             {
00201                 motion->parent->children.erase(motion->parent->children.begin() + i);
00202                 break;
00203             }
00204     }
00205 
00206     /* remove children */
00207     for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00208     {
00209         motion->children[i]->parent = NULL;
00210         removeMotion(motion->children[i]);
00211     }
00212 
00213     if (motion->state)
00214         si_->freeState(motion->state);
00215     delete motion;
00216 }
00217 
00218 void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
00219 {
00220     Planner::getPlannerData(data);
00221 
00222     std::vector<Motion*> motions;
00223     if (nn_)
00224         nn_->list(motions);
00225 
00226     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00227     {
00228         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00229         if (motions[i]->valid)
00230             data.tagState(motions[i]->state, 1);
00231     }
00232 }