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