ompl/control/planners/rrt/src/RRT.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/control/planners/rrt/RRT.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 #include "ompl/tools/config/SelfConfig.h" 00040 #include <limits> 00041 00042 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT") 00043 { 00044 specs_.approximateSolutions = true; 00045 siC_ = si.get(); 00046 addIntermediateStates_ = false; 00047 lastGoalMotion_ = NULL; 00048 00049 goalBias_ = 0.05; 00050 00051 Planner::declareParam<double>("goal_bias", this, &RRT::setGoalBias, &RRT::getGoalBias, "0.:.05:1."); 00052 Planner::declareParam<bool>("intermediate_states", this, &RRT::setIntermediateStates, &RRT::getIntermediateStates); 00053 } 00054 00055 ompl::control::RRT::~RRT() 00056 { 00057 freeMemory(); 00058 } 00059 00060 void ompl::control::RRT::setup() 00061 { 00062 base::Planner::setup(); 00063 if (!nn_) 00064 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace())); 00065 nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2)); 00066 } 00067 00068 void ompl::control::RRT::clear() 00069 { 00070 Planner::clear(); 00071 sampler_.reset(); 00072 controlSampler_.reset(); 00073 freeMemory(); 00074 if (nn_) 00075 nn_->clear(); 00076 lastGoalMotion_ = NULL; 00077 } 00078 00079 void ompl::control::RRT::freeMemory() 00080 { 00081 if (nn_) 00082 { 00083 std::vector<Motion*> motions; 00084 nn_->list(motions); 00085 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00086 { 00087 if (motions[i]->state) 00088 si_->freeState(motions[i]->state); 00089 if (motions[i]->control) 00090 siC_->freeControl(motions[i]->control); 00091 delete motions[i]; 00092 } 00093 } 00094 } 00095 00096 ompl::base::PlannerStatus ompl::control::RRT::solve(const base::PlannerTerminationCondition &ptc) 00097 { 00098 checkValidity(); 00099 base::Goal *goal = pdef_->getGoal().get(); 00100 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); 00101 00102 while (const base::State *st = pis_.nextStart()) 00103 { 00104 Motion *motion = new Motion(siC_); 00105 si_->copyState(motion->state, st); 00106 siC_->nullControl(motion->control); 00107 nn_->add(motion); 00108 } 00109 00110 if (nn_->size() == 0) 00111 { 00112 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00113 return base::PlannerStatus::INVALID_START; 00114 } 00115 00116 if (!sampler_) 00117 sampler_ = si_->allocStateSampler(); 00118 if (!controlSampler_) 00119 controlSampler_ = siC_->allocDirectedControlSampler(); 00120 00121 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size()); 00122 00123 Motion *solution = NULL; 00124 Motion *approxsol = NULL; 00125 double approxdif = std::numeric_limits<double>::infinity(); 00126 00127 Motion *rmotion = new Motion(siC_); 00128 base::State *rstate = rmotion->state; 00129 Control *rctrl = rmotion->control; 00130 base::State *xstate = si_->allocState(); 00131 00132 while (ptc == false) 00133 { 00134 /* sample random state (with goal biasing) */ 00135 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) 00136 goal_s->sampleGoal(rstate); 00137 else 00138 sampler_->sampleUniform(rstate); 00139 00140 /* find closest state in the tree */ 00141 Motion *nmotion = nn_->nearest(rmotion); 00142 00143 /* sample a random control that attempts to go towards the random state, and also sample a control duration */ 00144 unsigned int cd = controlSampler_->sampleTo(rctrl, nmotion->control, nmotion->state, rmotion->state); 00145 00146 if (addIntermediateStates_) 00147 { 00148 // this code is contributed by Jennifer Barry 00149 std::vector<base::State *> pstates; 00150 cd = siC_->propagateWhileValid(nmotion->state, rctrl, cd, pstates, true); 00151 00152 if (cd >= siC_->getMinControlDuration()) 00153 { 00154 Motion *lastmotion = nmotion; 00155 bool solved = false; 00156 size_t p = 0; 00157 for ( ; p < pstates.size(); ++p) 00158 { 00159 /* create a motion */ 00160 Motion *motion = new Motion(); 00161 motion->state = pstates[p]; 00162 //we need multiple copies of rctrl 00163 motion->control = siC_->allocControl(); 00164 siC_->copyControl(motion->control, rctrl); 00165 motion->steps = 1; 00166 motion->parent = lastmotion; 00167 lastmotion = motion; 00168 nn_->add(motion); 00169 double dist = 0.0; 00170 solved = goal->isSatisfied(motion->state, &dist); 00171 if (solved) 00172 { 00173 approxdif = dist; 00174 solution = motion; 00175 break; 00176 } 00177 if (dist < approxdif) 00178 { 00179 approxdif = dist; 00180 approxsol = motion; 00181 } 00182 } 00183 00184 //free any states after we hit the goal 00185 while (++p < pstates.size()) 00186 si_->freeState(pstates[p]); 00187 if (solved) 00188 break; 00189 } 00190 else 00191 for (size_t p = 0 ; p < pstates.size(); ++p) 00192 si_->freeState(pstates[p]); 00193 } 00194 else 00195 { 00196 if (cd >= siC_->getMinControlDuration()) 00197 { 00198 /* create a motion */ 00199 Motion *motion = new Motion(siC_); 00200 si_->copyState(motion->state, rmotion->state); 00201 siC_->copyControl(motion->control, rctrl); 00202 motion->steps = cd; 00203 motion->parent = nmotion; 00204 00205 nn_->add(motion); 00206 double dist = 0.0; 00207 bool solv = goal->isSatisfied(motion->state, &dist); 00208 if (solv) 00209 { 00210 approxdif = dist; 00211 solution = motion; 00212 break; 00213 } 00214 if (dist < approxdif) 00215 { 00216 approxdif = dist; 00217 approxsol = motion; 00218 } 00219 } 00220 } 00221 } 00222 00223 bool solved = false; 00224 bool approximate = false; 00225 if (solution == NULL) 00226 { 00227 solution = approxsol; 00228 approximate = true; 00229 } 00230 00231 if (solution != NULL) 00232 { 00233 lastGoalMotion_ = solution; 00234 00235 /* construct the solution path */ 00236 std::vector<Motion*> mpath; 00237 while (solution != NULL) 00238 { 00239 mpath.push_back(solution); 00240 solution = solution->parent; 00241 } 00242 00243 /* set the solution path */ 00244 PathControl *path = new PathControl(si_); 00245 for (int i = mpath.size() - 1 ; i >= 0 ; --i) 00246 if (mpath[i]->parent) 00247 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); 00248 else 00249 path->append(mpath[i]->state); 00250 solved = true; 00251 pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName()); 00252 } 00253 00254 if (rmotion->state) 00255 si_->freeState(rmotion->state); 00256 if (rmotion->control) 00257 siC_->freeControl(rmotion->control); 00258 delete rmotion; 00259 si_->freeState(xstate); 00260 00261 OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size()); 00262 00263 return base::PlannerStatus(solved, approximate); 00264 } 00265 00266 void ompl::control::RRT::getPlannerData(base::PlannerData &data) const 00267 { 00268 Planner::getPlannerData(data); 00269 00270 std::vector<Motion*> motions; 00271 if (nn_) 00272 nn_->list(motions); 00273 00274 double delta = siC_->getPropagationStepSize(); 00275 00276 if (lastGoalMotion_) 00277 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); 00278 00279 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00280 { 00281 const Motion *m = motions[i]; 00282 if (m->parent) 00283 { 00284 if (data.hasControls()) 00285 data.addEdge(base::PlannerDataVertex(m->parent->state), 00286 base::PlannerDataVertex(m->state), 00287 control::PlannerDataEdgeControl(m->control, m->steps * delta)); 00288 else 00289 data.addEdge(base::PlannerDataVertex(m->parent->state), 00290 base::PlannerDataVertex(m->state)); 00291 } 00292 else 00293 data.addStartVertex(base::PlannerDataVertex(m->state)); 00294 } 00295 }