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