00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00080 class BallTreeRRTstar : public base::Planner
00081 {
00082 public:
00083
00084 BallTreeRRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "BallTreeRRTstar")
00085 {
00086 specs_.approximateSolutions = true;
00087 specs_.optimizingPaths = true;
00088
00089 goalBias_ = 0.05;
00090 maxDistance_ = 0.0;
00091 ballRadiusMax_ = 0.0;
00092 ballRadiusConst_ = 1.0;
00093 rO_ = std::numeric_limits<double>::infinity();
00094 delayCC_ = true;
00095 }
00096
00097 virtual ~BallTreeRRTstar(void)
00098 {
00099 freeMemory();
00100 }
00101
00102 virtual void getPlannerData(base::PlannerData &data) const;
00103
00104 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00105
00106 virtual void clear(void);
00107
00117 void setGoalBias(double goalBias)
00118 {
00119 goalBias_ = goalBias;
00120 }
00121
00123 double getGoalBias(void) const
00124 {
00125 return goalBias_;
00126 }
00127
00133 void setRange(double distance)
00134 {
00135 maxDistance_ = distance;
00136 }
00137
00139 double getRange(void) const
00140 {
00141 return maxDistance_;
00142 }
00143
00153 void setBallRadiusConstant(double ballRadiusConstant)
00154 {
00155 ballRadiusConst_ = ballRadiusConstant;
00156 }
00157
00161 double getBallRadiusConstant(void) const
00162 {
00163 return ballRadiusConst_;
00164 }
00165
00174 void setMaxBallRadius(double maxBallRadius)
00175 {
00176 ballRadiusMax_ = maxBallRadius;
00177 }
00178
00181 double getMaxBallRadius(void) const
00182 {
00183 return ballRadiusMax_;
00184 }
00185
00190 void setInitialVolumeRadius(double rO)
00191 {
00192 rO_ = rO;
00193 }
00194
00196 double getInitialVolumeRadius(void) const
00197 {
00198 return rO_;
00199 }
00200
00202 bool inVolume(base::State *state)
00203 {
00204 for (unsigned int i = 0 ; i < motions_.size() ; ++i)
00205 {
00206 if ((si_->distance(motions_[i]->state, state) <= motions_[i]->volRadius))
00207 return true;
00208 }
00209 return false;
00210 }
00211
00212
00214 template<template<typename T> class NN>
00215 void setNearestNeighbors(void)
00216 {
00217 nn_.reset(new NN<Motion*>());
00218 }
00219
00227 void setDelayCC(bool delayCC)
00228 {
00229 delayCC_ = delayCC;
00230 }
00231
00233 bool getDelayCC(void) const
00234 {
00235 return delayCC_;
00236 }
00237
00238 virtual void setup(void);
00239
00240 protected:
00241
00243 class Motion
00244 {
00245 public:
00246
00247 Motion(double rO) : state(NULL), parent(NULL), cost(0.0), volRadius(rO)
00248 {
00249 }
00250
00252 Motion(const base::SpaceInformationPtr &si, double rO) : state(si->allocState()), parent(NULL), cost(0.0), volRadius(rO)
00253
00254 {
00255 }
00256
00257 ~Motion(void)
00258 {
00259 }
00260
00262 base::State *state;
00263
00265 Motion *parent;
00266
00268 double cost;
00269
00271 double volRadius;
00272 };
00273
00275 void freeMemory(void);
00276
00278 void addMotion(Motion* m)
00279 {
00280 nn_->add(m);
00281 motions_.push_back(m);
00282 }
00283
00285 static bool compareMotion(const Motion* a, const Motion* b)
00286 {
00287 return (a->cost < b->cost);
00288 }
00290 double distanceFunction(const Motion* a, const Motion* b) const
00291 {
00292 return (si_->distance(a->state, b->state)) - a->volRadius;
00293 }
00294
00296 base::StateSamplerPtr sampler_;
00297
00299 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00300
00302 std::vector<Motion*> motions_;
00303
00305 double goalBias_;
00306
00308 double maxDistance_;
00309
00311 RNG rng_;
00312
00314 double ballRadiusConst_;
00315
00317 double ballRadiusMax_;
00318
00320 bool delayCC_;
00321
00323 double rO_;
00324 };
00325
00326 }
00327 }
00328
00329 #endif