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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines