All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
RRT.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/RRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <limits>
00042 
00043 void ompl::geometric::RRT::clear(void)
00044 {
00045     Planner::clear();
00046     sampler_.reset();
00047     freeMemory();
00048     if (nn_)
00049         nn_->clear();
00050 }
00051 
00052 void ompl::geometric::RRT::setup(void)
00053 {
00054     Planner::setup();
00055     SelfConfig sc(si_, getName());
00056     sc.configurePlannerRange(maxDistance_);
00057 
00058     if (!nn_)
00059         nn_.reset(new NearestNeighborsGNAT<Motion*>());
00060     nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
00061 }
00062 
00063 void ompl::geometric::RRT::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::RRT::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         nn_->add(motion);
00089     }
00090 
00091     if (nn_->size() == 0)
00092     {
00093         msg_.error("There are no valid initial states!");
00094         return false;
00095     }
00096 
00097     if (!sampler_)
00098         sampler_ = si_->allocStateSampler();
00099 
00100     msg_.inform("Starting with %u states", nn_->size());
00101 
00102     Motion *solution  = NULL;
00103     Motion *approxsol = NULL;
00104     double  approxdif = std::numeric_limits<double>::infinity();
00105     Motion *rmotion   = new Motion(si_);
00106     base::State *rstate = rmotion->state;
00107     base::State *xstate = si_->allocState();
00108 
00109     while (ptc() == false)
00110     {
00111 
00112         /* sample random state (with goal biasing) */
00113         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00114             goal_s->sampleGoal(rstate);
00115         else
00116             sampler_->sampleUniform(rstate);
00117 
00118         /* find closest state in the tree */
00119         Motion *nmotion = nn_->nearest(rmotion);
00120         base::State *dstate = rstate;
00121 
00122         /* find state to add */
00123         double d = si_->distance(nmotion->state, rstate);
00124         if (d > maxDistance_)
00125         {
00126             si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00127             dstate = xstate;
00128         }
00129 
00130         if (si_->checkMotion(nmotion->state, dstate))
00131         {
00132             /* create a motion */
00133             Motion *motion = new Motion(si_);
00134             si_->copyState(motion->state, dstate);
00135             motion->parent = nmotion;
00136 
00137             nn_->add(motion);
00138             double dist = 0.0;
00139             bool solved = goal->isSatisfied(motion->state, &dist);
00140             if (solved)
00141             {
00142                 approxdif = dist;
00143                 solution = motion;
00144                 break;
00145             }
00146             if (dist < approxdif)
00147             {
00148                 approxdif = dist;
00149                 approxsol = motion;
00150             }
00151         }
00152     }
00153 
00154     bool approximate = false;
00155     if (solution == NULL)
00156     {
00157         solution = approxsol;
00158         approximate = true;
00159     }
00160 
00161     if (solution != NULL)
00162     {
00163         /* construct the solution path */
00164         std::vector<Motion*> mpath;
00165         while (solution != NULL)
00166         {
00167             mpath.push_back(solution);
00168             solution = solution->parent;
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         goal->setDifference(approxdif);
00176         goal->setSolutionPath(base::PathPtr(path), approximate);
00177 
00178         if (approximate)
00179             msg_.warn("Found approximate solution");
00180     }
00181 
00182     si_->freeState(xstate);
00183     if (rmotion->state)
00184         si_->freeState(rmotion->state);
00185     delete rmotion;
00186 
00187     msg_.inform("Created %u states", nn_->size());
00188 
00189     return goal->isAchieved();
00190 }
00191 
00192 void ompl::geometric::RRT::getPlannerData(base::PlannerData &data) const
00193 {
00194     Planner::getPlannerData(data);
00195 
00196     std::vector<Motion*> motions;
00197     if (nn_)
00198         nn_->list(motions);
00199 
00200     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00201         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00202 }