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