All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/contrib/rrt_star/BallTreeRRTstar.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
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 Rice University 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 /* Authors: Alejandro Perez, Sertac Karaman, Ioan Sucan */
00036 
00037 #ifndef OMPL_CONTRIB_RRT_STAR_BTRRTSTAR_
00038 #define OMPL_CONTRIB_RRT_STAR_BTRRTSTAR_
00039 
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/datastructures/NearestNeighbors.h"
00042 #include "ompl/base/spaces/RealVectorStateSpace.h"
00043 #include <limits>
00044 #include <vector>
00045 
00046 
00047 namespace ompl
00048 {
00049 
00050     namespace geometric
00051     {
00052 
00078         class BallTreeRRTstar : public base::Planner
00079         {
00080         public:
00081 
00082             BallTreeRRTstar(const base::SpaceInformationPtr &si);
00083 
00084             virtual ~BallTreeRRTstar(void);
00085 
00086             virtual void getPlannerData(base::PlannerData &data) const;
00087 
00088             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00089 
00090             virtual void clear(void);
00091 
00101             void setGoalBias(double goalBias)
00102             {
00103                 goalBias_ = goalBias;
00104             }
00105 
00107             double getGoalBias(void) const
00108             {
00109                 return goalBias_;
00110             }
00111 
00117             void setRange(double distance)
00118             {
00119                 maxDistance_ = distance;
00120             }
00121 
00123             double getRange(void) const
00124             {
00125                 return maxDistance_;
00126             }
00127 
00137             void setBallRadiusConstant(double ballRadiusConstant)
00138             {
00139                 ballRadiusConst_ = ballRadiusConstant;
00140             }
00141 
00145             double getBallRadiusConstant(void) const
00146             {
00147                 return ballRadiusConst_;
00148             }
00149 
00158             void setMaxBallRadius(double maxBallRadius)
00159             {
00160                 ballRadiusMax_ = maxBallRadius;
00161             }
00162 
00165             double getMaxBallRadius(void) const
00166             {
00167                 return ballRadiusMax_;
00168             }
00169 
00174             void setInitialVolumeRadius(double rO)
00175             {
00176                 rO_ = rO;
00177             }
00178 
00180             double getInitialVolumeRadius(void) const
00181             {
00182                 return rO_;
00183             }
00184 
00186             bool inVolume(base::State *state)
00187             {
00188                 for (unsigned int i = 0 ; i < motions_.size() ; ++i)
00189                 {
00190                     if ((si_->distance(motions_[i]->state, state) <= motions_[i]->volRadius))
00191                         return true;
00192                 }
00193                 return false;
00194             }
00195 
00196 
00198             template<template<typename T> class NN>
00199             void setNearestNeighbors(void)
00200             {
00201                 nn_.reset(new NN<Motion*>());
00202             }
00203 
00211             void setDelayCC(bool delayCC)
00212             {
00213                 delayCC_ = delayCC;
00214             }
00215 
00217             bool getDelayCC(void) const
00218             {
00219                 return delayCC_;
00220             }
00221 
00222             virtual void setup(void);
00223 
00224         protected:
00225 
00227             class Motion
00228             {
00229             public:
00230 
00231                 Motion(double rO) : state(NULL), parent(NULL), cost(0.0), volRadius(rO)
00232                 {
00233                 }
00234 
00236                 Motion(const base::SpaceInformationPtr &si, double rO) : state(si->allocState()), parent(NULL), cost(0.0), volRadius(rO)
00237 
00238                 {
00239                 }
00240 
00241                 ~Motion(void)
00242                 {
00243                 }
00244 
00246                 base::State       *state;
00247 
00249                 Motion            *parent;
00250 
00252                 double             cost;
00253 
00255                 double             volRadius;
00256 
00258                 std::vector<Motion*> children;
00259             };
00260 
00262             void freeMemory(void);
00263 
00265             void addMotion(Motion* m)
00266             {
00267                 nn_->add(m);
00268                 motions_.push_back(m);
00269             }
00270 
00272             static bool compareMotion(const Motion* a, const Motion* b)
00273             {
00274                 return (a->cost < b->cost);
00275             }
00277             double distanceFunction(const Motion* a, const Motion* b) const
00278             {
00279                 return (si_->distance(a->state, b->state)) - a->volRadius;
00280             }
00281 
00283             void removeFromParent(Motion *m);
00284 
00286             void updateChildCosts(Motion *m, double delta);
00287 
00289             base::StateSamplerPtr                          sampler_;
00290 
00292             boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00293 
00295             std::vector<Motion*>                           motions_;
00296 
00298             double                                         goalBias_;
00299 
00301             double                                         maxDistance_;
00302 
00304             RNG                                            rng_;
00305 
00307             double                                         ballRadiusConst_;
00308 
00310             double                                         ballRadiusMax_;
00311 
00313             bool                                           delayCC_;
00314 
00316             double                                         rO_;
00317         };
00318 
00319     }
00320 }
00321 
00322 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines