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);
00085
00086 virtual ~BallTreeRRTstar(void);
00087
00088 virtual void getPlannerData(base::PlannerData &data) const;
00089
00090 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00091
00092 virtual void clear(void);
00093
00103 void setGoalBias(double goalBias)
00104 {
00105 goalBias_ = goalBias;
00106 }
00107
00109 double getGoalBias(void) const
00110 {
00111 return goalBias_;
00112 }
00113
00119 void setRange(double distance)
00120 {
00121 maxDistance_ = distance;
00122 }
00123
00125 double getRange(void) const
00126 {
00127 return maxDistance_;
00128 }
00129
00139 void setBallRadiusConstant(double ballRadiusConstant)
00140 {
00141 ballRadiusConst_ = ballRadiusConstant;
00142 }
00143
00147 double getBallRadiusConstant(void) const
00148 {
00149 return ballRadiusConst_;
00150 }
00151
00160 void setMaxBallRadius(double maxBallRadius)
00161 {
00162 ballRadiusMax_ = maxBallRadius;
00163 }
00164
00167 double getMaxBallRadius(void) const
00168 {
00169 return ballRadiusMax_;
00170 }
00171
00176 void setInitialVolumeRadius(double rO)
00177 {
00178 rO_ = rO;
00179 }
00180
00182 double getInitialVolumeRadius(void) const
00183 {
00184 return rO_;
00185 }
00186
00188 bool inVolume(base::State *state)
00189 {
00190 for (unsigned int i = 0 ; i < motions_.size() ; ++i)
00191 {
00192 if ((si_->distance(motions_[i]->state, state) <= motions_[i]->volRadius))
00193 return true;
00194 }
00195 return false;
00196 }
00197
00198
00200 template<template<typename T> class NN>
00201 void setNearestNeighbors(void)
00202 {
00203 nn_.reset(new NN<Motion*>());
00204 }
00205
00213 void setDelayCC(bool delayCC)
00214 {
00215 delayCC_ = delayCC;
00216 }
00217
00219 bool getDelayCC(void) const
00220 {
00221 return delayCC_;
00222 }
00223
00224 virtual void setup(void);
00225
00226 protected:
00227
00229 class Motion
00230 {
00231 public:
00232
00233 Motion(double rO) : state(NULL), parent(NULL), cost(0.0), volRadius(rO)
00234 {
00235 }
00236
00238 Motion(const base::SpaceInformationPtr &si, double rO) : state(si->allocState()), parent(NULL), cost(0.0), volRadius(rO)
00239
00240 {
00241 }
00242
00243 ~Motion(void)
00244 {
00245 }
00246
00248 base::State *state;
00249
00251 Motion *parent;
00252
00254 double cost;
00255
00257 double volRadius;
00258
00260 std::vector<Motion*> children;
00261 };
00262
00264 void freeMemory(void);
00265
00267 void addMotion(Motion* m)
00268 {
00269 nn_->add(m);
00270 motions_.push_back(m);
00271 }
00272
00274 static bool compareMotion(const Motion* a, const Motion* b)
00275 {
00276 return (a->cost < b->cost);
00277 }
00279 double distanceFunction(const Motion* a, const Motion* b) const
00280 {
00281 return (si_->distance(a->state, b->state)) - a->volRadius;
00282 }
00283
00285 void removeFromParent(Motion *m);
00286
00288 void updateChildCosts(Motion *m, double delta);
00289
00291 base::StateSamplerPtr sampler_;
00292
00294 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00295
00297 std::vector<Motion*> motions_;
00298
00300 double goalBias_;
00301
00303 double maxDistance_;
00304
00306 RNG rng_;
00307
00309 double ballRadiusConst_;
00310
00312 double ballRadiusMax_;
00313
00315 bool delayCC_;
00316
00318 double rO_;
00319 };
00320
00321 }
00322 }
00323
00324 #endif