All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
RRTConnect.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/RRTConnect.h"
00038 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 
00042 void ompl::geometric::RRTConnect::setup(void)
00043 {
00044     Planner::setup();
00045     SelfConfig sc(si_, getName());
00046     sc.configurePlannerRange(maxDistance_);
00047 
00048     if (!tStart_)
00049         tStart_.reset(new NearestNeighborsGNAT<Motion*>());
00050     if (!tGoal_)
00051         tGoal_.reset(new NearestNeighborsGNAT<Motion*>());
00052     tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
00053     tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
00054 }
00055 
00056 void ompl::geometric::RRTConnect::freeMemory(void)
00057 {
00058     std::vector<Motion*> motions;
00059 
00060     if (tStart_)
00061     {
00062         tStart_->list(motions);
00063         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00064         {
00065             if (motions[i]->state)
00066                 si_->freeState(motions[i]->state);
00067             delete motions[i];
00068         }
00069     }
00070 
00071     if (tGoal_)
00072     {
00073         tGoal_->list(motions);
00074         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00075         {
00076             if (motions[i]->state)
00077                 si_->freeState(motions[i]->state);
00078             delete motions[i];
00079         }
00080     }
00081 }
00082 
00083 void ompl::geometric::RRTConnect::clear(void)
00084 {
00085     Planner::clear();
00086     sampler_.reset();
00087     freeMemory();
00088     if (tStart_)
00089         tStart_->clear();
00090     if (tGoal_)
00091         tGoal_->clear();
00092 }
00093 
00094 ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
00095 {
00096     /* find closest state in the tree */
00097     Motion *nmotion = tree->nearest(rmotion);
00098 
00099     /* assume we can reach the state we go towards */
00100     bool reach = true;
00101 
00102     /* find state to add */
00103     base::State *dstate = rmotion->state;
00104     double d = si_->distance(nmotion->state, rmotion->state);
00105     if (d > maxDistance_)
00106     {
00107         si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
00108         dstate = tgi.xstate;
00109         reach = false;
00110     }
00111 
00112     if (si_->checkMotion(nmotion->state, dstate))
00113     {
00114         /* create a motion */
00115         Motion *motion = new Motion(si_);
00116         si_->copyState(motion->state, dstate);
00117         motion->parent = nmotion;
00118         motion->root = nmotion->root;
00119         tgi.xmotion = motion;
00120 
00121         tree->add(motion);
00122         if (reach)
00123             return REACHED;
00124         else
00125             return ADVANCED;
00126     }
00127     else
00128         return TRAPPED;
00129 }
00130 
00131 bool ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
00132 {
00133     checkValidity();
00134     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00135 
00136     if (!goal)
00137     {
00138         msg_.error("Unknown type of goal (or goal undefined)");
00139         return false;
00140     }
00141 
00142     while (const base::State *st = pis_.nextStart())
00143     {
00144         Motion *motion = new Motion(si_);
00145         si_->copyState(motion->state, st);
00146         motion->root = motion->state;
00147         tStart_->add(motion);
00148     }
00149 
00150     if (tStart_->size() == 0)
00151     {
00152         msg_.error("Motion planning start tree could not be initialized!");
00153         return false;
00154     }
00155 
00156     if (!goal->canSample())
00157     {
00158         msg_.error("Insufficient states in sampleable goal region");
00159         return false;
00160     }
00161 
00162     if (!sampler_)
00163         sampler_ = si_->allocStateSampler();
00164 
00165     msg_.inform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));
00166 
00167     TreeGrowingInfo tgi;
00168     tgi.xstate = si_->allocState();
00169 
00170     Motion   *rmotion   = new Motion(si_);
00171     base::State *rstate = rmotion->state;
00172     bool   startTree    = true;
00173 
00174     while (ptc() == false)
00175     {
00176         TreeData &tree      = startTree ? tStart_ : tGoal_;
00177         startTree = !startTree;
00178         TreeData &otherTree = startTree ? tStart_ : tGoal_;
00179 
00180         if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
00181         {
00182             const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00183             if (st)
00184             {
00185                 Motion* motion = new Motion(si_);
00186                 si_->copyState(motion->state, st);
00187                 motion->root = motion->state;
00188                 tGoal_->add(motion);
00189             }
00190 
00191             if (tGoal_->size() == 0)
00192             {
00193                 msg_.error("Unable to sample any valid states for goal tree");
00194                 break;
00195             }
00196         }
00197 
00198         /* sample random state */
00199         sampler_->sampleUniform(rstate);
00200 
00201         GrowState gs = growTree(tree, tgi, rmotion);
00202 
00203         if (gs != TRAPPED)
00204         {
00205             /* remember which motion was just added */
00206             Motion *addedMotion = tgi.xmotion;
00207 
00208             /* attempt to connect trees */
00209 
00210             /* if reached, it means we used rstate directly, no need top copy again */
00211             if (gs != REACHED)
00212                 si_->copyState(rstate, tgi.xstate);
00213 
00214             GrowState gsc = ADVANCED;
00215             while (gsc == ADVANCED)
00216                 gsc = growTree(otherTree, tgi, rmotion);
00217 
00218             /* if we connected the trees in a valid way (start and goal pair is valid)*/
00219             if (gsc == REACHED && goal->isStartGoalPairValid(startTree ? tgi.xmotion->root : addedMotion->root,
00220                                                              startTree ? addedMotion->root : tgi.xmotion->root))
00221             {
00222                 /* construct the solution path */
00223                 Motion *solution = tgi.xmotion;
00224                 std::vector<Motion*> mpath1;
00225                 while (solution != NULL)
00226                 {
00227                     mpath1.push_back(solution);
00228                     solution = solution->parent;
00229                 }
00230 
00231                 solution = addedMotion;
00232                 std::vector<Motion*> mpath2;
00233                 while (solution != NULL)
00234                 {
00235                     mpath2.push_back(solution);
00236                     solution = solution->parent;
00237                 }
00238 
00239                 if (!startTree)
00240                     mpath2.swap(mpath1);
00241 
00242                 PathGeometric *path = new PathGeometric(si_);
00243                 path->states.reserve(mpath1.size() + mpath2.size());
00244                 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00245                     path->states.push_back(si_->cloneState(mpath1[i]->state));
00246                 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00247                     path->states.push_back(si_->cloneState(mpath2[i]->state));
00248 
00249                 goal->setDifference(0.0);
00250                 goal->setSolutionPath(base::PathPtr(path));
00251                 break;
00252             }
00253         }
00254     }
00255 
00256     si_->freeState(tgi.xstate);
00257     si_->freeState(rstate);
00258     delete rmotion;
00259 
00260     msg_.inform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
00261 
00262     return goal->isAchieved();
00263 }
00264 
00265 void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
00266 {
00267     Planner::getPlannerData(data);
00268 
00269     std::vector<Motion*> motions;
00270     if (tStart_)
00271         tStart_->list(motions);
00272 
00273     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00274     {
00275         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00276         data.tagState(motions[i]->state, 1);
00277     }
00278 
00279     motions.clear();
00280     if (tGoal_)
00281         tGoal_->list(motions);
00282 
00283     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00284     {
00285         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00286         data.tagState(motions[i]->state, 2);
00287     }
00288 }