ompl/geometric/planners/rrt/src/TRRT.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: Dave Coleman */ 00036 00037 #include "ompl/geometric/planners/rrt/TRRT.h" 00038 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h" 00039 #include "ompl/base/goals/GoalSampleableRegion.h" 00040 #include "ompl/tools/config/SelfConfig.h" 00041 #include "ompl/tools/config/MagicConstants.h" 00042 #include <limits> 00043 00044 ompl::geometric::TRRT::TRRT(const base::SpaceInformationPtr &si) : base::Planner(si, "TRRT") 00045 { 00046 // Standard RRT Variables 00047 specs_.approximateSolutions = true; 00048 specs_.directed = true; 00049 00050 goalBias_ = 0.05; 00051 maxDistance_ = 0.0; // set in setup() 00052 lastGoalMotion_ = NULL; 00053 00054 Planner::declareParam<double>("range", this, &TRRT::setRange, &TRRT::getRange, "0.:1.:10000."); 00055 Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1."); 00056 00057 // TRRT Specific Variables 00058 frontierThreshold_ = 0.0; // set in setup() 00059 kConstant_ = 0.0; // set in setup() 00060 maxStatesFailed_ = 10; // threshold for when to start increasing the temperatuer 00061 tempChangeFactor_ = 2.0; // how much to decrease or increase the temp each time 00062 minTemperature_ = 10e-10; // lower limit of the temperature change 00063 initTemperature_ = 10e-6; // where the temperature starts out 00064 frontierNodeRatio_ = 0.1; // 1/10, or 1 nonfrontier for every 10 frontier 00065 00066 Planner::declareParam<unsigned int>("max_states_failed", this, &TRRT::setMaxStatesFailed, &TRRT::getMaxStatesFailed, "1:1000"); 00067 Planner::declareParam<double>("temp_change_factor", this, &TRRT::setTempChangeFactor, &TRRT::getTempChangeFactor,"0.:.1:10."); 00068 Planner::declareParam<double>("min_temperature", this, &TRRT::setMinTemperature, &TRRT::getMinTemperature); 00069 Planner::declareParam<double>("init_temperature", this, &TRRT::setInitTemperature, &TRRT::getInitTemperature); 00070 Planner::declareParam<double>("frontier_threshold", this, &TRRT::setFrontierThreshold, &TRRT::getFrontierThreshold); 00071 Planner::declareParam<double>("frontierNodeRatio", this, &TRRT::setFrontierNodeRatio, &TRRT::getFrontierNodeRatio); 00072 Planner::declareParam<double>("k_constant", this, &TRRT::setKConstant, &TRRT::getKConstant); 00073 } 00074 00075 ompl::geometric::TRRT::~TRRT() 00076 { 00077 freeMemory(); 00078 } 00079 00080 void ompl::geometric::TRRT::clear() 00081 { 00082 Planner::clear(); 00083 sampler_.reset(); 00084 freeMemory(); 00085 if (nearestNeighbors_) 00086 nearestNeighbors_->clear(); 00087 lastGoalMotion_ = NULL; 00088 00089 // Clear TRRT specific variables --------------------------------------------------------- 00090 numStatesFailed_ = 0; 00091 temp_ = initTemperature_; 00092 nonfrontierCount_ = 1; 00093 frontierCount_ = 1; // init to 1 to prevent division by zero error 00094 } 00095 00096 void ompl::geometric::TRRT::setup() 00097 { 00098 Planner::setup(); 00099 tools::SelfConfig selfConfig(si_, getName()); 00100 00101 bool usingDefaultObjective = false; 00102 if (!pdef_->hasOptimizationObjective()) 00103 { 00104 OMPL_INFORM("%s: No optimization objective specified.", getName().c_str()); 00105 usingDefaultObjective = true; 00106 } 00107 else 00108 { 00109 usingDefaultObjective = false; 00110 } 00111 00112 if (usingDefaultObjective) 00113 { 00114 opt_.reset(new base::MechanicalWorkOptimizationObjective(si_)); 00115 OMPL_INFORM("%s: Defaulting to optimizing path length.", getName().c_str()); 00116 } 00117 else 00118 opt_ = pdef_->getOptimizationObjective(); 00119 00120 // Set maximum distance a new node can be from its nearest neighbor 00121 if (maxDistance_ < std::numeric_limits<double>::epsilon()) 00122 { 00123 selfConfig.configurePlannerRange(maxDistance_); 00124 maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION; 00125 } 00126 00127 // Set the threshold that decides if a new node is a frontier node or non-frontier node 00128 if (frontierThreshold_ < std::numeric_limits<double>::epsilon()) 00129 { 00130 frontierThreshold_ = si_->getMaximumExtent() * 0.01; 00131 OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_); 00132 } 00133 00134 // Autoconfigure the K constant 00135 if (kConstant_ < std::numeric_limits<double>::epsilon()) 00136 { 00137 // Find the average cost of states by sampling 00138 double averageCost = opt_->averageStateCost(magic::TEST_STATE_COUNT).value(); 00139 kConstant_ = averageCost; 00140 OMPL_DEBUG("%s: K constant detected to be %lf", getName().c_str(), kConstant_); 00141 } 00142 00143 // Create the nearest neighbor function the first time setup is run 00144 if (!nearestNeighbors_) 00145 nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace())); 00146 00147 // Set the distance function 00148 nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2)); 00149 00150 // Setup TRRT specific variables --------------------------------------------------------- 00151 numStatesFailed_ = 0; 00152 temp_ = initTemperature_; 00153 nonfrontierCount_ = 1; 00154 frontierCount_ = 1; // init to 1 to prevent division by zero error 00155 } 00156 00157 void ompl::geometric::TRRT::freeMemory() 00158 { 00159 // Delete all motions, states and the nearest neighbors data structure 00160 if (nearestNeighbors_) 00161 { 00162 std::vector<Motion*> motions; 00163 nearestNeighbors_->list(motions); 00164 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00165 { 00166 if (motions[i]->state) 00167 si_->freeState(motions[i]->state); 00168 delete motions[i]; 00169 } 00170 } 00171 } 00172 00173 ompl::base::PlannerStatus 00174 ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTerminationCondition) 00175 { 00176 // Basic error checking 00177 checkValidity(); 00178 00179 // Goal information 00180 base::Goal *goal = pdef_->getGoal().get(); 00181 base::GoalSampleableRegion *goalRegion = dynamic_cast<base::GoalSampleableRegion*>(goal); 00182 00183 // Input States --------------------------------------------------------------------------------- 00184 00185 // Loop through valid input states and add to tree 00186 while (const base::State *state = pis_.nextStart()) 00187 { 00188 // Allocate memory for a new start state motion based on the "space-information"-size 00189 Motion *motion = new Motion(si_); 00190 00191 // Copy destination <= source 00192 si_->copyState(motion->state, state); 00193 00194 // Set cost for this start state 00195 motion->cost = opt_->stateCost(motion->state); 00196 00197 // Add start motion to the tree 00198 nearestNeighbors_->add(motion); 00199 } 00200 00201 // Check that input states exist 00202 if (nearestNeighbors_->size() == 0) 00203 { 00204 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00205 return base::PlannerStatus::INVALID_START; 00206 } 00207 00208 // Create state sampler if this is TRRT's first run 00209 if (!sampler_) 00210 sampler_ = si_->allocStateSampler(); 00211 00212 // Debug 00213 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nearestNeighbors_->size()); 00214 00215 00216 // Solver variables ------------------------------------------------------------------------------------ 00217 00218 // the final solution 00219 Motion *solution = NULL; 00220 // the approximate solution, returned if no final solution found 00221 Motion *approxSolution = NULL; 00222 // track the distance from goal to closest solution yet found 00223 double approxDifference = std::numeric_limits<double>::infinity(); 00224 00225 // distance between states - the intial state and the interpolated state (may be the same) 00226 double randMotionDistance; 00227 double motionDistance; 00228 00229 // Create random motion and a pointer (for optimization) to its state 00230 Motion *randMotion = new Motion(si_); 00231 Motion *nearMotion; 00232 00233 // STATES 00234 // The random state 00235 base::State *randState = randMotion->state; 00236 // The new state that is generated between states *to* and *from* 00237 base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state 00238 // The chosen state btw rand_state and interpolated_state 00239 base::State *newState; 00240 00241 // Begin sampling -------------------------------------------------------------------------------------- 00242 while (plannerTerminationCondition() == false) 00243 { 00244 // I. 00245 00246 // Sample random state (with goal biasing probability) 00247 if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample()) 00248 { 00249 // Bias sample towards goal 00250 goalRegion->sampleGoal(randState); 00251 } 00252 else 00253 { 00254 // Uniformly Sample 00255 sampler_->sampleUniform(randState); 00256 } 00257 00258 // II. 00259 00260 // Find closest state in the tree 00261 nearMotion = nearestNeighbors_->nearest(randMotion); 00262 00263 // III. 00264 00265 // Distance from near state q_n to a random state 00266 randMotionDistance = si_->distance(nearMotion->state, randState); 00267 00268 // Check if the rand_state is too far away 00269 if (randMotionDistance > maxDistance_) 00270 { 00271 // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state. 00272 // The memory location of *state* is not required to be different from the memory of either *from* or *to*. 00273 si_->getStateSpace()->interpolate(nearMotion->state, randState, 00274 maxDistance_ / randMotionDistance, interpolatedState); 00275 00276 // Update the distance between near and new with the interpolated_state 00277 motionDistance = si_->distance(nearMotion->state, interpolatedState); 00278 00279 // Use the interpolated state as the new state 00280 newState = interpolatedState; 00281 } 00282 else 00283 { 00284 // Random state is close enough 00285 newState = randState; 00286 00287 // Copy the distance 00288 motionDistance = randMotionDistance; 00289 } 00290 00291 // IV. 00292 // this stage integrates collision detections in the presence of obstacles and checks for collisions 00293 00301 if (!si_->checkMotion(nearMotion->state, newState)) 00302 continue; // try a new sample 00303 00304 00305 // Minimum Expansion Control 00306 // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the 00307 // new nodes contribute only to refine already explored regions. 00308 if (!minExpansionControl(randMotionDistance)) 00309 { 00310 continue; // give up on this one and try a new sample 00311 } 00312 00313 base::Cost childCost = opt_->stateCost(newState); 00314 00315 // Only add this motion to the tree if the tranistion test accepts it 00316 if (!transitionTest(childCost.value(), nearMotion->cost.value(), motionDistance)) 00317 { 00318 continue; // give up on this one and try a new sample 00319 } 00320 00321 // V. 00322 00323 // Create a motion 00324 Motion *motion = new Motion(si_); 00325 si_->copyState(motion->state, newState); 00326 motion->parent = nearMotion; // link q_new to q_near as an edge 00327 motion->cost = childCost; 00328 00329 // Add motion to data structure 00330 nearestNeighbors_->add(motion); 00331 00332 // VI. 00333 00334 // Check if this motion is the goal 00335 double distToGoal = 0.0; 00336 bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal); 00337 if (isSatisfied) 00338 { 00339 approxDifference = distToGoal; // the tolerated error distance btw state and goal 00340 solution = motion; // set the final solution 00341 break; 00342 } 00343 00344 // Is this the closest solution we've found so far 00345 if (distToGoal < approxDifference) 00346 { 00347 approxDifference = distToGoal; 00348 approxSolution = motion; 00349 } 00350 00351 } // end of solver sampling loop 00352 00353 00354 // Finish solution processing -------------------------------------------------------------------- 00355 00356 bool solved = false; 00357 bool approximate = false; 00358 00359 // Substitute an empty solution with the best approximation 00360 if (solution == NULL) 00361 { 00362 solution = approxSolution; 00363 approximate = true; 00364 } 00365 00366 // Generate solution path for real/approx solution 00367 if (solution != NULL) 00368 { 00369 lastGoalMotion_ = solution; 00370 00371 // construct the solution path 00372 std::vector<Motion*> mpath; 00373 while (solution != NULL) 00374 { 00375 mpath.push_back(solution); 00376 solution = solution->parent; 00377 } 00378 00379 // set the solution path 00380 PathGeometric *path = new PathGeometric(si_); 00381 for (int i = mpath.size() - 1 ; i >= 0 ; --i) 00382 path->append(mpath[i]->state); 00383 00384 pdef_->addSolutionPath(base::PathPtr(path), approximate, approxDifference, getName()); 00385 solved = true; 00386 } 00387 00388 // Clean up --------------------------------------------------------------------------------------- 00389 00390 si_->freeState(interpolatedState); 00391 if (randMotion->state) 00392 si_->freeState(randMotion->state); 00393 delete randMotion; 00394 00395 OMPL_INFORM("%s: Created %u states", getName().c_str(), nearestNeighbors_->size()); 00396 00397 return base::PlannerStatus(solved, approximate); 00398 } 00399 00400 void ompl::geometric::TRRT::getPlannerData(base::PlannerData &data) const 00401 { 00402 Planner::getPlannerData(data); 00403 00404 std::vector<Motion*> motions; 00405 if (nearestNeighbors_) 00406 nearestNeighbors_->list(motions); 00407 00408 if (lastGoalMotion_) 00409 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); 00410 00411 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00412 { 00413 if (motions[i]->parent == NULL) 00414 data.addStartVertex(base::PlannerDataVertex(motions[i]->state)); 00415 else 00416 data.addEdge(base::PlannerDataVertex(motions[i]->parent->state), 00417 base::PlannerDataVertex(motions[i]->state)); 00418 } 00419 } 00420 00421 bool ompl::geometric::TRRT::transitionTest(double childCost, double parentCost, double distance) 00422 { 00423 // Always accept if new state has same or lower cost than old state 00424 if (childCost <= parentCost) 00425 return true; 00426 00427 // Difference in cost 00428 double costSlope = (childCost - parentCost) / distance; 00429 00430 // The probability of acceptance of a new configuration is defined by comparing its cost c_j 00431 // relatively to the cost c_i of its parent in the tree. Based on the Metropolis criterion. 00432 double transitionProbability = 1.; // if cost_slope is <= 0, probabilty is 1 00433 00434 // Only return at end 00435 bool result = false; 00436 00437 // Calculate tranision probabilty 00438 if (costSlope > 0) 00439 { 00440 transitionProbability = exp(-costSlope / (kConstant_ * temp_)); 00441 } 00442 00443 // Check if we can accept it 00444 if (rng_.uniform01() <= transitionProbability) 00445 { 00446 if (temp_ > minTemperature_) 00447 { 00448 temp_ /= tempChangeFactor_; 00449 00450 // Prevent temp_ from getting too small 00451 if (temp_ < minTemperature_) 00452 { 00453 temp_ = minTemperature_; 00454 } 00455 } 00456 00457 numStatesFailed_ = 0; 00458 00459 result = true; 00460 } 00461 else 00462 { 00463 // State has failed 00464 if (numStatesFailed_ >= maxStatesFailed_) 00465 { 00466 temp_ *= tempChangeFactor_; 00467 numStatesFailed_ = 0; 00468 } 00469 else 00470 { 00471 ++numStatesFailed_; 00472 } 00473 00474 } 00475 00476 return result; 00477 } 00478 00479 bool ompl::geometric::TRRT::minExpansionControl(double randMotionDistance) 00480 { 00481 // Decide to accept or not 00482 if (randMotionDistance > frontierThreshold_) 00483 { 00484 // participates in the tree expansion 00485 ++frontierCount_; 00486 00487 return true; 00488 } 00489 else 00490 { 00491 // participates in the tree refinement 00492 00493 // check our ratio first before accepting it 00494 if ((double)nonfrontierCount_ / (double)frontierCount_ > frontierNodeRatio_) 00495 { 00496 // Increment so that the temperature rises faster 00497 ++numStatesFailed_; 00498 00499 // reject this node as being too much refinement 00500 return false; 00501 } 00502 else 00503 { 00504 ++nonfrontierCount_; 00505 return true; 00506 } 00507 } 00508 }