incl/rrtext.hh

   1 #ifndef RRTS_RRTEXT_H
   2 #define RRTS_RRTEXT_H
   3
   4 #include "rrts.hh"
   5
   6 // ext2
   7 #include "cute_c2.h"
   8
   9 // ext4
  10 #define GRID 1 // in meters
  11 #define GRID_WIDTH 40 // in meters
  12 #define GRID_HEIGHT 40 // in meters
  13 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
  14 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
  15
  16 // ext9
  17 #define GRID_MAX_HI 60
  18
  19 namespace rrts {
  20
  21 /*! \brief Random sampling in the circuit between root and goal.
  22  *
  23  * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
  24  */
  25 class RRTExt14 : public virtual RRTS {
  26 private:
  27         Point circle_c_;
  28         double circle_r_ = 0.0;
  29         std::uniform_real_distribution<double> udr_;
  30         std::uniform_real_distribution<double> udt_;
  31         std::uniform_real_distribution<double> udh_;
  32         RRTNode sample();
  33 public:
  34         RRTExt14();
  35         void reset();
  36 };
  37
  38 /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
  39 class RRTExt13 : public virtual RRTS {
  40         private:
  41         public:
  42                 void reset();
  43                 std::vector<RRTNode *> orig_path_;
  44                 double orig_path_cost_ = 9999;
  45                 std::vector<RRTNode *> first_optimized_path_;
  46                 double first_optimized_path_cost_ = 9999;
  47                 void first_path_optimization();
  48                 void second_path_optimization();
  49                 void compute_path();
  50                 Json::Value json();
  51                 void json(Json::Value jvi);
  52
  53                 // getter, setter
  54                 std::vector<RRTNode *> &orig_path()
  55                 {
  56                         return this->orig_path_;
  57                 };
  58                 double &orig_path_cost() { return this->orig_path_cost_; }
  59                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
  60                 std::vector<RRTNode *> &first_optimized_path()
  61                 {
  62                         return this->first_optimized_path_;
  63                 };
  64                 double &first_optimized_path_cost() {
  65                         return this->first_optimized_path_cost_;
  66                 }
  67                 void first_optimized_path_cost(double c) {
  68                         this->first_optimized_path_cost_ = c;
  69                 }
  70 };
  71
  72 /*! \brief Different `steer` procedures.
  73
  74 Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
  75 */
  76 class RRTExt12 : public virtual RRTS {
  77         protected:
  78                 void steer1(RRTNode &f, RRTNode &t);
  79                 bool connect();
  80         public:
  81                 bool next();
  82 };
  83
  84 /*! \brief Goal Zone.
  85
  86 */
  87 class RRTExt11 : public virtual RRTS {
  88         protected:
  89                 bool goal_found(RRTNode &f);
  90 };
  91
  92 /*! \brief Different costs extension.
  93
  94 Use different cost for bulding tree data structure and searching in the
  95 structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and Reza
  96 Jazar. “Randomized Bidirectional B-Spline Parameterization Motion Planning.”
  97 IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February
  98 2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
  99
 100 */
 101 class RRTExt10 : public virtual RRTS {
 102         public:
 103                 /*! \brief Reeds and Shepp path length.
 104                 */
 105                 double cost_build(RRTNode &f, RRTNode &t);
 106                 /*! \brief Heuristics distance.
 107                 */
 108                 double cost_search(RRTNode &f, RRTNode &t);
 109 };
 110
 111 /*! \brief Use grid data structure to store RRT nodes.
 112
 113 This approach speeds up the search process for the nearest neighbor and
 114 the near vertices procedures.
 115 */
 116 class RRTExt9 : public virtual RRTS {
 117         private:
 118                 class Cell {
 119                         private:
 120                                 bool changed_ = false;
 121                                 std::vector<RRTNode *> nodes_;
 122                         public:
 123                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 124                                 void store_node(RRTNode *n);
 125
 126                                 // getter, setter
 127                                 bool changed() const
 128                                 {
 129                                         return this->changed_;
 130                                 }
 131                                 std::vector<RRTNode *> &nodes()
 132                                 {
 133                                         return this->nodes_;
 134                                 }
 135
 136                                 Cell();
 137                 };
 138                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
 139                 double x_min_ = 0;
 140                 double x_max_ = 0;
 141                 double y_min_ = 0;
 142                 double y_max_ = 0;
 143                 double h_min_ = 0;
 144                 double h_max_ = 2 * M_PI;
 145
 146                 unsigned int xi(RRTNode n);
 147                 unsigned int yi(RRTNode n);
 148                 unsigned int hi(RRTNode n);
 149         public:
 150                 void init();
 151                 void deinit();
 152                 void store_node(RRTNode n);
 153                 RRTNode *nn(RRTNode &t);
 154                 std::vector<RRTNode *> nv(RRTNode &t);
 155 };
 156
 157 /*! \brief Use k-d tree data structure to store RRT nodes.
 158
 159 This approach speeds up the search process for the nearest neighbor and
 160 the near vertices procedures. This extension implements 3D K-d tree.
 161
 162 \see https://en.wikipedia.org/wiki/K-d_tree
 163 */
 164 class RRTExt8 : public virtual RRTS {
 165         private:
 166                 class KdNode {
 167                         private:
 168                                 RRTNode *node_ = nullptr;
 169                                 KdNode *left_ = nullptr;
 170                                 KdNode *right_ = nullptr;
 171                         public:
 172                                 // getter, setter
 173                                 RRTNode *node() const { return this->node_; }
 174                                 KdNode *&left() { return this->left_; }
 175                                 KdNode *&right() { return this->right_; }
 176                                 KdNode(RRTNode *n);
 177                 };
 178                 KdNode *kdroot_ = nullptr;
 179                 void delete_kd_nodes(KdNode *n);
 180                 void store_node(RRTNode *n, KdNode *&r, int l);
 181                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 182                 void nv(
 183                         std::vector<RRTNode*>& n,
 184                         RRTNode &t,
 185                         KdNode *r,
 186                         int l,
 187                         double const& d
 188                 );
 189         public:
 190                 void delete_kd_nodes()
 191                 {
 192                     this->delete_kd_nodes(this->kdroot_);
 193                     this->kdroot_ = nullptr;
 194                 }
 195                 void init();
 196                 void reset();
 197                 void deinit();
 198                 void store_node(RRTNode n);
 199                 RRTNode *nn(RRTNode &t);
 200                 std::vector<RRTNode *> nv(RRTNode &t);
 201 };
 202
 203 /*! \brief Use k-d tree data structure to store RRT nodes.
 204
 205 This approach speeds up the search process for the nearest neighbor and
 206 the near vertices procedures. This extension implements 2D K-d tree.
 207
 208 \see https://en.wikipedia.org/wiki/K-d_tree
 209 */
 210 class RRTExt7 : public virtual RRTS {
 211         private:
 212                 class KdNode {
 213                         private:
 214                                 RRTNode *node_ = nullptr;
 215                                 KdNode *left_ = nullptr;
 216                                 KdNode *right_ = nullptr;
 217                         public:
 218                                 // getter, setter
 219                                 RRTNode *node() const { return this->node_; }
 220                                 KdNode *&left() { return this->left_; }
 221                                 KdNode *&right() { return this->right_; }
 222                                 KdNode(RRTNode *n);
 223                 };
 224                 KdNode *kdroot_ = nullptr;
 225                 void delete_kd_nodes(KdNode *n);
 226                 void store_node(RRTNode *n, KdNode *&r, int l);
 227                 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
 228         public:
 229                 void init();
 230                 void deinit();
 231                 void store_node(RRTNode n);
 232                 RRTNode *nn(RRTNode &t);
 233                 std::vector<RRTNode *> nv(RRTNode &t);
 234 };
 235
 236 /*! \brief Reeds and Shepp cost for building and search.
 237 */
 238 class RRTExt6 : public virtual RRTS {
 239         public:
 240                 /*! \brief Reeds and Shepp path length.
 241                 */
 242                 double cost_build(RRTNode &f, RRTNode &t);
 243                 /*! \brief Reeds and Shepp path length.
 244                 */
 245                 double cost_search(RRTNode &f, RRTNode &t);
 246 };
 247
 248 /*! \brief Different costs extension.
 249
 250 Use different cost for bulding tree data structure and searching in the
 251 structure. This one is complementary to `rrtext1.cc`.
 252 */
 253 class RRTExt5 : public virtual RRTS {
 254         public:
 255                 /*! \brief Reeds and Shepp path length.
 256                 */
 257                 double cost_build(RRTNode &f, RRTNode &t);
 258                 /*! \brief Euclidean distance.
 259                 */
 260                 double cost_search(RRTNode &f, RRTNode &t);
 261 };
 262
 263 /*! \brief Use grid data structure to store RRT nodes.
 264
 265 This approach speeds up the search process for the nearest neighbor and
 266 the near vertices procedures.
 267 */
 268 class RRTExt4 : public virtual RRTS {
 269         private:
 270                 class Cell {
 271                         private:
 272                                 bool changed_ = false;
 273                                 std::vector<RRTNode *> nodes_;
 274                         public:
 275                                 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
 276                                 void store_node(RRTNode *n);
 277
 278                                 // getter, setter
 279                                 bool changed() const
 280                                 {
 281                                         return this->changed_;
 282                                 }
 283                                 std::vector<RRTNode *> &nodes()
 284                                 {
 285                                         return this->nodes_;
 286                                 }
 287
 288                                 Cell();
 289                 };
 290                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
 291                 double x_min_ = 0;
 292                 double x_max_ = 0;
 293                 double y_min_ = 0;
 294                 double y_max_ = 0;
 295
 296                 unsigned int xi(RRTNode n);
 297                 unsigned int yi(RRTNode n);
 298         public:
 299                 void init();
 300                 void deinit();
 301                 void store_node(RRTNode n);
 302                 RRTNode *nn(RRTNode &t);
 303                 std::vector<RRTNode *> nv(RRTNode &t);
 304 };
 305
 306 /*! \brief Use Dijkstra algorithm to find the shorter path.
 307 */
 308 class RRTExt3 : public virtual RRTS {
 309         private:
 310         public:
 311                 void reset();
 312                 std::vector<RRTNode *> orig_path_;
 313                 double orig_path_cost_ = 9999;
 314                 std::vector<RRTNode *> first_optimized_path_;
 315                 double first_optimized_path_cost_ = 9999;
 316                 void first_path_optimization();
 317                 void second_path_optimization();
 318                 void compute_path();
 319                 Json::Value json();
 320                 void json(Json::Value jvi);
 321
 322                 // getter, setter
 323                 std::vector<RRTNode *> &orig_path()
 324                 {
 325                         return this->orig_path_;
 326                 };
 327                 double &orig_path_cost() { return this->orig_path_cost_; }
 328                 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
 329                 std::vector<RRTNode *> &first_optimized_path()
 330                 {
 331                         return this->first_optimized_path_;
 332                 };
 333                 double &first_optimized_path_cost() {
 334                         return this->first_optimized_path_cost_;
 335                 }
 336                 void first_optimized_path_cost(double c) {
 337                         this->first_optimized_path_cost_ = c;
 338                 }
 339 };
 340
 341 /*! \brief Use cute_c2 for collision detection.
 342
 343 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 344 */
 345 class RRTExt2 : public virtual RRTS {
 346 private:
 347         c2Poly c2_bc_;
 348         c2x c2x_bc_;
 349         std::vector<c2Poly> c2_obstacles_;
 350         bool collide(RRTNode const& n);
 351         bool collide_steered();
 352 public:
 353         RRTExt2();
 354         Json::Value json() const;
 355         void json(Json::Value jvi);
 356 };
 357
 358 /*! \brief Different costs extension.
 359
 360 Use different cost for bulding tree data structure and searching in the
 361 structure.
 362 */
 363 class RRTExt1 : public virtual RRTS {
 364         public:
 365                 /*! \brief Reeds and Shepp path length.
 366                 */
 367                 double cost_build(RRTNode &f, RRTNode &t);
 368                 /*! \brief Matej's heuristics.
 369                 */
 370                 double cost_search(RRTNode &f, RRTNode &t);
 371 };
 372
 373 } // namespace rrts
 374 #endif /* RRTS_RRTEXT_H */