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