dwa_algorithm

README.md

@@ -1,10 +1,15 @@
+## Requirements
+pip install scikit-fuzzy 
+
+pip install --upgrade numpy 
+
+pip install tensorflow keras gym 
+
+pip install tensorflow or pip install tensorflow==2.10.0 
 # rrt
-Collection of rrt-based algorithms that scale to n-dimensions:
+Collection of rrt-based algorithms in 3-dimensions:
 - rrt
-- rrt* (rrt-star)
-- rrt* (bidirectional)
-- rrt* (bidriectional, lazy shortening)
-- rrt connect
+
 
 Utilizes [R-trees](https://en.wikipedia.org/wiki/R-tree) to improve performance by avoiding point-wise collision-checking and distance-checking.
 
@@ -37,14 +42,8 @@ Assign resolution of edges:
 
 ### Examples
 Visualization examples can be found for rrt and rrt* in both 2 and 3 dimensions.
-- [2D RRT](https://plot.ly/~szanlongo/79/plot/)
+
 - [3D RRT](https://plot.ly/~szanlongo/81/plot/)
-- [2D RRT*](https://plot.ly/~szanlongo/83/plot/)
-- [3D RRT*](https://plot.ly/~szanlongo/89/plot/)
-- [2D Bidirectional RRT*](https://plot.ly/~szanlongo/85/plot/)
-- [3D Bidirectional RRT*](https://plot.ly/~szanlongo/87/plot/)
-- [2D Heuristic Bidirectional RRT*](https://plot.ly/~szanlongo/91/plot/)
-- [3D Heuristic Bidirectional RRT*](https://plot.ly/~szanlongo/93/plot/)
 
 ## Contributing
 
@@ -54,12 +53,3 @@ Visualization examples can be found for rrt and rrt* in both 2 and 3 dimensions.
 4. Push to the branch: `git push origin my-new-feature`
 5. Submit a pull request :D
 
-## References
-
-1. Steven Michael Lavalle. [Planning Algorithms.](https://lavalle.pl/planning/) New York (Ny), Cambridge University Press, 2014, pp. 228–237, lavalle.pl/planning/.
-2. LaValle, Steven. "[Rapidly-exploring random trees: A new tool for path planning.](https://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf)" Research Report 9811 (1998).
-3. Kuffner, James J., and Steven M. LaValle. "[RRT-connect: An efficient approach to single-query path planning.](https://www.cs.cmu.edu/afs/cs/academic/class/15494-s14/readings/kuffner_icra2000.pdf)" Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065). Vol. 2. IEEE, 2000.
-
-## License
-
-[MIT License](https://github.com/motion-planning/rrt-algorithms/blob/master/LICENSE)

examples/rrt/rrt_3d.py

@@ -1,18 +1,22 @@
 # This file is subject to the terms and conditions defined in
 # file 'LICENSE', which is part of this source code package.
 import numpy as np
+import sys
+sys.path.append('/home/dell/rrt-algorithms')
 
 from rrt_algorithms.rrt.rrt import RRT
-from rrt_algorithms.search_space.search_space import SearchSpace
-from rrt_algorithms.utilities.plotting import Plot
+from rrt_algorithms.search_space.search_space0 import SearchSpace
+from rrt_algorithms.utilities.plotting0 import Plot
+
+
 
 X_dimensions = np.array([(0, 100), (0, 100), (0, 100)])  # dimensions of Search Space
 # obstacles
 Obstacles = np.array(
-    [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40),
+    [(10, 10, 10, 15, 15, 15),(20, 20, 20, 30, 30, 30),(30, 30, 30, 35, 35, 35), (1, 2, 6, 4, 4, 8), (20, 60, 20, 25, 65, 25), (60, 60, 20, 80, 80, 40),
      (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)])
 x_init = (0, 0, 0)  # starting location
-x_goal = (100, 100, 100)  # goal location
+x_goal = (8, 50, 50)  # goal location
 
 q = 8  # length of tree edges
 r = 1  # length of smallest edge to check for intersection with obstacles

examples/rrt/rrt_3d_cylinder1.py

@@ -0,0 +1,54 @@
+import numpy as np
+from rtree import index  # Import the index module from the rtree package
+import sys
+sys.path.append('/home/dell/rrt-algorithms')
+
+from rrt_algorithms.rrt.rrt import RRT
+from rrt_algorithms.search_space.search_space import SearchSpace
+from rrt_algorithms.utilities.plotting import Plot
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+#import numpy as np
+# Define the search space dimensions
+X_dimensions = np.array([(0, 100), (0, 100), (0, 100)])
+
+# Define cylindrical obstacles
+obstacles = [
+    (10, 82, 0, 85, 15),  
+    (80, 20, 0, 60, 19),   
+    (10, 30, 0, 90, 15),   
+    (80, 80, 0, 70, 22), 
+    (50, 50, 0, 50, 22),  # Cylinder at (50, 50) with height 15 along z-axis and radius 5
+    ]
+# Define initial and goal positions
+x_init = (-2, 0, 0)  # Starting location
+x_goal = (80, 80, 100)  # Goal location
+
+# Define RRT parameters
+q = 30  # Length of tree edges
+r = 1  # Length of smallest edge to check for intersection with obstacles
+max_samples = 1024  # Max number of samples to take before timing out
+prc = 0.1  # Probability of checking for a connection to goal
+
+# Create the search space
+X = SearchSpace(X_dimensions, obstacles)
+print("Does SearchSpace have obstacle_free method?", hasattr(X, 'obstacle_free'))
+
+# Create RRT search
+rrt = RRT(X, q, x_init, x_goal, max_samples, r, prc)
+path = rrt.rrt_search()
+
+# Plotting setup
+plot = Plot("rrt_3d")
+
+
+
+# Plot the RRT tree, path, and cylindrical obstacles
+plot.plot_tree(X, rrt.trees)
+if path is not None:
+    plot.plot_path(X, path)
+plot.plot_obstacles(X, obstacles)  # Plot the cylindrical obstacles
+plot.plot_start(X, x_init)
+plot.plot_goal(X, x_goal)
+plot.draw(auto_open=True)
+

examples/rrt/rrt_3d_cylinder2.py

@@ -0,0 +1,72 @@
+import numpy as np
+from rtree import index  # Import the index module from the rtree package
+import sys
+sys.path.append('/home/dell/rrt-algorithms')
+
+from rrt_algorithms.rrt.rrt import RRT
+from rrt_algorithms.search_space.search_space import SearchSpace
+from rrt_algorithms.utilities.plotting import Plot
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+
+# Define the search space dimensions
+X_dimensions = np.array([(0, 100), (0, 100), (0, 100)])
+
+# Define cylindrical obstacles
+obstacles = [
+    (10, 82, 0, 85, 15),  
+    (80, 20, 0, 60, 19),   
+    (10, 30, 0, 90, 15),   
+    (80, 80, 0, 70, 22), 
+    (50, 50, 0, 50, 22),  # Cylinder at (50, 50) with height 15 along z-axis and radius 5
+]
+
+# Define initial and goal positions
+x_init = (-2, 0, 0)  # Starting location
+x_intermediate = (50, 50, 100)  # Intermediate goal location
+x_goal = (80, 80, 100)  # Final goal location
+
+# Define RRT parameters
+q = 30  # Length of tree edges
+r = 1  # Length of smallest edge to check for intersection with obstacles
+max_samples = 1024  # Max number of samples to take before timing out
+prc = 0.1  # Probability of checking for a connection to goal
+
+# Create the search space
+X = SearchSpace(X_dimensions, obstacles)
+
+# First RRT search: from x_init to x_intermediate
+print("Starting RRT search from initial point to intermediate goal...")
+rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc)
+path1 = rrt.rrt_search()
+
+# If a path is found to the intermediate goal, continue to the final goal
+if path1 is not None:
+    # Second RRT search: from x_intermediate to x_goal
+    print("Starting RRT search from intermediate goal to final goal...")
+    rrt = RRT(X, q, x_intermediate, x_goal, max_samples, r, prc)
+    path2 = rrt.rrt_search()
+
+    # Combine the two paths
+    if path2 is not None:
+        path = path1 + path2[1:]  # Combine paths, avoiding duplicate point at x_intermediate
+    else:
+        print("Could not find a path to the final goal.")
+        path = path1  # Use only the first path
+else:
+    print("Could not find a path to the intermediate goal.")
+    path = None
+
+# Plotting setup
+plot = Plot("rrt_3d")
+
+# Plot the RRT tree, path, and cylindrical obstacles
+plot.plot_tree(X, rrt.trees)
+if path is not None:
+    plot.plot_path(X, path)
+plot.plot_obstacles(X, obstacles)  # Plot the cylindrical obstacles
+plot.plot_start(X, x_init)
+plot.plot_goal(X, x_intermediate, color="pink")  # Plot the intermediate goal in pink
+plot.plot_goal(X, x_goal, color="green")  # Plot the final goal in green
+plot.draw(auto_open=True)
+

examples/rrt/rrt_3d_cylinder3.py

@@ -0,0 +1,81 @@
+import numpy as np
+from rtree import index  # Import the index module from the rtree package
+import sys
+sys.path.append('/home/dell/rrt-algorithms')
+
+from rrt_algorithms.rrt.rrt import RRT
+from rrt_algorithms.search_space.search_space import SearchSpace
+from rrt_algorithms.utilities.plotting import Plot
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+
+# Define the search space dimensions
+X_dimensions = np.array([(0, 100), (0, 100), (0, 100)])
+
+# Define cylindrical obstacles
+obstacles = [
+    (10, 82, 0, 85, 15),  
+    (80, 20, 0, 60, 19),   
+    (10, 30, 0, 90, 15),   
+    (80, 80, 0, 70, 22), 
+    (50, 50, 0, 50, 22),  # Cylinder at (50, 50) with height 15 along z-axis and radius 5
+]
+
+# Define initial and goal positions
+x_init = (-2, 0, 0)  # Starting location
+x_intermediate = (50, 50, 100)  # First intermediate goal location
+x_second_goal = (70, 20, 100)  # Second intermediate goal location
+x_final_goal = (80, 80, 100)  # Final goal location
+
+# Define RRT parameters
+q = 30  # Length of tree edges
+r = 1  # Length of smallest edge to check for intersection with obstacles
+max_samples = 1024  # Max number of samples to take before timing out
+prc = 0.1  # Probability of checking for a connection to goal
+
+# Create the search space
+X = SearchSpace(X_dimensions, obstacles)
+
+# First RRT search: from x_init to x_intermediate
+print("Starting RRT search from initial point to first intermediate goal...")
+rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc)
+path1 = rrt.rrt_search()
+
+# If a path is found to the first intermediate goal, continue to the second intermediate goal
+if path1 is not None:
+    print("Starting RRT search from first intermediate goal to second intermediate goal...")
+    rrt = RRT(X, q, x_intermediate, x_second_goal, max_samples, r, prc)
+    path2 = rrt.rrt_search()
+
+    if path2 is not None:
+        print("Starting RRT search from second intermediate goal to final goal...")
+        rrt = RRT(X, q, x_second_goal, x_final_goal, max_samples, r, prc)
+        path3 = rrt.rrt_search()
+
+        # Combine all paths
+        if path3 is not None:
+            path = path1 + path2[1:] + path3[1:]  # Combine paths, avoiding duplicate points
+        else:
+            print("Could not find a path to the final goal.")
+            path = path1 + path2[1:]  # Use the first two paths
+    else:
+        print("Could not find a path to the second intermediate goal.")
+        path = path1  # Use only the first path
+else:
+    print("Could not find a path to the first intermediate goal.")
+    path = None
+
+# Plotting setup
+plot = Plot("rrt_3d")
+
+# Plot the RRT tree, path, and cylindrical obstacles
+plot.plot_tree(X, rrt.trees)
+if path is not None:
+    plot.plot_path(X, path)
+plot.plot_obstacles(X, obstacles)  # Plot the cylindrical obstacles
+plot.plot_start(X, x_init)
+plot.plot_goal(X, x_intermediate, color="pink")  # Plot the first intermediate goal in pink
+plot.plot_goal(X, x_second_goal, color="blue")  # Plot the second intermediate goal in blue
+plot.plot_goal(X, x_final_goal, color="green")  # Plot the final goal in green
+plot.draw(auto_open=True)
+

examples/rrt/rrt_3d_cylinder_DWA.py

@@ -0,0 +1,98 @@
+import numpy as np
+from rtree import index
+import sys
+sys.path.append('/home/dell/rrt-algorithms')
+
+from rrt_algorithms.rrt.rrt import RRT
+from rrt_algorithms.search_space.search_space import SearchSpace
+from rrt_algorithms.utilities.plotting import Plot
+from rrt_algorithms.dwa_algorithm.DWA import DWA  # Assuming you have a DWA implementation
+
+# Define the search space dimensions and obstacles
+X_dimensions = np.array([(0, 100), (0, 100), (0, 100)])
+obstacles = [
+    (10, 82, 0, 85, 15),  
+    (80, 20, 0, 60, 19),   
+    (10, 30, 0, 90, 15),   
+    (80, 80, 0, 70, 22), 
+    (50, 50, 0, 50, 22),
+]
+
+# Initial and goal positions
+x_init = (-2, 0, 0)
+x_intermediate = (50, 50, 100)
+x_second_goal = (70, 20, 100)
+x_final_goal = (80, 80, 100)
+
+# RRT parameters
+q = 30
+r = 1
+max_samples = 1024
+prc = 0.1
+
+# DWA parameters
+dwa_params = {
+    'max_speed': 1.0,
+    'min_speed': 0.0,
+    'max_yaw_rate': 40.0 * np.pi / 180.0,
+    'max_accel': 0.2,
+    'v_reso': 0.01,
+    'yaw_rate_reso': 0.1 * np.pi / 180.0,
+    'dt': 0.1,
+    'predict_time': 3.0,
+    'to_goal_cost_gain': 1.0,
+    'speed_cost_gain': 1.0,
+    'obstacle_cost_gain': 1.0,
+}
+
+# Create the search space
+X = SearchSpace(X_dimensions, obstacles)
+
+# RRT pathfinding to first intermediate goal
+print("RRT to first intermediate goal...")
+rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc)
+path1 = rrt.rrt_search()
+
+if path1 is not None:
+    print("RRT to second intermediate goal...")
+    rrt = RRT(X, q, x_intermediate, x_second_goal, max_samples, r, prc)
+    path2 = rrt.rrt_search()
+
+    if path2 is not None:
+        print("RRT to final goal...")
+        rrt = RRT(X, q, x_second_goal, x_final_goal, max_samples, r, prc)
+        path3 = rrt.rrt_search()
+
+        # Combine all paths
+        if path3 is not None:
+            path = path1 + path2[1:] + path3[1:]
+        else:
+            path = path1 + path2[1:]
+    else:
+        path = path1
+else:
+    path = None
+
+# Apply DWA for local optimization along the RRT path
+if path is not None:
+    dwa = DWA(dwa_params)
+    optimized_path = []
+
+    for i in range(len(path) - 1):
+        start_point = path[i] + (0.0, 0.0)  # Initialize v and w to 0, using tuple
+        end_point = path[i + 1]
+
+        local_path = dwa.plan(start_point, end_point, X, obstacles)
+        optimized_path.extend(local_path)
+
+# Plotting
+plot = Plot("rrt_dwa_3d")
+plot.plot_tree(X, rrt.trees)
+if optimized_path is not None:
+    plot.plot_path(X, optimized_path)
+plot.plot_obstacles(X, obstacles)
+plot.plot_start(X, x_init)
+plot.plot_goal(X, x_intermediate, color="pink")
+plot.plot_goal(X, x_second_goal, color="blue")
+plot.plot_goal(X, x_final_goal, color="green")
+plot.draw(auto_open=True)