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