ompl/geometric/planners/stride/STRIDE.h
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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 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 /* Author: Bryant Gipson, Mark Moll */
00036 
00037 #ifndef OMPL_GEOMETRIC_PLANNERS_STRIDE_STRIDE_
00038 #define OMPL_GEOMETRIC_PLANNERS_STRIDE_STRIDE_
00039 
00040 #include "ompl/datastructures/Grid.h"
00041 #include "ompl/geometric/planners/PlannerIncludes.h"
00042 #include "ompl/base/ProjectionEvaluator.h"
00043 #include "ompl/datastructures/PDF.h"
00044 #include <boost/unordered_map.hpp>
00045 #include <boost/scoped_ptr.hpp>
00046 #include <vector>
00047 
00048 namespace ompl
00049 {
00050 
00051     template<typename _T>
00052     class NearestNeighborsGNAT;
00053 
00054     namespace geometric
00055     {
00056 
00080         class STRIDE : public base::Planner
00081         {
00082             public:
00083 
00085                 STRIDE(const base::SpaceInformationPtr &si, bool useProjectedDistance = false,
00086                         unsigned int degree = 16, unsigned int minDegree = 12, unsigned int maxDegree = 18,
00087                         unsigned int maxNumPtsPerLeaf = 6, double estimatedDimension = 0.0);
00088                 virtual ~STRIDE();
00089 
00090                 virtual void setup();
00091 
00092                 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00093 
00094                 virtual void clear();
00095 
00103                 void setGoalBias(double goalBias)
00104                 {
00105                     goalBias_ = goalBias;
00106                 }
00107 
00109                 double getGoalBias() const
00110                 {
00111                     return goalBias_;
00112                 }
00113 
00117                 void setUseProjectedDistance(bool useProjectedDistance)
00118                 {
00119                     useProjectedDistance_ = useProjectedDistance;
00120                 }
00124                 bool getUseProjectedDistance() const
00125                 {
00126                     return useProjectedDistance_;
00127                 }
00128 
00130                 void setDegree(unsigned int degree)
00131                 {
00132                     degree_ = degree;
00133                 }
00135                 unsigned int getDegree() const
00136                 {
00137                     return degree_;
00138                 }
00140                 void setMinDegree(unsigned int minDegree)
00141                 {
00142                     minDegree_ = minDegree;
00143                 }
00145                 unsigned int getMinDegree() const
00146                 {
00147                     return minDegree_;
00148                 }
00150                 void setMaxDegree(unsigned int maxDegree)
00151                 {
00152                     maxDegree_ = maxDegree;
00153                 }
00155                 unsigned int getMaxDegree() const
00156                 {
00157                     return maxDegree_;
00158                 }
00161                 void setMaxNumPtsPerLeaf(unsigned int maxNumPtsPerLeaf)
00162                 {
00163                     maxNumPtsPerLeaf_ = maxNumPtsPerLeaf;
00164                 }
00167                 unsigned int getMaxNumPtsPerLeaf() const
00168                 {
00169                     return maxNumPtsPerLeaf_;
00170                 }
00174                 void setEstimatedDimension(double estimatedDimension)
00175                 {
00176                     estimatedDimension_ = estimatedDimension;
00177                 }
00181                 double getEstimatedDimension() const
00182                 {
00183                     return estimatedDimension_;
00184                 }
00185 
00191                 void setRange(double distance)
00192                 {
00193                     maxDistance_ = distance;
00194                 }
00195 
00197                 double getRange() const
00198                 {
00199                     return maxDistance_;
00200                 }
00207                 void setMinValidPathFraction(double fraction)
00208                 {
00209                     minValidPathFraction_ = fraction;
00210                 }
00211 
00213                 double getMinValidPathFraction() const
00214                 {
00215                     return minValidPathFraction_;
00216                 }
00219                 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00220                 {
00221                     projectionEvaluator_ = projectionEvaluator;
00222                 }
00223 
00226                 void setProjectionEvaluator(const std::string &name)
00227                 {
00228                     projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00229                 }
00230 
00232                 const base::ProjectionEvaluatorPtr& getProjectionEvaluator() const
00233                 {
00234                     return projectionEvaluator_;
00235                 }
00236 
00237                 virtual void getPlannerData(base::PlannerData &data) const;
00238 
00239             protected:
00240 
00242                 class Motion
00243                 {
00244                 public:
00245                     Motion() : state(NULL), parent(NULL)
00246                     {
00247                     }
00248 
00250                     Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
00251                     {
00252                     }
00253 
00254                     ~Motion()
00255                     {
00256                     }
00257 
00259                     base::State       *state;
00260 
00262                     Motion            *parent;
00263                 };
00264 
00265 
00267                 void freeMemory();
00268 
00270                 void setupTree();
00271 
00273                 double distanceFunction(const Motion *a, const Motion *b) const
00274                 {
00275                     return si_->distance(a->state, b->state);
00276                 }
00277 
00279                 double projectedDistanceFunction(const Motion *a, const Motion *b) const
00280                 {
00281                     unsigned int num_dims = projectionEvaluator_->getDimension();
00282                     ompl::base::EuclideanProjection aproj(num_dims), bproj(num_dims);
00283                     projectionEvaluator_->project(a->state, aproj);
00284                     projectionEvaluator_->project(b->state, bproj);
00285                     return boost::numeric::ublas::norm_2(aproj - bproj);
00286                 }
00287 
00289                 void addMotion(Motion *motion);
00290 
00292                 Motion* selectMotion();
00293 
00295                 base::ValidStateSamplerPtr   sampler_;
00296 
00298                 base::ProjectionEvaluatorPtr projectionEvaluator_;
00299 
00301                 boost::scoped_ptr<NearestNeighborsGNAT<Motion*> >
00302                                              tree_;
00303 
00305                 double                       goalBias_;
00306 
00308                 double                       maxDistance_;
00309 
00311                 bool                         useProjectedDistance_;
00313                 unsigned int                 degree_;
00315                 unsigned int                 minDegree_;
00317                 unsigned int                 maxDegree_;
00319                 unsigned int                 maxNumPtsPerLeaf_;
00321                 double                       estimatedDimension_;
00327                 double                       minValidPathFraction_;
00328 
00330                 RNG                          rng_;
00331         };
00332 
00333     }
00334 }
00335 
00336 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines