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