ompl/geometric/planners/rrt/TRRT.h
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 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
00038 #define OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
00039 
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/datastructures/NearestNeighbors.h"
00042 #include "ompl/base/OptimizationObjective.h"
00043 
00044 /*
00045   NOTES:
00046   **Variable Names that have been converted to longer versions from standards:
00047   nearest_neighbors_ -> nn_
00048   planner_termination_condition -> ptc
00049 
00050   **Inherited Member Variables Key:
00051   si_ -> SpaceInformation
00052   pdef_ -> ProblemDefinition
00053   pis_ -> PlannerInputStates - Utility class to extract valid input states
00054 */
00055 
00056 
00057 namespace ompl
00058 {
00059 
00060     namespace geometric
00061     {
00077         class TRRT : public base::Planner
00078         {
00079         public:
00080 
00082             TRRT(const base::SpaceInformationPtr &si);
00083 
00084             virtual ~TRRT();
00085 
00086             virtual void getPlannerData(base::PlannerData &data) const;
00087 
00088             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition);
00089 
00090             virtual void clear();
00091 
00101             void setGoalBias(double goalBias)
00102             {
00103                 goalBias_ = goalBias;
00104             }
00105 
00107             double getGoalBias() const
00108             {
00109                 return goalBias_;
00110             }
00111 
00117             void setRange(double distance)
00118             {
00119                 maxDistance_ = distance;
00120             }
00121 
00123             double getRange() const
00124             {
00125                 return maxDistance_;
00126             }
00127 
00129             void setMaxStatesFailed( double maxStatesFailed )
00130             {
00131                 maxStatesFailed_ = maxStatesFailed;
00132             }
00133 
00135             double getMaxStatesFailed( void ) const
00136             {
00137                 return maxStatesFailed_;
00138             }
00139 
00141             void setTempChangeFactor( double tempChangeFactor )
00142             {
00143                 tempChangeFactor_ = tempChangeFactor;
00144             }
00145 
00147             double getTempChangeFactor( void ) const
00148             {
00149                 return tempChangeFactor_;
00150             }
00151 
00153             void setMinTemperature( double minTemperature )
00154             {
00155                 minTemperature_ = minTemperature;
00156             }
00157 
00159             double getMinTemperature( void ) const
00160             {
00161                 return minTemperature_;
00162             }
00163 
00165             void setInitTemperature( double initTemperature )
00166             {
00167                 initTemperature_ = initTemperature;
00168             }
00169 
00171             double getInitTemperature( void ) const
00172             {
00173                 return initTemperature_;
00174             }
00175 
00178             void setFrontierThreshold( double frontier_threshold )
00179             {
00180                 frontierThreshold_ = frontier_threshold;
00181             }
00182 
00185             double getFrontierThreshold( void ) const
00186             {
00187                 return frontierThreshold_;
00188             }
00189 
00192             void setFrontierNodeRatio( double frontierNodeRatio )
00193             {
00194                 frontierNodeRatio_ = frontierNodeRatio;
00195             }
00196 
00199             double getFrontierNodeRatio( void ) const
00200             {
00201                 return frontierNodeRatio_;
00202             }
00203 
00205             void setKConstant( double kConstant )
00206             {
00207                 kConstant_ = kConstant;
00208             }
00209 
00211             double getKConstant( void ) const
00212             {
00213                 return kConstant_;
00214             }
00215 
00217             template<template<typename T> class NN>
00218             void setNearestNeighbors()
00219             {
00220                 nearestNeighbors_.reset(new NN<Motion*>());
00221             }
00222 
00223             virtual void setup();
00224 
00225         protected:
00226 
00227 
00232             class Motion
00233             {
00234             public:
00235 
00236                 Motion() : state(NULL), parent(NULL)
00237                 {
00238                 }
00239 
00241                 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
00242                 {
00243                 }
00244 
00245                 ~Motion()
00246                 {
00247                 }
00248 
00250                 base::State       *state;
00251 
00253                 Motion            *parent;
00254 
00256                 base::Cost            cost;
00257 
00258             };
00259 
00261             void freeMemory();
00262 
00264             double distanceFunction(const Motion *a, const Motion *b) const
00265             {
00266                 return si_->distance(a->state, b->state);
00267             }
00268 
00274             bool transitionTest( double childCost, double parentCost, double distance );
00275 
00277             bool minExpansionControl( double randMotionDistance );
00278 
00280             base::StateSamplerPtr                          sampler_;
00281 
00283             boost::shared_ptr< NearestNeighbors<Motion*> > nearestNeighbors_;
00284 
00286             double                                         goalBias_;
00287 
00289             double                                         maxDistance_;
00290 
00292             RNG                                            rng_;
00293 
00295             Motion                                         *lastGoalMotion_;
00296 
00298             bool                                            verbose_;
00299 
00300             // *********************************************************************************************************
00301             // TRRT-Specific Variables
00302             // *********************************************************************************************************
00303 
00304             // Transtion Test -----------------------------------------------------------------------
00305 
00309             double                                          temp_;
00310 
00314             double                                          kConstant_;
00315 
00317             unsigned int                                    maxStatesFailed_;
00318 
00320             double                                          tempChangeFactor_;
00321 
00323             double                                          minTemperature_;
00324 
00326             double                                          initTemperature_;
00327 
00329             unsigned int                                    numStatesFailed_;
00330 
00331 
00332             // Minimum Expansion Control --------------------------------------------------------------
00333 
00335             double                                          nonfrontierCount_;
00336             double                                          frontierCount_;
00337 
00339             double                                          frontierThreshold_;
00340 
00342             double                                          frontierNodeRatio_;
00343 
00345             ompl::base::OptimizationObjectivePtr            opt_;
00346         };
00347     }
00348 }
00349 
00350 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines