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 terminate_ = true;
00096
00097 }
00098
00099 virtual ~BallTreeRRTstar(void)
00100 {
00101 freeMemory();
00102 }
00103
00104 virtual void getPlannerData(base::PlannerData &data) const;
00105
00106 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00107
00108 virtual void clear(void);
00109
00119 void setGoalBias(double goalBias)
00120 {
00121 goalBias_ = goalBias;
00122 }
00123
00125 double getGoalBias(void) const
00126 {
00127 return goalBias_;
00128 }
00129
00135 void setRange(double distance)
00136 {
00137 maxDistance_ = distance;
00138 }
00139
00141 double getRange(void) const
00142 {
00143 return maxDistance_;
00144 }
00145
00155 void setBallRadiusConstant(double ballRadiusConstant)
00156 {
00157 ballRadiusConst_ = ballRadiusConstant;
00158 }
00159
00163 double getBallRadiusConstant(void) const
00164 {
00165 return ballRadiusConst_;
00166 }
00167
00176 void setMaxBallRadius(double maxBallRadius)
00177 {
00178 ballRadiusMax_ = maxBallRadius;
00179 }
00180
00183 double getMaxBallRadius(void) const
00184 {
00185 return ballRadiusMax_;
00186 }
00187
00192 void setInitialVolumeRadius(double rO)
00193 {
00194 rO_ = rO;
00195 }
00196
00198 double getInitialVolumeRadius(void) const
00199 {
00200 return rO_;
00201 }
00202
00204 bool inVolume(base::State *state)
00205 {
00206 for (unsigned int i = 0 ; i < motions_.size() ; ++i)
00207 {
00208 if ((si_->distance(motions_[i]->state, state) <= motions_[i]->volRadius))
00209 return true;
00210 }
00211 return false;
00212 }
00213
00214
00216 template<template<typename T> class NN>
00217 void setNearestNeighbors(void)
00218 {
00219 nn_.reset(new NN<Motion*>());
00220 }
00221
00229 void setDelayCC(bool delayCC)
00230 {
00231 delayCC_ = delayCC;
00232 }
00233
00235 bool getDelayCC(void) const
00236 {
00237 return delayCC_;
00238 }
00239
00244 void setTerminate(bool terminate)
00245 {
00246 terminate_ = terminate;
00247 }
00248
00250 bool getTerminate(void) const
00251 {
00252 return terminate_;
00253 }
00254
00255 virtual void setup(void);
00256
00257 protected:
00258
00260 class Motion
00261 {
00262 public:
00263
00264 Motion(double rO) : state(NULL), parent(NULL), cost(0.0), volRadius(rO)
00265 {
00266 }
00267
00269 Motion(const base::SpaceInformationPtr &si, double rO) : state(si->allocState()), parent(NULL), cost(0.0), volRadius(rO)
00270
00271 {
00272 }
00273
00274 ~Motion(void)
00275 {
00276 }
00277
00279 base::State *state;
00280
00282 Motion *parent;
00283
00285 double cost;
00286
00288 double volRadius;
00289 };
00290
00292 void freeMemory(void);
00293
00295 void addMotion(Motion* m)
00296 {
00297 nn_->add(m);
00298 motions_.push_back(m);
00299 }
00300
00302 static bool compareMotion(const Motion* a, const Motion* b)
00303 {
00304 return (a->cost < b->cost);
00305 }
00307 double distanceFunction(const Motion* a, const Motion* b) const
00308 {
00309 return (si_->distance(a->state, b->state)) - a->volRadius;
00310 }
00311
00313 base::StateSamplerPtr sampler_;
00314
00316 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00317
00319 std::vector<Motion*> motions_;
00320
00322 double goalBias_;
00323
00325 double maxDistance_;
00326
00328 RNG rng_;
00329
00331 double ballRadiusConst_;
00332
00334 double ballRadiusMax_;
00335
00337 bool delayCC_;
00338
00340 double rO_;
00341
00343 bool terminate_;
00344
00345 };
00346
00347 }
00348 }
00349
00350 #endif