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
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