Merge pull request #21 from mlsdpk/sampling-based

Add rrt and rrt*

Author: mlsdpk

CMakeLists.txt

@@ -11,6 +11,9 @@ include_directories(include/States/Algorithms/GraphBased/BFS)
 include_directories(include/States/Algorithms/GraphBased/DFS)
 include_directories(include/States/Algorithms/GraphBased/DIJKSTRA)
 include_directories(include/States/Algorithms/GraphBased/ASTAR)
+include_directories(include/States/Algorithms/SamplingBased)
+include_directories(include/States/Algorithms/SamplingBased/RRT)
+include_directories(include/States/Algorithms/SamplingBased/RRT_STAR)
 
 set(EXECUTABLE_NAME "main")
 
@@ -24,6 +27,9 @@ add_executable(${EXECUTABLE_NAME}
   src/States/Algorithms/GraphBased/DFS/DFS.cpp
   src/States/Algorithms/GraphBased/DIJKSTRA/DIJKSTRA.cpp
   src/States/Algorithms/GraphBased/ASTAR/ASTAR.cpp
+  src/States/Algorithms/SamplingBased/SamplingBased.cpp
+  src/States/Algorithms/SamplingBased/RRT/RRT.cpp
+  src/States/Algorithms/SamplingBased/RRT_STAR/RRT_STAR.cpp
 )
 
 target_link_libraries(

README.md

@@ -4,7 +4,7 @@
 
 A tool for visualizing numerous pathfinding algorithms in two dimensions.
 
-This project involves minimal implementations of the popular planning algorithms, including both graph-based and sampling-based planners. We provide an easy-to-use GUI to control the animation process and explore different planner configurations. This is an ongoing work and current implementation of the project only involves four search-based planning algorithms: BFS, DFS, DIJKSTRA and A-Star. The project extensively uses SFML, ImGui and Modern C++ features such as smart pointers, lamda expressions along with multi-threading concepts.
+This project involves minimal implementations of the popular planning algorithms, including both graph-based and sampling-based planners. We provide an easy-to-use GUI to control the animation process and explore different planner configurations. This is an ongoing work and current implementation of the project only involves four search-based planning algorithms: BFS, DFS, DIJKSTRA and A-Star and two sampling-based planners: RRT and RRT*. The project extensively uses SFML, ImGui and Modern C++ features such as smart pointers, lamda expressions along with multi-threading concepts.
 
 ![](figures/img0.png)
 
@@ -25,7 +25,7 @@ The project depends on [SFML](https://github.com/SFML/SFML), [Dear ImGui](https:
 
 ## TODO
 
-### Search-based planners
+### Graph-based planners
 - [x] BFS
 - [x] DFS
 - [x] DIJKSTRA
@@ -35,9 +35,9 @@ The project depends on [SFML](https://github.com/SFML/SFML), [Dear ImGui](https:
 - [ ] LPA*
 
 ### Sampling-based planners
-- [ ] RRT
+- [x] RRT
 - [ ] RRT-Connect
-- [ ] RRT*
+- [x] RRT*
 - [ ] Informed-RRT*
 - [ ] FMT*
 - [ ] BIT*
@@ -48,5 +48,5 @@ The project depends on [SFML](https://github.com/SFML/SFML), [Dear ImGui](https:
 - [DIJKSTRA](https://ir.cwi.nl/pub/9256/9256D.pdf): A Note on Two Problems in Connexion with Graphs
 - [A*](https://ieeexplore.ieee.org/abstract/document/4082128?casa_token=0ltx8josfO0AAAAA:nA2z0T2qvr00C6rIhIM3Z7GhWJTQpFrYsdzpY9xc_VicZ0DZr5Q9KcclJT1215N3If6pae87MXRHHd0): A Formal Basis for the Heuristic Determination of Minimum Cost Paths
 - [LPA*](https://www.cs.cmu.edu/~maxim/files/aij04.pdf): Lifelong Planning A*
-- [RRT](https://www.cs.csustan.edu/~xliang/Courses/CS4710-21S/Papers/06%20RRT.pdf): Rapidly-exploring random trees: A new tool for path planning
+- [RRT](https://journals.sagepub.com/doi/pdf/10.1177/02783640122067453?casa_token=fgVkbBjl93wAAAAA:xatnfEy0HmRWnZyzPcPMHoWpW2ch4WIFYY1SSVT-OjyVKidKavkiE7D3QMl3cHSpof4BlXQcSVzhbvo): Randomized kinodynamic planning
 - [RRT*](https://journals.sagepub.com/doi/abs/10.1177/0278364911406761): Sampling-based algorithms for optimal motion planning

config/rrt.ini

@@ -14,8 +14,8 @@
 namespace path_finding_visualizer {
 
 static const std::vector<std::string> PLANNER_NAMES{"BFS", "DFS", "DIJKSTRA",
-                                                    "A*"};
-enum PLANNERS_IDS { BFS, DFS, DIJKSTRA, AStar };
+                                                    "A*",  "RRT", "RRT*"};
+enum PLANNERS_IDS { BFS, DFS, DIJKSTRA, AStar, RRT, RRT_STAR };
 
 class Game {
  public:

config/rrt_star.ini

@@ -24,6 +24,7 @@ class BFS : public GraphBased {
 
   // override render functions
   virtual void renderNodes() override;
+  virtual void renderParametersGui() override;
 
   // BFS algorithm function
   virtual void solveConcurrently(

figures/img0.png

@@ -33,7 +33,10 @@ class GraphBased : public State {
   void render() override;
 
   // virtual functions
+  virtual void clearObstacles();
   virtual void renderGui();
+  // render planner specific parameters
+  virtual void renderParametersGui() = 0;
   virtual void renderNodes() = 0;
   virtual void updateNodes() = 0;
   virtual void initAlgorithm() = 0;

include/Game.h

@@ -3,22 +3,12 @@
 #include <random>
 #include <set>
 
-#include "SamplingBased.h"
+#include "States/Algorithms/SamplingBased/SamplingBased.h"
 
-class RRT : public SamplingBased {
- private:
-  // vertices & edges
-  std::vector<std::shared_ptr<Point>> vertices_;
-  std::vector<std::pair<std::shared_ptr<Point>, std::shared_ptr<Point>>> edges_;
-
-  int max_vertices_;
-  double delta_q_;
-  double goal_radius_;
-
-  // background text
-  sf::Text maxVerticesText_;
-  sf::Text noOfVerticesText_;
+namespace path_finding_visualizer {
+namespace sampling_based {
 
+class RRT : public SamplingBased {
  public:
   // Constructor
   RRT(sf::RenderWindow *window, std::stack<std::unique_ptr<State>> &states);
@@ -27,28 +17,86 @@ class RRT : public SamplingBased {
   virtual ~RRT();
 
   // override initialization functions
-  void initialize() override;
-  void initAlgorithm() override;
+  virtual void initialize() override;
+  virtual void initPlanner() override;
+  virtual void initParameters() override;
 
   // override render functions
-  void renderBackground() override;
-  void renderAlgorithm() override;
+  virtual void renderPlannerData() override;
+  virtual void renderParametersGui() override;
 
   // override algorithm function
-  void solveConcurrently(
-      std::shared_ptr<Point> start_point, std::shared_ptr<Point> goal_point,
+  virtual void solveConcurrently(
+      std::shared_ptr<Vertex> start_point, std::shared_ptr<Vertex> goal_point,
       std::shared_ptr<MessageQueue<bool>> message_queue) override;
 
-  // new background related functionsm
-  void initBackgroundText();
-  void updateBackgroundText();
-
-  // new functions
-  void sample_free(Point &point);
-  void nearest(std::shared_ptr<Point> &x_near, const Point &x_rand);
-  void steer(std::shared_ptr<Point> &x_new,
-             const std::shared_ptr<Point> &x_near, const Point &x_rand);
-  bool isCollision(const std::shared_ptr<Point> &x_near,
-                   const std::shared_ptr<Point> &x_new);
-  std::vector<double> linspace(double start, double end, int num);
+  /**
+   * @brief Randomly sample a vertex
+   * @param v Sampled vertex
+   */
+  void sample(const std::shared_ptr<Vertex> &v);
+
+  /**
+   * @brief Find the nearest neighbour in a tree
+   * @param v Nearest vertex
+   */
+  void nearest(const std::shared_ptr<const Vertex> &x_rand,
+               std::shared_ptr<Vertex> &x_near);
+
+  /**
+   * @brief The cost to come of a vertex (g-value)
+   */
+  double cost(std::shared_ptr<Vertex> v);
+
+  /**
+   * @brief The euclidean distance between two vertices
+   */
+  double distance(const std::shared_ptr<const Vertex> &v1,
+                  const std::shared_ptr<const Vertex> &v2);
+
+  /**
+   * @brief Check whether collision or not between two vertices
+   * This function assumes from_v vertex is collision-free
+   * @param from_v Starting vertex
+   * @param to_v Ending vertex
+   * @return true if there is a collision otherwise false
+   */
+  bool isCollision(const std::shared_ptr<const Vertex> &from_v,
+                   const std::shared_ptr<const Vertex> &to_v);
+
+  /**
+   * @brief Find the new interpolated vertex from from_v vertex to to_v
+   * vertex
+   * @param from_v Starting vertex
+   * @param to_v Ending vertex
+   * @param t Interpolation distance
+   * @param v New vertex
+   */
+  void interpolate(const std::shared_ptr<const Vertex> &from_v,
+                   const std::shared_ptr<const Vertex> &to_v, const double t,
+                   const std::shared_ptr<Vertex> &v);
+
+  /**
+   * @brief Check whether a vertex lies within goal radius or not
+   */
+  bool inGoalRegion(const std::shared_ptr<const Vertex> &v);
+
+ protected:
+  /**
+   * @brief Interpolation distance during collsion checking
+   */
+  double interpolation_dist_;
+
+  /**
+   * @brief Maximum distance allowed between two vertices
+   */
+  double range_;
+
+  /**
+   * @brief Distance between vertex and goal
+   */
+  double goal_radius_;
 };
+
+}  // namespace sampling_based
+}  // namespace path_finding_visualizer

include/States/Algorithms/GraphBased/BFS/BFS.h

@@ -2,62 +2,72 @@
 
 #include <random>
 
-#include "SamplingBased.h"
-
-class RRT_STAR : public SamplingBased {
- private:
-  // vertices & edges
-  std::vector<std::shared_ptr<Point>> vertices_;
-  std::vector<std::pair<std::shared_ptr<Point>, std::shared_ptr<Point>>> edges_;
-
-  // rrt-star related
-  int max_vertices_;
-  double delta_q_;
-  double goal_radius_;
-  double rewireFactor_;
-  double maxDistance_;
-  double r_rrt_;
+#include "States/Algorithms/SamplingBased/RRT/RRT.h"
 
-  // background text
-  sf::Text maxVerticesText_;
-  sf::Text noOfVerticesText_;
+namespace path_finding_visualizer {
+namespace sampling_based {
 
+class RRT_STAR : public RRT {
  public:
   // Constructor
-  RRT_STAR(sf::RenderWindow *window,
-           std::stack<std::unique_ptr<State>> &states);
+  RRT_STAR(sf::RenderWindow* window,
+           std::stack<std::unique_ptr<State>>& states);
 
   // Destructor
   virtual ~RRT_STAR();
 
-  // override initialization functions
-  void initialize() override;
-  void initAlgorithm() override;
+  virtual void renderParametersGui() override;
 
-  // override render functions
-  void renderBackground() override;
-  void renderAlgorithm() override;
+  // override initialization functions
+  virtual void initialize() override;
+  virtual void initPlanner() override;
+  virtual void initParameters() override;
 
   // override algorithm function
-  void solveConcurrently(
-      std::shared_ptr<Point> start_point, std::shared_ptr<Point> goal_point,
+  virtual void solveConcurrently(
+      std::shared_ptr<Vertex> start_point, std::shared_ptr<Vertex> goal_point,
       std::shared_ptr<MessageQueue<bool>> message_queue) override;
 
-  // new background related functionsm
-  void initBackgroundText();
-  void updateBackgroundText();
-
-  // new functions
-  void sample_free(Point &point);
-  void nearest(std::shared_ptr<Point> &x_near, const Point &x_rand);
-  void near(std::vector<std::shared_ptr<Point>> &X_near,
-            const std::shared_ptr<Point> &x_new);
-  void steer(std::shared_ptr<Point> &x_new,
-             const std::shared_ptr<Point> &x_near, const Point &x_rand);
-  bool isCollision(const std::shared_ptr<Point> &x_near,
-                   const std::shared_ptr<Point> &x_new);
-  std::vector<double> linspace(double start, double end, int num);
-  double cost(std::shared_ptr<Point> p);
-  double l2_Distance(const std::shared_ptr<Point> &p1,
-                     const std::shared_ptr<Point> &p2);
+  /**
+   * @brief Find all the nearest neighbours inside the radius of particular
+   * vertex provided
+   * @param x_new Target vertex
+   * @param X_near Vector of nearest neighbours
+   */
+  void near(const std::shared_ptr<const Vertex>& x_new,
+            std::vector<std::shared_ptr<Vertex>>& X_near);
+
+  /**
+   * @brief Calculate r_rrt_ based on current measure
+   */
+  void updateRewiringLowerBounds();
+
+ protected:
+  /**
+   * @brief Find best goal parent at every n iteration
+   */
+  unsigned int update_goal_every_;
+
+  /**
+   * @brief Rewiring lower bound constant
+   */
+  double r_rrt_;
+
+  /**
+   * @brief Rewiring factor
+   */
+  double rewire_factor_;
+
+  /**
+   * @brief Measure (i.e., n-dimensional volume) of the current state space
+   */
+  double current_measure_;
+
+  /**
+   * @brief Vertices that lie within the goal radius
+   */
+  std::vector<std::shared_ptr<Vertex>> x_soln_;
 };
+
+}  // namespace sampling_based
+}  // namespace path_finding_visualizer