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 }