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