diff --git a/README.md b/README.md index be2ba5d..26a7966 100644 --- a/README.md +++ b/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) diff --git a/examples/rrt/dqn_model.h5 b/examples/rrt/dqn_model.h5 new file mode 100644 index 0000000..e69de29 diff --git a/examples/rrt/example.h5 b/examples/rrt/example.h5 new file mode 100644 index 0000000..359722e Binary files /dev/null and b/examples/rrt/example.h5 differ diff --git a/examples/rrt/rrt_3d.py b/examples/rrt/rrt_3d.py index 13f6d34..83f418b 100644 --- a/examples/rrt/rrt_3d.py +++ b/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 diff --git a/examples/rrt/rrt_3d_cylinder1.py b/examples/rrt/rrt_3d_cylinder1.py new file mode 100644 index 0000000..ec173d2 --- /dev/null +++ b/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) + diff --git a/examples/rrt/rrt_3d_cylinder2.py b/examples/rrt/rrt_3d_cylinder2.py new file mode 100644 index 0000000..3e4eab5 --- /dev/null +++ b/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) + diff --git a/examples/rrt/rrt_3d_cylinder3.py b/examples/rrt/rrt_3d_cylinder3.py new file mode 100644 index 0000000..1db477f --- /dev/null +++ b/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) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA.py b/examples/rrt/rrt_3d_cylinder_DWA.py new file mode 100644 index 0000000..c38be27 --- /dev/null +++ b/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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA1.py b/examples/rrt/rrt_3d_cylinder_DWA1.py new file mode 100644 index 0000000..c38be27 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA1.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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA2.py b/examples/rrt/rrt_3d_cylinder_DWA2.py new file mode 100644 index 0000000..23bc735 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA2.py @@ -0,0 +1,158 @@ +import numpy as np +import time +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 + +# 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 = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (200, 60, 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) + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, obstacles) + + average_velocity = dwa_params['max_speed'] # Assume max speed for energy calculation + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + if optimized_path is not None: + plot.plot_path(X, optimized_path, color='blue') + 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) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA3.py b/examples/rrt/rrt_3d_cylinder_DWA3.py new file mode 100644 index 0000000..5ece2bc --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA3.py @@ -0,0 +1,159 @@ +import numpy as np +import time +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 + +# 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 = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (100, 100, 0) + +# RRT parameters +q = 60 +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.3, # Increase acceleration to reach optimal speed faster + 'v_reso': 0.05, # Increase resolution for speed and yaw rate + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, # Reduce prediction time to speed up computation + 'to_goal_cost_gain': 1.5, # Increase goal cost to prioritize reaching the goal + 'speed_cost_gain': 0.5, # Decrease speed cost to allow faster speeds + 'obstacle_cost_gain': 1.0, +} + + +# Create the search space +X = SearchSpace(X_dimensions, obstacles) + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, obstacles) + + average_velocity = dwa_params['max_speed'] # Assume max speed for energy calculation + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + plot.plot_path(X, path, color='red') # Plot the original RRT path in red + plot.plot_path(X, optimized_path, color='blue') # Plot the optimized path in blue + 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) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH.py new file mode 100644 index 0000000..c0ba27c --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH.py @@ -0,0 +1,159 @@ +import numpy as np +import time +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_space1 import SearchSpace +from rrt_algorithms.utilities.plotting import Plot +from rrt_algorithms.dwa_algorithm.DWA import DWA + +# Define the search space dimensions and obstacles +# 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 = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (100, 100, 10) + +# RRT parameters +q = 500 +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.3, # Increase acceleration to reach optimal speed faster + 'v_reso': 0.05, # Increase resolution for speed and yaw rate + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, # Reduce prediction time to speed up computation + 'to_goal_cost_gain': 1.5, # Increase goal cost to prioritize reaching the goal + 'speed_cost_gain': 0.5, # Decrease speed cost to allow faster speeds + 'obstacle_cost_gain': 1.0, +} + +# Create the search space with efficient collision checking +X = SearchSpace(X_dimensions, obstacles) + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, obstacles) + + average_velocity = dwa_params['max_speed'] # Assume max speed for energy calculation + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + plot.plot_path(X, path, color='red') # Plot the original RRT path in red + plot.plot_path(X, optimized_path, color='blue') # Plot the optimized path in blue + 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) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1.py new file mode 100644 index 0000000..286de86 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1.py @@ -0,0 +1,179 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +import numpy as np +import time +from rtree import index +import sys +import uuid + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space1 import SearchSpace +from rrt_algorithms.utilities.plotting1 import Plot +from rrt_algorithms.dwa_algorithm.DWA1 import DWA +from rrt_algorithms.utilities.obstacle_generation1 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) + +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (100, 100, 10) + +# Create the search space +X = SearchSpace(X_dimensions) + +# Generate random obstacles using the SearchSpace object +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Predefined static obstacles +static_obstacles = [ + (10, 82, 0, 85, 15), + (80, 20, 0, 60, 19), + (10, 30, 0, 90, 15), + (80, 80, 0, 70, 20), + (50, 50, 0, 50, 13), +] + +# Combine predefined and random obstacles +all_obstacles = static_obstacles + Obstacles + +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +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.3, # Increase acceleration to reach optimal speed faster + 'v_reso': 0.05, # Increase resolution for speed and yaw rate + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, # Reduce prediction time to speed up computation + 'to_goal_cost_gain': 1.5, # Increase goal cost to prioritize reaching the goal + 'speed_cost_gain': 0.5, # Decrease speed cost to allow faster speeds + 'obstacle_cost_gain': 1.0, +} +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] # Assume max speed for energy calculation + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + plot.plot_path(X, path, color='red') # Plot the original RRT path in red + plot.plot_path(X, optimized_path, color='blue') # Plot the optimized path in blue + plot.plot_obstacles(X, all_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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.py new file mode 100644 index 0000000..e082ffb --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.py @@ -0,0 +1,175 @@ +# Texpanding the radius of the obstacles by the radius of the drone (10 cm) to ensure that the drone does not collide with any obstacles. This adjustment effectively treats the drone as a point but expands the obstacles to account for the drone's size. +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA2 import DWA +from rrt_algorithms.dwa_algorithm.DWA2 import FuzzyController +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) +#x_final_goal = (100, 100, 40) +# Create the search space +X = SearchSpace(X_dimensions) + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + plot.plot_path(X, path, color='red') # Plot the original RRT path in red + plot.plot_path(X, optimized_path, color='blue') # Plot the optimized path in blue + plot.plot_obstacles(X, all_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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+2.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+2.py new file mode 100644 index 0000000..53c9c17 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+2.py @@ -0,0 +1,207 @@ +# Texpanding the radius of the obstacles by the radius of the drone (10 cm) to ensure that the drone does not collide with any obstacles. This adjustment effectively treats the drone as a point but expands the obstacles to account for the drone's size. +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles +from tensorflow.keras.models import load_model + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) +#x_final_goal = (100, 100, 40) +# Create the search space + + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + +# Metrics calculation and plotting +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +# Initialize the DroneEnv environment + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=500) +# After training, use the learned policy in your DWA +state = env.reset() +done = False +optimized_path = [] + +while not done: + action = env.agent.act(state) + next_state, _, done = env.step(action) + optimized_path.append(next_state) + state = next_state + +# Initialize the DQN Agent +agent = DQNAgent(env.state_size, env.action_size) + +# Train the DQN agent +#agent.train(env, episodes=10) + +# Save the trained model +agent.model.save("dqn_model.keras") +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"RRT Path Energy Usage: {rrt_energy}") +print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + + +# Plotting +plot = Plot("rrt_dwa_3d") +plot.plot_tree(X, rrt.trees) +plot.plot_path(X, path, color='red') # Plot the original RRT path in red +for point in optimized_path: + print("Point in optimized_path:", point) + +# Filter out incorrectly structured points (e.g., points that aren't tuples or lists with at least two elements) +filtered_optimized_path = [point for point in optimized_path if isinstance(point, (list, tuple)) and len(point) >= 2] + +# Now plot using the filtered path +plot.plot_path(X, filtered_optimized_path, color='blue') # Plot the optimized path in blue + +plot.plot_obstacles(X, all_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) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+3.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+3.py new file mode 100644 index 0000000..68c7588 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+3.py @@ -0,0 +1,382 @@ +# Texpanding the radius of the obstacles by the radius of the drone (10 cm) to ensure that the drone does not collide with any obstacles. This adjustment effectively treats the drone as a point but expands the obstacles to account for the drone's size. +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') +import random +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles +from tensorflow.keras.models import load_model + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) +#x_final_goal = (100, 100, 40) +# Create the search space + + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + """ + Optimize the path using ALNS with adaptive neighborhoods. + :param path: Initial path to optimize. + :param X: Search space object. + :param all_obstacles: List of all obstacles in the environment. + :param max_iterations: Maximum number of iterations for ALNS. + :return: Optimized path. + """ + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + # Select neighborhood adaptively + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + # Evaluate the new path + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + + +def direct_connection(path, X, all_obstacles, r=1): + """ + Directly connect two non-adjacent points to shorten the path. + """ + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) # Select a random starting point + j = random.randint(i + 2, len(path) - 1) # Select a random ending point + + x_a = path[i][:3] # Extract only the spatial coordinates (x, y, z) + x_b = path[j][:3] # Extract only the spatial coordinates (x, y, z) + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] # Remove the points between i and j + + return new_path + + + +# RRT pathfinding to the first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS for global path optimization + alns_optimized_path = alns_optimize_path(optimized_path, X, all_obstacles, max_iterations=100) + + # Ensure all paths are correctly formatted and flatten nested structures + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) # Ensure only the first three elements are used + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + # Validate and correct paths + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + + # Metrics calculation + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + alns_path_length = path_length(alns_optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + alns_smoothness = path_smoothness(alns_optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + # Training the DQN + env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) + env.train(episodes=10) + # After training, use the learned policy in your DWA + + # Initialize the DroneEnv environment for path optimization + state = env.reset() + done = False + optimized_path = [] +agent = DQNAgent(env.state_size, env.action_size) + + + + + + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + optimized_path.append(next_state) + state = next_state + # Save the trained DQN model + #agent = DQNAgent(env.state_size, env.action_size) + # Save the trained model + agent.model.save("dqn_model.keras") + print("DQN model saved as dqn_model.keras") + + + + + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"ALNS Optimized Path Length: {alns_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"ALNS Optimized Path Energy Usage: {alns_energy}") + + +def inspect_and_print_path(path): + """ + Inspects and prints the structure of the path for debugging. + """ + for idx, point in enumerate(path): + print(f"Point {idx} in path: {point} (Type: {type(point)})") + if isinstance(point, (list, tuple)): + for sub_idx, sub_point in enumerate(point): + print(f" Sub-point {sub_idx}: {sub_point} (Type: {type(sub_point)})") +def inspect_path(path, path_name): + if path is None or len(path) == 0: + print(f"{path_name} is empty or None.") + else: + print(f"{path_name} contains {len(path)} points. Sample points: {path[:5]}") + +# Inspect paths +inspect_path(path1, "RRT Path 1") +inspect_path(path2, "RRT Path 2") +inspect_path(path3, "RRT Path 3") +inspect_path(optimized_path, "DWA Optimized Path") +inspect_path(alns_optimized_path, "ALNS Optimized Path") + +def flatten_path_points(path): + """ + Flattens any nested numpy arrays in the path and ensures that each point is a list or tuple with at least three elements. + Returns a cleaned and flattened path. + """ + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + # Flatten the array and convert to a list + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + else: + raise ValueError(f"Point {idx} in optimized_path is not a list or tuple: {point}") + return cleaned_path + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + + +# Debug the flattened path points +for idx, point in enumerate(optimized_path): + print(f"Point {idx} in optimized_path after flattening: {point}") + +# Plotting +plot = Plot("rrt_dwa_alns_3d") + +# Your existing plotting code follows +plot.plot_tree(X, rrt.trees) +plot.plot_path(X, path, color='red') # Plot the original RRT path in red +plot.plot_path(X, optimized_path, color='blue') # Plot the DWA optimized path in blue +plot.plot_path(X, alns_optimized_path, color='green') # Plot the ALNS optimized path in green +plot.plot_obstacles(X, all_obstacles) # Now this line should work +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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+4.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+4.py new file mode 100644 index 0000000..47d25a7 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+4.py @@ -0,0 +1,326 @@ +# Texpanding the radius of the obstacles by the radius of the drone (10 cm) to ensure that the drone does not collide with any obstacles. This adjustment effectively treats the drone as a point but expands the obstacles to account for the drone's size. +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') +import random +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles +from tensorflow.keras.models import load_model + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) +#x_final_goal = (100, 100, 40) +# Create the search space + + +# Generate random obstacles +n = 10 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 500 # Extension length +r = 1 +max_samples = 4000 # Maximum samples +prc = 0.1 # Probability of choosing goal as the next point + +# DWA parameters +dwa_params = { + 'max_speed': 2.0, + 'min_speed': 0.0, + 'max_yaw_rate': 40.0 * np.pi / 180.0, + 'max_accel': 0.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +# Functions for computing metrics +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +# ALNS functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + if path_length(new_path) < path_length(current_path): + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + new_path = path.copy() + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i][:3] + x_b = path[j][:3] + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] + + return new_path + +# Continuous Learning Loop Parameters +episodes = 10 # Number of training episodes +optimize_every = 10 # Frequency of re-optimization and re-training +# Initialize DQN Agent +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +try: + agent = DQNAgent(env.state_size, env.action_size) + agent.model = load_model("dqn_model.keras") + print("Loaded existing DQN model.") +except: + agent = DQNAgent(env.state_size, env.action_size) + print("Initialized new DQN model.") + +# Lists to store paths for animation +paths_to_animate = [] + +# Training Loop +for episode in range(episodes): + print(f"Episode: {episode + 1}/{episodes}") + + state = env.reset() + done = False + episode_path = [] + + # RRT Pathfinding to the goals + print("RRT pathfinding...") + rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) + path1 = rrt.rrt_search() + + if path1 is not None: + rrt = RRT(X, q, x_intermediate, x_second_goal, max_samples, r, prc) + path2 = rrt.rrt_search() + + if path2 is not None: + rrt = RRT(X, q, x_second_goal, x_final_goal, max_samples, r, prc) + path3 = rrt.rrt_search() + + if path3 is not None: + path = path1 + path2[1:] + path3[1:] + else: + path = path1 + path2[1:] + else: + path = path1 + else: + path = None + + if path is not None: + print("Optimizing path with DWA...") + dwa = DWA(dwa_params) + optimized_path = [] + + for i in range(len(path) - 1): + start_point = path[i] + (0.0, 0.0) + end_point = path[i + 1] + local_path = dwa.plan(start_point, end_point, X, all_obstacles) + optimized_path.extend(local_path) + + print("Optimizing path with ALNS...") + alns_optimized_path = alns_optimize_path(optimized_path, X, all_obstacles, max_iterations=100) + + paths_to_animate.append(alns_optimized_path) + + for idx, point in enumerate(alns_optimized_path): + action = agent.act(state) + next_state, reward, done = env.step(action) + agent.remember(state, action, reward, next_state, done) + episode_path.append(next_state) + state = next_state + + if (episode + 1) % optimize_every == 0: + print("Re-training the DQN model...") + agent.replay(32) + agent.model.save("dqn_model.keras") + + # Metrics calculation + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + alns_path_length = path_length(alns_optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + alns_smoothness = path_smoothness(alns_optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"ALNS Optimized Path Length: {alns_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"ALNS Optimized Path Energy Usage: {alns_energy}") + else: + print("No path found, skipping episode.") + +# Plotting and Animation +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') + +# Function to draw cylindrical obstacles +def draw_cylinder(ax, center_x, center_y, z_min, z_max, radius, color='red', alpha=0.5): + z = np.linspace(z_min, z_max, 100) + theta = np.linspace(0, 2*np.pi, 100) + theta_grid, z_grid = np.meshgrid(theta, z) + x_grid = radius * np.cos(theta_grid) + center_x + y_grid = radius * np.sin(theta_grid) + center_y + ax.plot_surface(x_grid, y_grid, z_grid, color=color, alpha=alpha) + +# Function to update the plot +def update(num): + ax.clear() + ax.set_xlim(0, 100) + ax.set_ylim(0, 100) + ax.set_zlim(0, 100) + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + ax.set_title(f"Path Optimization - Episode {num + 1}") + + path = paths_to_animate[num] + x_vals = [point[0] for point in path] + y_vals = [point[1] for point in path] + z_vals = [point[2] for point in path] + ax.plot(x_vals, y_vals, z_vals, color='green') + + for obs in all_obstacles: + draw_cylinder(ax, obs[0], obs[1], obs[2], obs[3], obs[4]) + + ax.scatter(x_init[0], x_init[1], x_init[2], c='blue', marker='o', label="Start") + ax.scatter(x_intermediate[0], x_intermediate[1], x_intermediate[2], c='pink', marker='^', label="Intermediate Goal") + ax.scatter(x_second_goal[0], x_second_goal[1], x_second_goal[2], c='purple', marker='s', label="Second Goal") + ax.scatter(x_final_goal[0], x_final_goal[1], x_final_goal[2], c='yellow', marker='*', label="Final Goal") + +ani = FuncAnimation(fig, update, frames=len(paths_to_animate), interval=10000, repeat=False) +plt.show() diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+5.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+5.py new file mode 100644 index 0000000..fd93781 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+5.py @@ -0,0 +1,334 @@ +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +r = 1 +max_samples = 2024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + # Select neighborhood adaptively + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + # Evaluate the new path + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) # Select a random starting point + j = random.randint(i + 2, len(path) - 1) # Select a random ending point + + x_a = path[i][:3] # Extract only the spatial coordinates (x, y, z) + x_b = path[j][:3] # Extract only the spatial coordinates (x, y, z) + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] # Remove the points between i and j + + return new_path + +# RRT pathfinding to the first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS for global path optimization + alns_optimized_path = alns_optimize_path(optimized_path, X, all_obstacles, max_iterations=100) + + # Ensure all paths are correctly formatted and flatten nested structures + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) # Ensure only the first three elements are used + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + # Validate and correct paths + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + + # Metrics calculation + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + alns_path_length = path_length(alns_optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + alns_smoothness = path_smoothness(alns_optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=10) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +optimized_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + optimized_path.append(next_state) + state = next_state + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") +print(f"RRT Path Energy Usage: {rrt_energy}") +print(f"DWA Optimized Path Energy Usage: {dwa_energy}") +print(f"ALNS Optimized Path Energy Usage: {alns_energy}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + # Flatten the array and convert to a list + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + else: + raise ValueError(f"Point {idx} in optimized_path is not a list or tuple: {point}") + return cleaned_path + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + +# Plotting +plot = Plot("rrt_dwa_alns_3d") + +# Plotting the paths +plot.plot_tree(X, rrt.trees) # Plot the RRT search tree +plot.plot_path(X, path, color='red') # Plot the original RRT path in red +plot.plot_path(X, optimized_path, color='blue') # Plot the DWA optimized path in blue +plot.plot_path(X, alns_optimized_path, color='green') # Plot the ALNS optimized path in green +plot.plot_obstacles(X, all_obstacles) # Plot all obstacles + +# Plot the start and goal positions +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") + +# Draw the plot +plot.draw(auto_open=True) + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png new file mode 100644 index 0000000..e9f3ab3 Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png differ diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.py new file mode 100644 index 0000000..34620ed --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.py @@ -0,0 +1,390 @@ +import numpy as np +import time +from rtree import index +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +r = 1 +max_samples = 2024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + # Select neighborhood adaptively + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + # Evaluate the new path + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) # Select a random starting point + j = random.randint(i + 2, len(path) - 1) # Select a random ending point + + x_a = path[i][:3] # Extract only the spatial coordinates (x, y, z) + x_b = path[j][:3] # Extract only the spatial coordinates (x, y, z) + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] # Remove the points between i and j + + return new_path + +def find_closest_point(path, goal): + distances = [np.linalg.norm(np.array(p[:3]) - np.array(goal)) for p in path] + return np.argmin(distances) + +# RRT pathfinding to the first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS optimization to each segment of the path + print("Applying ALNS optimization...") + alns_optimized_path = [] + + # Find the closest points to the goals + index_intermediate = find_closest_point(optimized_path, x_intermediate) + index_second_goal = find_closest_point(optimized_path, x_second_goal) + + # Segment 1: Start to Intermediate + segment1 = optimized_path[:index_intermediate + 1] + alns_optimized_segment1 = alns_optimize_path(segment1, X, all_obstacles, max_iterations=100) + + # Segment 2: Intermediate to Second Goal + segment2 = optimized_path[index_intermediate:index_second_goal + 1] + alns_optimized_segment2 = alns_optimize_path(segment2, X, all_obstacles, max_iterations=100) + + # Segment 3: Second Goal to Final Goal + segment3 = optimized_path[index_second_goal:] + alns_optimized_segment3 = alns_optimize_path(segment3, X, all_obstacles, max_iterations=100) + + # Combine the ALNS-optimized segments + alns_optimized_path = alns_optimized_segment1 + alns_optimized_segment2[1:] + alns_optimized_segment3[1:] + + # Ensure all paths are correctly formatted and flatten nested structures + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) # Ensure only the first three elements are used + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + # Validate and correct paths + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + +# Metrics calculation +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) +alns_path_length = path_length(alns_optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) +alns_smoothness = path_smoothness(alns_optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) +alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=10) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + elif isinstance(point, (list, tuple)) and len(point) >= 3: + cleaned_path.append(point[:3]) # Handle lists or tuples directly + else: + raise ValueError(f"Point {idx} in optimized_path is not correctly formatted: {point}") + return np.array(cleaned_path) + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) + ml_path = flatten_path_points(ml_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + +# Plotting and Animation + +# First figure: RRT, DWA, ALNS paths with ML path +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim(X_dimensions[0]) +ax.set_ylim(X_dimensions[1]) +ax.set_zlim(X_dimensions[2]) + +# Plot the RRT, DWA, and ALNS paths as dashed lines +ax.plot(*zip(*path), linestyle='--', color='red', label="RRT Path") +ax.plot(*zip(*optimized_path), linestyle='--', color='blue', label="DWA Optimized Path") +ax.plot(*zip(*alns_optimized_path), linestyle='--', color='green', label="ALNS Optimized Path") + +# Plot the ML path +ax.plot(*zip(*ml_path), color='orange', label="ML Path") + +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +# Plot start and goal points +ax.scatter(*x_init, color='magenta', label="Start") +ax.scatter(*x_intermediate, color='pink', label="Intermediate Goal") +ax.scatter(*x_second_goal, color='cyan', label="Second Goal") +ax.scatter(*x_final_goal, color='green', label="Final Goal") + +# Animation of the ML path on the same figure +drone_path_line, = ax.plot([], [], [], color='orange', linewidth=2) + +def update_line(num, ml_path, line): + line.set_data(ml_path[:num, 0], ml_path[:num, 1]) + line.set_3d_properties(ml_path[:num, 2]) + return line, + +ani = animation.FuncAnimation( + fig, update_line, frames=len(ml_path), fargs=(np.array(ml_path), drone_path_line), interval=100, blit=True +) + +# Add legend and show the figure +ax.legend() +plt.show() + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png new file mode 100644 index 0000000..7a9e1f8 Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png differ diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.py new file mode 100644 index 0000000..b9e4f32 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.py @@ -0,0 +1,398 @@ +import numpy as np +import time +from rtree import index +import sys +import uuid +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +r = 1 +max_samples = 2024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + # Select neighborhood adaptively + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + # Evaluate the new path + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) # Select a random starting point + j = random.randint(i + 2, len(path) - 1) # Select a random ending point + + x_a = path[i][:3] # Extract only the spatial coordinates (x, y, z) + x_b = path[j][:3] # Extract only the spatial coordinates (x, y, z) + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] # Remove the points between i and j + + return new_path + +def find_closest_point(path, goal): + distances = [np.linalg.norm(np.array(p[:3]) - np.array(goal)) for p in path] + return np.argmin(distances) + +# RRT pathfinding to the first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS optimization to each segment of the path + print("Applying ALNS optimization...") + alns_optimized_path = [] + + # Find the closest points to the goals + index_intermediate = find_closest_point(optimized_path, x_intermediate) + index_second_goal = find_closest_point(optimized_path, x_second_goal) + + # Segment 1: Start to Intermediate + segment1 = optimized_path[:index_intermediate + 1] + alns_optimized_segment1 = alns_optimize_path(segment1, X, all_obstacles, max_iterations=100) + + # Segment 2: Intermediate to Second Goal + segment2 = optimized_path[index_intermediate:index_second_goal + 1] + alns_optimized_segment2 = alns_optimize_path(segment2, X, all_obstacles, max_iterations=100) + + # Segment 3: Second Goal to Final Goal + segment3 = optimized_path[index_second_goal:] + alns_optimized_segment3 = alns_optimize_path(segment3, X, all_obstacles, max_iterations=100) + + # Combine the ALNS-optimized segments + alns_optimized_path = alns_optimized_segment1 + alns_optimized_segment2[1:] + alns_optimized_segment3[1:] + + # Ensure all paths are correctly formatted and flatten nested structures + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) # Ensure only the first three elements are used + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + # Validate and correct paths + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + +# Metrics calculation +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) +alns_path_length = path_length(alns_optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) +alns_smoothness = path_smoothness(alns_optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) +alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=10) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Convert ml_path to a numpy array +ml_path = np.array(ml_path) + +# Check if ml_path has valid data +if ml_path.size == 0: + print("ML path is empty. Ensure the DQN agent is working correctly.") +else: + print("ML path:", ml_path) + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + elif isinstance(point, (list, tuple)) and len(point) >= 3: + cleaned_path.append(point[:3]) # Handle lists or tuples directly + else: + raise ValueError(f"Point {idx} in optimized_path is not correctly formatted: {point}") + return np.array(cleaned_path) + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) + ml_path = flatten_path_points(ml_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + +# Plotting and Animation + +# First figure: RRT, DWA, ALNS paths with ML path +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim(X_dimensions[0]) +ax.set_ylim(X_dimensions[1]) +ax.set_zlim(X_dimensions[2]) + +# Plot the RRT, DWA, and ALNS paths as dashed lines +ax.plot(*zip(*path), linestyle='--', color='red', label="RRT Path") +ax.plot(*zip(*optimized_path), linestyle='--', color='blue', label="DWA Optimized Path") +ax.plot(*zip(*alns_optimized_path), linestyle='--', color='green', label="ALNS Optimized Path") + +# Plot the ML path +ax.plot(*ml_path.T, color='black', label="ML Path") + +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +# Plot start and goal points +ax.scatter(*x_init, color='magenta', label="Start") +ax.scatter(*x_intermediate, color='pink', label="Intermediate Goal") +ax.scatter(*x_second_goal, color='cyan', label="Second Goal") +ax.scatter(*x_final_goal, color='green', label="Final Goal") + +# Animation of the ML path on the same figure +drone_path_line, = ax.plot([], [], [], color='orange', linewidth=2) + +def update_line(num, ml_path, line): + line.set_data(ml_path[:num, 0], ml_path[:num, 1]) + line.set_3d_properties(ml_path[:num, 2]) + return line, + +ani = animation.FuncAnimation( + fig, update_line, frames=len(ml_path), fargs=(ml_path, drone_path_line), interval=100, blit=True +) + +# Add legend and show the figure +ax.legend() +plt.show() + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size.py new file mode 100644 index 0000000..79d1a7a --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size.py @@ -0,0 +1,171 @@ +# Texpanding the radius of the obstacles by the radius of the drone (10 cm) to ensure that the drone does not collide with any obstacles. This adjustment effectively treats the drone as a point but expands the obstacles to account for the drone's size. +import numpy as np +import time +from rtree import index +import sys +import uuid + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA2 import DWA +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +# Initial and goal positions +x_init = (50, 50, -2) +x_intermediate = (50, 50, 100) +x_second_goal = (20, 60, 100) +x_final_goal = (0, 0, 0) +#x_final_goal = (100, 100, 40) +# Create the search space +X = SearchSpace(X_dimensions) + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +# RRT pathfinding to first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +rrt = RRT(X, q, x_init, x_intermediate, max_samples, r, prc) +path1 = rrt.rrt_search() +rrt_time = time.time() - start_time + +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 = [] + + start_time = time.time() + 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, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Metrics + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + + rrt_smoothness = path_smoothness(path) + dwa_smoothness = path_smoothness(optimized_path) + + rrt_clearance = min_obstacle_clearance(path, all_obstacles) + dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) + + average_velocity = dwa_params['max_speed'] + rrt_energy = compute_energy_usage(path, average_velocity) + dwa_energy = compute_energy_usage(optimized_path, average_velocity) + + # Print Metrics + print(f"RRT Path Length: {rrt_path_length}") + print(f"DWA Optimized Path Length: {dwa_path_length}") + print(f"RRT Path Smoothness: {rrt_smoothness}") + print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") + print(f"RRT Path Minimum Clearance: {rrt_clearance}") + print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") + print(f"RRT Path Energy Usage: {rrt_energy}") + print(f"DWA Optimized Path Energy Usage: {dwa_energy}") + print(f"RRT Path Computation Time: {rrt_time} seconds") + print(f"DWA Optimized Path Computation Time: {dwa_time} seconds") + + # Plotting + plot = Plot("rrt_dwa_3d") + plot.plot_tree(X, rrt.trees) + plot.plot_path(X, path, color='red') # Plot the original RRT path in red + plot.plot_path(X, optimized_path, color='blue') # Plot the optimized path in blue + plot.plot_obstacles(X, all_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) diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png new file mode 100644 index 0000000..5acca41 Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png differ diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.py new file mode 100644 index 0000000..0826fe2 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.py @@ -0,0 +1,554 @@ +#generate random poisitions: +import numpy as np +import time +from rtree import index +import sys +import uuid +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Helper function to generate random positions +def generate_random_position(dimensions): + x_min, x_max = dimensions[0] + y_min, y_max = dimensions[1] + z_min, z_max = dimensions[2] + return ( + random.uniform(x_min, x_max), + random.uniform(y_min, y_max), + random.uniform(z_min, z_max) + ) + +# Initial and goal positions (Randomized) +x_init = generate_random_position(X_dimensions) # Random starting position +x_intermediate = generate_random_position(X_dimensions) # Random intermediate goal +x_second_goal = generate_random_position(X_dimensions) # Random second goal +x_final_goal = generate_random_position(X_dimensions) # Random final goal + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles + +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 300 +r = 1 +max_samples = 2024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i][:3] + x_b = path[j][:3] + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] + + return new_path + +def find_closest_point(path, goal): + distances = [np.linalg.norm(np.array(p[:3]) - np.array(goal)) for p in path] + return np.argmin(distances) + + +#adjust speed as required + +def is_inside_obstacle(point, obstacles): + """ + Check if a point is inside any obstacle. + Assumes obstacles are represented as (x_center, y_center, z_min, z_max, radius). + """ + for obs in obstacles: + x_center, y_center, z_min, z_max, radius = obs + distance = np.linalg.norm(np.array(point[:2]) - np.array([x_center, y_center])) + if distance < radius and z_min <= point[2] <= z_max: + return True + return False + +# Speed calculation and adjustment function +def calculate_required_speed(distance, time): + if time > 0: + return distance / time + return float('inf') # Infinite speed if no time constraint + +def adjust_speed_to_reach_goal(path, total_time, dwa_params, label, ax=None): + if path is None or len(path) == 0: + print(f"Warning: No path found for {label}, skipping speed adjustment.") + return + + path_length_value = path_length(path) + required_speed = calculate_required_speed(path_length_value, total_time) + + # Clip speed to the maximum allowable speed + adjusted_speed = min(required_speed, dwa_params['max_speed']) + dwa_params['max_speed'] = adjusted_speed + + # Print speed in terminal + print(f"Adjusted speed for {label}: {adjusted_speed:.2f} units/s (Distance: {path_length_value:.2f} units, Time: {total_time} s)") + + # Annotate speed on the plot + if ax is not None: + midpoint = np.mean(path, axis=0) + ax.text(midpoint[0], midpoint[1], midpoint[2], f"Speed: {adjusted_speed:.2f} u/s", color='black', fontsize=10) + +# Function to calculate path length +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +# Function to try RRT to a goal and return the path +def try_rrt_to_goal(X, q, start, goal, max_samples, r, prc, obstacles): + rrt = RRT(X, q, start, goal, max_samples, r, prc) + path = rrt.rrt_search() + if path is None: + print(f"RRT failed to find a path to {goal}.") + return path + +# RRT pathfinding to goals +specified_time_intermediate = 20 +specified_time_second = 30 +specified_time_final = 50 + + +print("RRT to first intermediate goal...") +start_time = time.time() +path1 = try_rrt_to_goal(X, q, x_init, x_intermediate, max_samples, r, prc, all_obstacles) +rrt_time = time.time() - start_time + +if path1 is not None: + adjust_speed_to_reach_goal(path1, specified_time_intermediate, dwa_params,"intermediate goal") # Adjust speed for the intermediate goal + + print("RRT to second intermediate goal...") + path2 = try_rrt_to_goal(X, q, x_intermediate, x_second_goal, max_samples, r, prc, all_obstacles) + + if path2 is not None: + adjust_speed_to_reach_goal(path2, specified_time_second, dwa_params,"second goal") # Adjust speed for the second goal + + print("RRT to final goal...") + path3 = try_rrt_to_goal(X, q, x_second_goal, x_final_goal, max_samples, r, prc, all_obstacles) + + # Combine all paths + if path3 is not None: + adjust_speed_to_reach_goal(path3, specified_time_final, dwa_params,"final goal") # Adjust speed for the final goal + path = path1 + path2[1:] + path3[1:] + else: + path = path1 + path2[1:] + else: + path = path1 +else: + path = None + +# Before calculating the path length, check if the path is valid (not None) +if path is not None: + # Calculate and print path length + rrt_path_length = path_length(path) + print(f"RRT Path Length: {rrt_path_length:.2f}") +else: + print("Error: RRT failed to find a valid path. Path length calculation skipped.") + + +# Apply DWA for local optimization along the RRT path +if path is not None: + dwa = DWA(dwa_params) + optimized_path = [] + + start_time = time.time() + for i in range(len(path) - 1): + start_point = path[i] + (0.0, 0.0) + end_point = path[i + 1] + + local_path = dwa.plan(start_point, end_point, X, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS optimization to each segment of the path + print("Applying ALNS optimization...") + alns_optimized_path = [] + + index_intermediate = find_closest_point(optimized_path, x_intermediate) + index_second_goal = find_closest_point(optimized_path, x_second_goal) + + segment1 = optimized_path[:index_intermediate + 1] + alns_optimized_segment1 = alns_optimize_path(segment1, X, all_obstacles, max_iterations=100) + + segment2 = optimized_path[index_intermediate:index_second_goal + 1] + alns_optimized_segment2 = alns_optimize_path(segment2, X, all_obstacles, max_iterations=100) + + segment3 = optimized_path[index_second_goal:] + alns_optimized_segment3 = alns_optimize_path(segment3, X, all_obstacles, max_iterations=100) + + alns_optimized_path = alns_optimized_segment1 + alns_optimized_segment2[1:] + alns_optimized_segment3[1:] + + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + +# Continue the rest of your code as before... + + + +# Metrics calculation +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) +alns_path_length = path_length(alns_optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) +alns_smoothness = path_smoothness(alns_optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) +alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=20) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Convert ml_path to a numpy array +ml_path = np.array(ml_path) + +# Check if ml_path has valid data +if ml_path.size == 0: + print("ML path is empty. Ensure the DQN agent is working correctly.") +else: + print("ML path:", ml_path) + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + elif isinstance(point, (list, tuple)) and len(point) >= 3: + cleaned_path.append(point[:3]) # Handle lists or tuples directly + else: + raise ValueError(f"Point {idx} in optimized_path is not correctly formatted: {point}") + return np.array(cleaned_path) + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) + ml_path = flatten_path_points(ml_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + +# Plotting and Animation + +# First figure: RRT, DWA, ALNS paths with ML path +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim(X_dimensions[0]) +ax.set_ylim(X_dimensions[1]) +ax.set_zlim(X_dimensions[2]) + +# Plot the RRT, DWA, and ALNS paths as dashed lines +ax.plot(*zip(*path), linestyle='--', color='red', label="RRT Path") +ax.plot(*zip(*optimized_path), linestyle='--', color='blue', label="DWA Optimized Path") +ax.plot(*zip(*alns_optimized_path), linestyle='--', color='green', label="ALNS Optimized Path") + +# Plot the ML path +ax.plot(*ml_path.T, color='black', label="ML Path") + +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +# Plot start and goal points +ax.scatter(*x_init, color='magenta', label="Start") +ax.scatter(*x_intermediate, color='pink', label="Intermediate Goal") +ax.scatter(*x_second_goal, color='cyan', label="Second Goal") +ax.scatter(*x_final_goal, color='green', label="Final Goal") + +# Animation of the ML path on the same figure +drone_path_line, = ax.plot([], [], [], color='orange', linewidth=2) + +def update_line(num, ml_path, line): + line.set_data(ml_path[:num, 0], ml_path[:num, 1]) + line.set_3d_properties(ml_path[:num, 2]) + return line, + +ani = animation.FuncAnimation( + fig, update_line, frames=len(ml_path), fargs=(ml_path, drone_path_line), interval=100, blit=True +) + +# Add legend and show the figure +ax.legend() +plt.show() +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +print("RRT to first intermediate goal...") +path1 = try_rrt_to_goal(X, 500, x_init, x_intermediate, 4024, 1, 0.1, all_obstacles) + +if path1 is not None: + adjust_speed_to_reach_goal(path1, specified_time_intermediate, dwa_params, "Intermediate Goal", ax) + ax.plot(*zip(*path1), linestyle='--', color='red', label="RRT to Intermediate") + + print("RRT to second intermediate goal...") + path2 = try_rrt_to_goal(X, 500, x_intermediate, x_second_goal, 4024, 1, 0.1, all_obstacles) + + if path2 is not None: + adjust_speed_to_reach_goal(path2, specified_time_second, dwa_params, "Second Goal", ax) + ax.plot(*zip(*path2), linestyle='--', color='blue', label="RRT to Second Goal") + + print("RRT to final goal...") + path3 = try_rrt_to_goal(X, 500, x_second_goal, x_final_goal, 4024, 1, 0.1, all_obstacles) + + if path3 is not None: + adjust_speed_to_reach_goal(path3, specified_time_final, dwa_params, "Final Goal", ax) + ax.plot(*zip(*path3), linestyle='--', color='green', label="RRT to Final Goal") + else: + print("Failed to reach the final goal.") + else: + print("Failed to reach the second goal.") +else: + print("Failed to reach the intermediate goal.") + +# Apply DWA for local optimization along the RRT path +if path1 is not None and path2 is not None and path3 is not None: + dwa = DWA(dwa_params) + optimized_path = [] + + for i in range(len(path1) - 1): + start_point = path1[i] + (0.0, 0.0) + end_point = path1[i + 1] + local_path = dwa.plan(start_point, end_point, X, all_obstacles) + optimized_path.extend(local_path) + + # Adjust speed for the DWA optimized path + adjust_speed_to_reach_goal(optimized_path, specified_time_final, dwa_params, "DWA Optimized Path", ax) + ax.plot(*zip(*optimized_path), linestyle='--', color='cyan', label="DWA Optimized Path") + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=10) + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Convert ml_path to a numpy array +ml_path = np.array(ml_path) + +# Check if ml_path has valid data +if ml_path.size == 0: + print("ML path is empty. Ensure the DQN agent is working correctly.") +else: + ax.plot(*ml_path.T, color='black', label="ML Path") + ax.legend() + +# Show the plot +plt.show() + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_11.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_11.py new file mode 100644 index 0000000..687fbd1 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_11.py @@ -0,0 +1,535 @@ +#generate random poisitions: +import numpy as np +import time +from rtree import index +import sys +import uuid +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Helper function to generate random positions +def generate_random_position(dimensions): + x_min, x_max = dimensions[0] + y_min, y_max = dimensions[1] + z_min, z_max = dimensions[2] + return ( + random.uniform(x_min, x_max), + random.uniform(y_min, y_max), + random.uniform(z_min, z_max) + ) + +# Initial and goal positions (Randomized) +x_init = generate_random_position(X_dimensions) # Random starting position +x_intermediate = generate_random_position(X_dimensions) # Random intermediate goal +x_second_goal = generate_random_position(X_dimensions) # Random second goal +x_final_goal = generate_random_position(X_dimensions) # Random final goal + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles + +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 300 +r = 1 +max_samples = 2024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i][:3] + x_b = path[j][:3] + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] + + return new_path + +def find_closest_point(path, goal): + distances = [np.linalg.norm(np.array(p[:3]) - np.array(goal)) for p in path] + return np.argmin(distances) + + +#adjust speed as required + +def is_inside_obstacle(point, obstacles): + """ + Check if a point is inside any obstacle. + Assumes obstacles are represented as (x_center, y_center, z_min, z_max, radius). + """ + for obs in obstacles: + x_center, y_center, z_min, z_max, radius = obs + distance = np.linalg.norm(np.array(point[:2]) - np.array([x_center, y_center])) + if distance < radius and z_min <= point[2] <= z_max: + return True + return False + +# Speed calculation and adjustment function +def calculate_required_speed(distance, time): + if time > 0: + return distance / time + return float('inf') # Infinite speed if no time constraint + +def adjust_speed_to_reach_goal(path, total_time, dwa_params, label, ax=None): + if path is None or len(path) == 0: + print(f"Warning: No path found for {label}, skipping speed adjustment.") + return + + path_length_value = path_length(path) + required_speed = calculate_required_speed(path_length_value, total_time) + + # Clip speed to the maximum allowable speed + adjusted_speed = min(required_speed, dwa_params['max_speed']) + dwa_params['max_speed'] = adjusted_speed + + # Print speed in terminal + print(f"Adjusted speed for {label}: {adjusted_speed:.2f} units/s (Distance: {path_length_value:.2f} units, Time: {total_time} s)") + + # Annotate speed on the plot + if ax is not None: + midpoint = np.mean(path, axis=0) + ax.text(midpoint[0], midpoint[1], midpoint[2], f"Speed: {adjusted_speed:.2f} u/s", color='black', fontsize=10) + +# Function to calculate path length +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +# Function to try RRT to a goal and return the path +def try_rrt_to_goal(X, q, start, goal, max_samples, r, prc, obstacles): + rrt = RRT(X, q, start, goal, max_samples, r, prc) + path = rrt.rrt_search() + if path is None: + print(f"RRT failed to find a path to {goal}.") + return path + +# RRT pathfinding to goals +specified_time_intermediate = 20 +specified_time_second = 30 +specified_time_final = 50 + + +print("RRT to first intermediate goal...") +start_time = time.time() +path1 = try_rrt_to_goal(X, q, x_init, x_intermediate, max_samples, r, prc, all_obstacles) +rrt_time = time.time() - start_time + +if path1 is not None: + adjust_speed_to_reach_goal(path1, specified_time_intermediate, dwa_params,"Intermediate Goal") # Adjust speed for the intermediate goal + + print("RRT to second intermediate goal...") + path2 = try_rrt_to_goal(X, q, x_intermediate, x_second_goal, max_samples, r, prc, all_obstacles) + + if path2 is not None: + adjust_speed_to_reach_goal(path2, specified_time_second, dwa_params,"Second Goal") # Adjust speed for the second goal + + print("RRT to final goal...") + path3 = try_rrt_to_goal(X, q, x_second_goal, x_final_goal, max_samples, r, prc, all_obstacles) + + # Combine all paths + if path3 is not None: + adjust_speed_to_reach_goal(path3, specified_time_final, dwa_params,"Final Goal") + # Adjust speed for the final goal + path = path1 + path2[1:] + path3[1:] + else: + path = path1 + path2[1:] + else: + path = path1 +else: + path = None + +# Before calculating the path length, check if the path is valid (not None) +if path is not None: + # Calculate and print path length + rrt_path_length = path_length(path) + print(f"RRT Path Length: {rrt_path_length:.2f}") +else: + print("Error: RRT failed to find a valid path. Path length calculation skipped.") + + +# Apply DWA for local optimization along the RRT path +if path is not None: + dwa = DWA(dwa_params) + optimized_path = [] + + start_time = time.time() + for i in range(len(path) - 1): + start_point = path[i] + (0.0, 0.0) + end_point = path[i + 1] + + local_path = dwa.plan(start_point, end_point, X, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS optimization to each segment of the path + print("Applying ALNS optimization...") + alns_optimized_path = [] + + index_intermediate = find_closest_point(optimized_path, x_intermediate) + index_second_goal = find_closest_point(optimized_path, x_second_goal) + + segment1 = optimized_path[:index_intermediate + 1] + alns_optimized_segment1 = alns_optimize_path(segment1, X, all_obstacles, max_iterations=100) + + segment2 = optimized_path[index_intermediate:index_second_goal + 1] + alns_optimized_segment2 = alns_optimize_path(segment2, X, all_obstacles, max_iterations=100) + + segment3 = optimized_path[index_second_goal:] + alns_optimized_segment3 = alns_optimize_path(segment3, X, all_obstacles, max_iterations=100) + + alns_optimized_path = alns_optimized_segment1 + alns_optimized_segment2[1:] + alns_optimized_segment3[1:] + + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + +# Continue the rest of your code as before... + + + +# Metrics calculation +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) +alns_path_length = path_length(alns_optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) +alns_smoothness = path_smoothness(alns_optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) +alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=20) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Convert ml_path to a numpy array +ml_path = np.array(ml_path) + +# Check if ml_path has valid data +if ml_path.size == 0: + print("ML path is empty. Ensure the DQN agent is working correctly.") +else: + print("ML path:", ml_path) + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + elif isinstance(point, (list, tuple)) and len(point) >= 3: + cleaned_path.append(point[:3]) # Handle lists or tuples directly + else: + raise ValueError(f"Point {idx} in optimized_path is not correctly formatted: {point}") + return np.array(cleaned_path) + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) + ml_path = flatten_path_points(ml_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + + + +# Time calculation function +def calculate_time_to_goal(path_length, speed): + """Calculate the time it takes to reach the goal given the path length and speed.""" + if speed <= 0: + raise ValueError("Speed must be positive to calculate time.") + return path_length / speed + +# Function to print time to reach the goal for each method +def print_time_to_goal(label, path_length, speed): + time_to_goal = calculate_time_to_goal(path_length, speed) + print(f"{label} Time to Goal: {time_to_goal:.2f} seconds") + return time_to_goal + +# Now include the time calculation and improvement comparison +if path is not None: + rrt_path_length = path_length(path) + dwa_path_length = path_length(optimized_path) + alns_path_length = path_length(alns_optimized_path) + + # Assuming max speed for each method (adjusted by algorithms) + rrt_speed = dwa_params['max_speed'] # RRT typically uses the DWA's max speed + dwa_speed = dwa_params['max_speed'] + alns_speed = dwa_params['max_speed'] + + # Machine learning (ML) path speed + ml_speed = dwa_params['max_speed'] # Assuming ML also uses DWA's speed for comparison + + # Print and calculate the time for each algorithm + rrt_time = print_time_to_goal("RRT", rrt_path_length, rrt_speed) + dwa_time = print_time_to_goal("DWA Optimized", dwa_path_length, dwa_speed) + alns_time = print_time_to_goal("ALNS Optimized", alns_path_length, alns_speed) + + # If ML path is valid, calculate its time to goal + if ml_path.size > 0: + ml_path_length = path_length(ml_path) + ml_time = print_time_to_goal("Machine Learning Optimized", ml_path_length, ml_speed) + else: + ml_time = None + print("ML path is not valid for time calculation.") + + # Improvement analysis + print("\n--- Improvement Analysis ---") + if ml_time is not None: + improvement_from_rrt = rrt_time - ml_time + improvement_from_dwa = dwa_time - ml_time + improvement_from_alns = alns_time - ml_time + + print(f"Improvement from RRT: {improvement_from_rrt:.2f} seconds") + print(f"Improvement from DWA: {improvement_from_dwa:.2f} seconds") + print(f"Improvement from ALNS: {improvement_from_alns:.2f} seconds") + else: + print("ML time improvement calculation skipped due to invalid ML path.") + +# The rest of the code (plotting, animation) remains the same as before. + +# Plotting and Animation + +# First figure: RRT, DWA, ALNS paths with ML path +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim(X_dimensions[0]) +ax.set_ylim(X_dimensions[1]) +ax.set_zlim(X_dimensions[2]) + +# Plot the RRT, DWA, and ALNS paths as dashed lines +ax.plot(*zip(*path), linestyle='--', color='red', label="RRT Path") +ax.plot(*zip(*optimized_path), linestyle='--', color='blue', label="DWA Optimized Path") +ax.plot(*zip(*alns_optimized_path), linestyle='--', color='green', label="ALNS Optimized Path") + +# Plot the ML path +ax.plot(*ml_path.T, color='black', label="ML Path") + +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +# Plot start and goal points +ax.scatter(*x_init, color='magenta', label="Start") +ax.scatter(*x_intermediate, color='pink', label="Intermediate Goal") +ax.scatter(*x_second_goal, color='cyan', label="Second Goal") +ax.scatter(*x_final_goal, color='green', label="Final Goal") + +# Animation of the ML path on the same figure +drone_path_line, = ax.plot([], [], [], color='orange', linewidth=2) + +def update_line(num, ml_path, line): + line.set_data(ml_path[:num, 0], ml_path[:num, 1]) + line.set_3d_properties(ml_path[:num, 2]) + return line, + +ani = animation.FuncAnimation( + fig, update_line, frames=len(ml_path), fargs=(ml_path, drone_path_line), interval=100, blit=True +) + +# Add legend and show the figure +ax.legend() +plt.show() + + + + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png new file mode 100644 index 0000000..aa70853 Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png differ diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9.py b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9.py new file mode 100644 index 0000000..da88516 --- /dev/null +++ b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9.py @@ -0,0 +1,434 @@ +#generate random poisitions: +import numpy as np +import time +from rtree import index +import sys +import uuid +import skfuzzy as fuzz +from skfuzzy import control as ctrl +import random +from tensorflow.keras.models import load_model +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA, FuzzyController +from rrt_algorithms.dqn_algorithm.DQN import DQNAgent +from rrt_algorithms.dqn_algorithm.DQN import DroneEnv +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +# Define the search space dimensions +X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) +X = SearchSpace(X_dimensions) + +# Helper function to generate random positions +def generate_random_position(dimensions): + x_min, x_max = dimensions[0] + y_min, y_max = dimensions[1] + z_min, z_max = dimensions[2] + return ( + random.uniform(x_min, x_max), + random.uniform(y_min, y_max), + random.uniform(z_min, z_max) + ) + +# Initial and goal positions (Randomized) +x_init = generate_random_position(X_dimensions) # Random starting position +x_intermediate = generate_random_position(X_dimensions) # Random intermediate goal +x_second_goal = generate_random_position(X_dimensions) # Random second goal +x_final_goal = generate_random_position(X_dimensions) # Random final goal + +# Generate random obstacles +n = 7 # Number of random obstacles +Obstacles = generate_random_cylindrical_obstacles(X, x_init, x_final_goal, n) + +# Define static 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), +] + +# Combine static and randomly generated obstacles into one list +all_obstacles = obstacles + Obstacles + +# Initialize the SearchSpace with all obstacles +X = SearchSpace(X_dimensions, all_obstacles) + +# RRT parameters +q = 700 +r = 1 +max_samples = 4024 +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.3, + 'v_reso': 0.05, + 'yaw_rate_reso': 0.2 * np.pi / 180.0, + 'dt': 0.1, + 'predict_time': 2.0, + 'to_goal_cost_gain': 1.5, + 'speed_cost_gain': 0.5, + 'obstacle_cost_gain': 1.0, +} + +def compute_energy_usage(path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i]) - np.array(path[i-1])) + energy += distance * velocity + return energy + +def min_obstacle_clearance(path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + +def path_length(path): + return sum(np.linalg.norm(np.array(path[i+1]) - np.array(path[i])) for i in range(len(path) - 1)) + +def path_smoothness(path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i]) - np.array(path[i-1]) + vec2 = np.array(path[i+1]) - np.array(path[i]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + +# Adaptive Large Neighborhood Search (ALNS) Functions +def alns_optimize_path(path, X, all_obstacles, max_iterations=100): + neighborhoods = [segment_change, detour_addition, direct_connection] + neighborhood_scores = {segment_change: 1.0, detour_addition: 1.0, direct_connection: 1.0} + current_path = path.copy() + + for _ in range(max_iterations): + neighborhood = random.choices(neighborhoods, weights=neighborhood_scores.values())[0] + new_path = neighborhood(current_path, X, all_obstacles) + + new_path_length = path_length(new_path) + current_path_length = path_length(current_path) + + if new_path_length < current_path_length: + current_path = new_path + neighborhood_scores[neighborhood] *= 1.1 # Reward successful neighborhood + else: + neighborhood_scores[neighborhood] *= 0.9 # Penalize unsuccessful neighborhood + + return current_path + +def segment_change(path, X, all_obstacles, r=1): + if len(path) < 3: + return path + + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i] + x_b = path[j] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + new_point_spatial = spatial_x_a + (spatial_x_b - spatial_x_a) / 2 + new_point = list(new_point_spatial) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, new_point_spatial, r) and X.collision_free(new_point_spatial, spatial_x_b, r): + new_path = path[:i + 1] + [new_point] + path[j:] + return new_path + + return path + +def detour_addition(path, X, all_obstacles, r=1): + if len(path) < 2: + return path + + i = random.randint(0, len(path) - 2) + x_a = path[i] + x_b = path[i + 1] + + spatial_x_a = np.array(x_a[:3]) + spatial_x_b = np.array(x_b[:3]) + + detour_point_3d = spatial_x_a + (np.random.rand(3) - 0.5) * 2 * r + detour_point = list(detour_point_3d) + list(x_a[3:]) + + if X.collision_free(spatial_x_a, detour_point_3d, r) and X.collision_free(detour_point_3d, spatial_x_b, r): + new_path = path[:i + 1] + [detour_point] + path[i + 1:] + return new_path + + return path + +def direct_connection(path, X, all_obstacles, r=1): + if len(path) < 3: + return path # Can't directly connect if there aren't enough points + + new_path = path.copy() + i = random.randint(0, len(path) - 3) + j = random.randint(i + 2, len(path) - 1) + + x_a = path[i][:3] + x_b = path[j][:3] + + if X.collision_free(x_a, x_b, r): + new_path = new_path[:i + 1] + new_path[j:] + + return new_path + +def find_closest_point(path, goal): + distances = [np.linalg.norm(np.array(p[:3]) - np.array(goal)) for p in path] + return np.argmin(distances) + + +def is_inside_obstacle(point, obstacles): + """ + Check if a point is inside any obstacle. + Assumes obstacles are represented as (x_center, y_center, z_min, z_max, radius). + """ + for obs in obstacles: + x_center, y_center, z_min, z_max, radius = obs + distance = np.linalg.norm(np.array(point[:2]) - np.array([x_center, y_center])) + if distance < radius and z_min <= point[2] <= z_max: + return True + return False + +# Function to try RRT to a goal and return the path or None +def try_rrt_to_goal(X, q, start, goal, max_samples, r, prc, obstacles): + if is_inside_obstacle(goal, obstacles): + print(f"Destination {goal} is inside an obstacle and not achievable.") + return None + else: + rrt = RRT(X, q, start, goal, max_samples, r, prc) + return rrt.rrt_search() + +# RRT pathfinding to the first intermediate goal +print("RRT to first intermediate goal...") +start_time = time.time() +path1 = try_rrt_to_goal(X, q, x_init, x_intermediate, max_samples, r, prc, all_obstacles) +rrt_time = time.time() - start_time + +if path1 is not None: + print("RRT to second intermediate goal...") + path2 = try_rrt_to_goal(X, q, x_intermediate, x_second_goal, max_samples, r, prc, all_obstacles) + + if path2 is not None: + print("RRT to final goal...") + path3 = try_rrt_to_goal(X, q, x_second_goal, x_final_goal, max_samples, r, prc, all_obstacles) + + # 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 + +# Before calculating the path length, check if the path is valid (not None) +if path is not None: + # Calculate and print path length + rrt_path_length = path_length(path) + print(f"RRT Path Length: {rrt_path_length}") +else: + print("Error: RRT failed to find a valid path. Path length calculation skipped.") + + +# Apply DWA for local optimization along the RRT path +if path is not None: + dwa = DWA(dwa_params) + optimized_path = [] + + start_time = time.time() + for i in range(len(path) - 1): + start_point = path[i] + (0.0, 0.0) + end_point = path[i + 1] + + local_path = dwa.plan(start_point, end_point, X, all_obstacles) + optimized_path.extend(local_path) + dwa_time = time.time() - start_time + + # Apply ALNS optimization to each segment of the path + print("Applying ALNS optimization...") + alns_optimized_path = [] + + index_intermediate = find_closest_point(optimized_path, x_intermediate) + index_second_goal = find_closest_point(optimized_path, x_second_goal) + + segment1 = optimized_path[:index_intermediate + 1] + alns_optimized_segment1 = alns_optimize_path(segment1, X, all_obstacles, max_iterations=100) + + segment2 = optimized_path[index_intermediate:index_second_goal + 1] + alns_optimized_segment2 = alns_optimize_path(segment2, X, all_obstacles, max_iterations=100) + + segment3 = optimized_path[index_second_goal:] + alns_optimized_segment3 = alns_optimize_path(segment3, X, all_obstacles, max_iterations=100) + + alns_optimized_path = alns_optimized_segment1 + alns_optimized_segment2[1:] + alns_optimized_segment3[1:] + + def validate_and_correct_path(path): + corrected_path = [] + for idx, point in enumerate(path): + if isinstance(point, (list, tuple)) and len(point) >= 2: + if isinstance(point[0], (list, tuple)): + point = [item for sublist in point for item in sublist] + corrected_path.append(point[:3]) + else: + raise ValueError(f"Point {idx} in path is incorrectly formatted: {point}") + return corrected_path + + optimized_path = validate_and_correct_path(optimized_path) + alns_optimized_path = validate_and_correct_path(alns_optimized_path) + +# Continue the rest of your code as before... + + + +# Metrics calculation +rrt_path_length = path_length(path) +dwa_path_length = path_length(optimized_path) +alns_path_length = path_length(alns_optimized_path) + +rrt_smoothness = path_smoothness(path) +dwa_smoothness = path_smoothness(optimized_path) +alns_smoothness = path_smoothness(alns_optimized_path) + +rrt_clearance = min_obstacle_clearance(path, all_obstacles) +dwa_clearance = min_obstacle_clearance(optimized_path, all_obstacles) +alns_clearance = min_obstacle_clearance(alns_optimized_path, all_obstacles) + +average_velocity = dwa_params['max_speed'] +rrt_energy = compute_energy_usage(path, average_velocity) +dwa_energy = compute_energy_usage(optimized_path, average_velocity) +alns_energy = compute_energy_usage(alns_optimized_path, average_velocity) + + +# Training the DQN +env = DroneEnv(X, x_init, x_final_goal, all_obstacles, dwa_params) +env.train(episodes=50) +# After training, use the learned policy in your DWA + +# Initialize the DroneEnv environment for path optimization +state = env.reset() +done = False +ml_path = [] +agent = DQNAgent(env.state_size, env.action_size) + +while not done: + action = agent.act(state) + next_state, _, done = env.step(action) + ml_path.append(next_state) + state = next_state + +# Convert ml_path to a numpy array +ml_path = np.array(ml_path) + +# Check if ml_path has valid data +if ml_path.size == 0: + print("ML path is empty. Ensure the DQN agent is working correctly.") +else: + print("ML path:", ml_path) + +# Save the trained DQN model +agent.model.save("dqn_model.keras") +print("DQN model saved as dqn_model.keras") + +# Print Metrics +print(f"RRT Path Length: {rrt_path_length}") +print(f"DWA Optimized Path Length: {dwa_path_length}") +print(f"ALNS Optimized Path Length: {alns_path_length}") +print(f"RRT Path Smoothness: {rrt_smoothness}") +print(f"DWA Optimized Path Smoothness: {dwa_smoothness}") +print(f"ALNS Optimized Path Smoothness: {alns_smoothness}") +print(f"RRT Path Minimum Clearance: {rrt_clearance}") +print(f"DWA Optimized Path Minimum Clearance: {dwa_clearance}") +print(f"ALNS Optimized Path Minimum Clearance: {alns_clearance}") + +# Ensure the final paths are properly flattened and structured +def flatten_path_points(path): + cleaned_path = [] + for idx, point in enumerate(path): + if isinstance(point, np.ndarray): + flat_point = point.flatten().tolist() + if len(flat_point) >= 3: + cleaned_path.append(flat_point[:3]) # Take only the first three elements (x, y, z) + else: + raise ValueError(f"Point {idx} in optimized_path is incorrectly formatted: {flat_point}") + elif isinstance(point, (list, tuple)) and len(point) >= 3: + cleaned_path.append(point[:3]) # Handle lists or tuples directly + else: + raise ValueError(f"Point {idx} in optimized_path is not correctly formatted: {point}") + return np.array(cleaned_path) + +# Apply the flattening function to optimized_path before plotting +try: + optimized_path = flatten_path_points(optimized_path) + ml_path = flatten_path_points(ml_path) +except ValueError as e: + print(f"Error encountered during flattening: {e}") + +# Plotting and Animation + +# First figure: RRT, DWA, ALNS paths with ML path +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +ax.set_xlim(X_dimensions[0]) +ax.set_ylim(X_dimensions[1]) +ax.set_zlim(X_dimensions[2]) + +# Plot the RRT, DWA, and ALNS paths as dashed lines +ax.plot(*zip(*path), linestyle='--', color='red', label="RRT Path") +ax.plot(*zip(*optimized_path), linestyle='--', color='blue', label="DWA Optimized Path") +ax.plot(*zip(*alns_optimized_path), linestyle='--', color='green', label="ALNS Optimized Path") + +# Plot the ML path +ax.plot(*ml_path.T, color='black', label="ML Path") + +# Plot static obstacles +for obs in all_obstacles: + x_center, y_center, z_min, z_max, radius = obs + u = np.linspace(0, 2 * np.pi, 100) + v = np.linspace(z_min, z_max, 100) + x = radius * np.outer(np.cos(u), np.ones(len(v))) + x_center + y = radius * np.outer(np.sin(u), np.ones(len(v))) + y_center + z = np.outer(np.ones(len(u)), v) + ax.plot_surface(x, y, z, color='gray', alpha=0.3) + +# Plot start and goal points +ax.scatter(*x_init, color='magenta', label="Start") +ax.scatter(*x_intermediate, color='pink', label="Intermediate Goal") +ax.scatter(*x_second_goal, color='cyan', label="Second Goal") +ax.scatter(*x_final_goal, color='green', label="Final Goal") + +# Animation of the ML path on the same figure +drone_path_line, = ax.plot([], [], [], color='orange', linewidth=2) + +def update_line(num, ml_path, line): + line.set_data(ml_path[:num, 0], ml_path[:num, 1]) + line.set_3d_properties(ml_path[:num, 2]) + return line, + +ani = animation.FuncAnimation( + fig, update_line, frames=len(ml_path), fargs=(ml_path, drone_path_line), interval=100, blit=True +) + +# Add legend and show the figure +ax.legend() +plt.show() + + diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png new file mode 100644 index 0000000..9491a69 Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png differ diff --git a/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png new file mode 100644 index 0000000..5ec921c Binary files /dev/null and b/examples/rrt/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png differ diff --git a/examples/rrt/rrt_3d_with_random_obstacles.py b/examples/rrt/rrt_3d_with_random_obstacles.py index 380d706..dfee03a 100644 --- a/examples/rrt/rrt_3d_with_random_obstacles.py +++ b/examples/rrt/rrt_3d_with_random_obstacles.py @@ -1,11 +1,12 @@ # 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.search_space.search_space0 import SearchSpace from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot +from rrt_algorithms.utilities.plotting0 import Plot X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space x_init = (0, 0, 0) # starting location diff --git a/examples/rrt/test.py b/examples/rrt/test.py new file mode 100644 index 0000000..517c76d --- /dev/null +++ b/examples/rrt/test.py @@ -0,0 +1,29 @@ +import h5py +import numpy as np + +# Sample data +data = np.random.random((100, 100)) +def print_h5_structure(name, obj): + """Function to print the structure of an HDF5 file.""" + if isinstance(obj, h5py.Group): + print(f"{name} (Group)") + elif isinstance(obj, h5py.Dataset): + print(f"{name} (Dataset) - Shape: {obj.shape} - Dtype: {obj.dtype}") +# Writing data to an HDF5 file +file_path = "example.h5" + +with h5py.File(file_path, "w") as h5file: + # Creating a dataset in the file + h5file.create_dataset("dataset_name", data=data) + # Optionally flush data to disk + h5file.flush() + +# Verify that data was written correctly +with h5py.File(file_path, "r") as h5file: + if "dataset_name" in h5file: + dataset = h5file["dataset_name"] + print("Data shape:", dataset.shape) + print("Sample data:", dataset[:5, :5]) + else: + print("Dataset not found in the file.") + diff --git a/examples/rrt_bid/rrt_star_bid_2d.py b/examples/rrt_bid/rrt_star_bid_2d.py deleted file mode 100644 index 9f7827d..0000000 --- a/examples/rrt_bid/rrt_star_bid_2d.py +++ /dev/null @@ -1,38 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star_bid import RRTStarBidirectional -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), - (60, 20, 80, 40), (60, 60, 80, 80)]) -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.1 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStarBidirectional( - X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star_bidirectional() - -# plot -plot = Plot("rrt_star_bid_2d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_bid/rrt_star_bid_3d.py b/examples/rrt_bid/rrt_star_bid_3d.py deleted file mode 100644 index 1ce2d40..0000000 --- a/examples/rrt_bid/rrt_star_bid_3d.py +++ /dev/null @@ -1,39 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star_bid import RRTStarBidirectional -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting 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), - (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 - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.01 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStarBidirectional( - X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star_bidirectional() - -# plot -plot = Plot("rrt_star_bid_3d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_bid_h/rrt_star_bid_h_2d.py b/examples/rrt_bid_h/rrt_star_bid_h_2d.py deleted file mode 100644 index 7cf3ec4..0000000 --- a/examples/rrt_bid_h/rrt_star_bid_h_2d.py +++ /dev/null @@ -1,37 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), - (60, 20, 80, 40), (60, 60, 80, 80)]) -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.01 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star_bid_h() - -# plot -plot = Plot("rrt_star_bid_h_2d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_bid_h/rrt_star_bid_h_3d.py b/examples/rrt_bid_h/rrt_star_bid_h_3d.py deleted file mode 100644 index 35f1b81..0000000 --- a/examples/rrt_bid_h/rrt_star_bid_h_3d.py +++ /dev/null @@ -1,38 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting 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), - (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 - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.01 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star_bid_h() - -# plot -plot = Plot("rrt_star_bid_h_3d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_bid_h/rrt_star_bid_h_3d_with_random_obstacles.py b/examples/rrt_bid_h/rrt_star_bid_h_3d_with_random_obstacles.py deleted file mode 100644 index 794aee6..0000000 --- a/examples/rrt_bid_h/rrt_star_bid_h_3d_with_random_obstacles.py +++ /dev/null @@ -1,38 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -x_init = (0, 0, 0) # starting location -x_goal = (100, 100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.01 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions) -n = 50 -Obstacles = generate_random_obstacles(X, x_init, x_goal, n) - -# create rrt_search -rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star_bid_h() - -# plot -plot = Plot("rrt_star_bid_h_3d_with_random_obstacles") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_connect/rrt_connect_2d.py b/examples/rrt_connect/rrt_connect_2d.py deleted file mode 100644 index 0c6f37d..0000000 --- a/examples/rrt_connect/rrt_connect_2d.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_connect import RRTConnect -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), - (60, 20, 80, 40), (60, 60, 80, 80)]) -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 2 # length of tree edges -r = 0.5 # 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 search space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) -path = rrt_connect.rrt_connect() -# plot -plot = Plot("rrt_connect2d") -plot.plot_tree(X, rrt_connect.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_connect/rrt_connect_2d_with_random_obstacles.py b/examples/rrt_connect/rrt_connect_2d_with_random_obstacles.py deleted file mode 100644 index 5ed0a09..0000000 --- a/examples/rrt_connect/rrt_connect_2d_with_random_obstacles.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_connect import RRTConnect -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 2 # length of tree edges -r = 0.5 # length of smallest edge to check for intersection with obstacles -max_samples = 2048 # max number of samples to take before timing out -prc = 0.1 # probability of checking for a connection to goal - -# create search space -X = SearchSpace(X_dimensions) -n = 50 -Obstacles = generate_random_obstacles(X, x_init, x_goal, n) - -# create rrt_search -rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) -path = rrt_connect.rrt_connect() -# plot -plot = Plot("rrt_connect_2d_with_random_obstacles") -plot.plot_tree(X, rrt_connect.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_connect/rrt_connect_3d.py b/examples/rrt_connect/rrt_connect_3d.py deleted file mode 100644 index feaf20c..0000000 --- a/examples/rrt_connect/rrt_connect_3d.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_connect import RRTConnect -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting 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), - (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 - -q = 2 # length of tree edges -r = 0.5 # 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 search space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) -path = rrt_connect.rrt_connect() -# plot -plot = Plot("rrt_connect_3d") -plot.plot_tree(X, rrt_connect.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_connect/rrt_connect_3d_with_random_obstacles.py b/examples/rrt_connect/rrt_connect_3d_with_random_obstacles.py deleted file mode 100644 index ba992ba..0000000 --- a/examples/rrt_connect/rrt_connect_3d_with_random_obstacles.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_connect import RRTConnect -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -x_init = (0, 0, 0) # starting location -x_goal = (100, 100, 100) # goal location - -q = 2 # length of tree edges -r = 0.5 # 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 search space -X = SearchSpace(X_dimensions) -n = 50 -Obstacles = generate_random_obstacles(X, x_init, x_goal, n) - -# create rrt_search -rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) -path = rrt_connect.rrt_connect() -# plot -plot = Plot("rrt_connect_3d_with_random_obstacles") -plot.plot_tree(X, rrt_connect.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_star/rrt_star_2d.py b/examples/rrt_star/rrt_star_2d.py deleted file mode 100644 index 2fd9093..0000000 --- a/examples/rrt_star/rrt_star_2d.py +++ /dev/null @@ -1,37 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star import RRTStar -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -# obstacles -Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), - (60, 20, 80, 40), (60, 60, 80, 80)]) -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.1 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star() - -# plot -plot = Plot("rrt_star_2d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_star/rrt_star_2d_with_random_obstacles.py b/examples/rrt_star/rrt_star_2d_with_random_obstacles.py deleted file mode 100644 index 9c0cd4e..0000000 --- a/examples/rrt_star/rrt_star_2d_with_random_obstacles.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star import RRTStar -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space -x_init = (0, 0) # starting location -x_goal = (100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.1 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions) -n = 50 -Obstacles = generate_random_obstacles(X, x_init, x_goal, n) -# create rrt_search -rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star() - -# plot -plot = Plot("rrt_star_2d_with_random_obstacles") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_star/rrt_star_3d.py b/examples/rrt_star/rrt_star_3d.py deleted file mode 100644 index 4b6e6d6..0000000 --- a/examples/rrt_star/rrt_star_3d.py +++ /dev/null @@ -1,38 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star import RRTStar -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.plotting 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), - (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 - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.1 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions, Obstacles) - -# create rrt_search -rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star() - -# plot -plot = Plot("rrt_star_3d") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/examples/rrt_star/rrt_star_3d_with_random_obstacles.py b/examples/rrt_star/rrt_star_3d_with_random_obstacles.py deleted file mode 100644 index b1bf49b..0000000 --- a/examples/rrt_star/rrt_star_3d_with_random_obstacles.py +++ /dev/null @@ -1,37 +0,0 @@ -# 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 - -from rrt_algorithms.rrt.rrt_star import RRTStar -from rrt_algorithms.search_space.search_space import SearchSpace -from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles -from rrt_algorithms.utilities.plotting import Plot - -X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space -x_init = (0, 0, 0) # starting location -x_goal = (100, 100, 100) # goal location - -q = 8 # 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 -rewire_count = 32 # optional, number of nearby branches to rewire -prc = 0.1 # probability of checking for a connection to goal - -# create Search Space -X = SearchSpace(X_dimensions) -n = 50 -Obstacles = generate_random_obstacles(X, x_init, x_goal, n) -# create rrt_search -rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) -path = rrt.rrt_star() - -# plot -plot = Plot("rrt_star_3d_with_random_obstacles") -plot.plot_tree(X, rrt.trees) -if path is not None: - plot.plot_path(X, path) -plot.plot_obstacles(X, Obstacles) - -plot.plot_start(X, x_init) -plot.plot_goal(X, x_goal) -plot.draw(auto_open=True) diff --git a/output/1 b/output/1 new file mode 100644 index 0000000..53752db --- /dev/null +++ b/output/1 @@ -0,0 +1 @@ +output diff --git a/output/visualizations/12 b/output/visualizations/12 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/output/visualizations/12 @@ -0,0 +1 @@ + diff --git a/output/visualizations/5th_iteration.png b/output/visualizations/5th_iteration.png new file mode 100644 index 0000000..1534a06 Binary files /dev/null and b/output/visualizations/5th_iteration.png differ diff --git a/output/visualizations/6th_iteration.png b/output/visualizations/6th_iteration.png new file mode 100644 index 0000000..2835282 Binary files /dev/null and b/output/visualizations/6th_iteration.png differ diff --git a/output/visualizations/6th_iteration_.png b/output/visualizations/6th_iteration_.png new file mode 100644 index 0000000..4d99828 Binary files /dev/null and b/output/visualizations/6th_iteration_.png differ diff --git a/output/visualizations/7TH_Iteration.png b/output/visualizations/7TH_Iteration.png new file mode 100644 index 0000000..301ad9b Binary files /dev/null and b/output/visualizations/7TH_Iteration.png differ diff --git a/output/visualizations/8th_iteration.png b/output/visualizations/8th_iteration.png new file mode 100644 index 0000000..4ade0cb Binary files /dev/null and b/output/visualizations/8th_iteration.png differ diff --git a/output/visualizations/rrt_3d.html b/output/visualizations/rrt_3d.html new file mode 100644 index 0000000..fbefbd4 --- /dev/null +++ b/output/visualizations/rrt_3d.html @@ -0,0 +1,14 @@ + + + +
+
+ + \ No newline at end of file diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1+1.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1+1.png new file mode 100644 index 0000000..178809c Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1+1.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1.png new file mode 100644 index 0000000..3800016 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_.png new file mode 100644 index 0000000..0f8ae38 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.png new file mode 100644 index 0000000..f0301a2 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+1.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png new file mode 100644 index 0000000..e9f3ab3 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+7.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png new file mode 100644 index 0000000..7a9e1f8 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size+8.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size.png new file mode 100644 index 0000000..622956f Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png new file mode 100644 index 0000000..5acca41 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10_1.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10_1.png new file mode 100644 index 0000000..6a7ed92 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_10_1.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png new file mode 100644 index 0000000..aa70853 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9+3.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png new file mode 100644 index 0000000..9491a69 Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_1.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png new file mode 100644 index 0000000..5ec921c Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_9_2.png differ diff --git a/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_q700.png b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_q700.png new file mode 100644 index 0000000..6fc78cd Binary files /dev/null and b/output/visualizations/rrt_3d_cylinder_DWA_BVH_1_drone_size_q700.png differ diff --git a/output/visualizations/rrt_3d_with_random_obstacles.html b/output/visualizations/rrt_3d_with_random_obstacles.html new file mode 100644 index 0000000..914ccb4 --- /dev/null +++ b/output/visualizations/rrt_3d_with_random_obstacles.html @@ -0,0 +1,14 @@ + + + +
+
+ + \ No newline at end of file diff --git a/output/visualizations/rrt_connect_3d.html b/output/visualizations/rrt_connect_3d.html new file mode 100644 index 0000000..8623af3 --- /dev/null +++ b/output/visualizations/rrt_connect_3d.html @@ -0,0 +1,14 @@ + + + +
+
+ + \ No newline at end of file diff --git a/output/visualizations/rrt_connect_3d_with_random_obstacles.html b/output/visualizations/rrt_connect_3d_with_random_obstacles.html new file mode 100644 index 0000000..a119443 --- /dev/null +++ b/output/visualizations/rrt_connect_3d_with_random_obstacles.html @@ -0,0 +1,14 @@ + + + +
+
+ + \ No newline at end of file diff --git a/output/visualizations/rrt_dwa_3d.html b/output/visualizations/rrt_dwa_3d.html new file mode 100644 index 0000000..05462e9 --- /dev/null +++ b/output/visualizations/rrt_dwa_3d.html @@ -0,0 +1,14 @@ + + + +
+
+ + \ No newline at end of file diff --git a/rrt_algorithms/dqn_algorithm/12 b/rrt_algorithms/dqn_algorithm/12 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/rrt_algorithms/dqn_algorithm/12 @@ -0,0 +1 @@ + diff --git a/rrt_algorithms/dqn_algorithm/DQN.py b/rrt_algorithms/dqn_algorithm/DQN.py new file mode 100644 index 0000000..61f53fe --- /dev/null +++ b/rrt_algorithms/dqn_algorithm/DQN.py @@ -0,0 +1,200 @@ +import numpy as np +import tensorflow as tf +from tensorflow.keras.models import Sequential +from tensorflow.keras.layers import Dense +from tensorflow.keras.optimizers import Adam +from collections import deque +import random +import sys +import uuid +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA +from rrt_algorithms.dwa_algorithm.DWA3 import FuzzyController +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles +class DQNAgent: + def __init__(self, state_size, action_size): + self.state_size = state_size + self.action_size = action_size + self.memory = deque(maxlen=2000) + self.gamma = 0.95 # discount rate + self.epsilon = 1.0 # exploration rate + self.epsilon_min = 0.01 + self.epsilon_decay = 0.995 + self.learning_rate = 0.001 + self.model = self._build_model() + + def _build_model(self): + model = Sequential() + model.add(Dense(24, input_dim=6, activation='relu')) # Input_dim should be 6 + model.add(Dense(24, activation='relu')) + model.add(Dense(self.action_size, activation='linear')) + model.compile(loss='mse', optimizer=Adam(learning_rate=self.learning_rate)) + return model + + def remember(self, state, action, reward, next_state, done): + self.memory.append((state, action, reward, next_state, done)) + + def act(self, state): + if np.random.rand() <= self.epsilon: + return random.randrange(self.action_size) + act_values = self.model.predict(state) + return np.argmax(act_values[0]) + + def replay(self, batch_size): + # Ensure there are enough samples in memory to form a full batch + if len(self.memory) < batch_size: + return + + minibatch = random.sample(self.memory, batch_size) + for state, action, reward, next_state, done in minibatch: + target = reward + if not done: + target = (reward + self.gamma * + np.amax(self.model.predict(next_state)[0])) + target_f = self.model.predict(state) + target_f[0][action] = target + self.model.fit(state, target_f, epochs=1, verbose=0) + if self.epsilon > self.epsilon_min: + self.epsilon *= self.epsilon_decay + +class DroneEnv: + def __init__(self, X, start, goal, obstacles, dwa_params): + self.X = X + self.start = start + self.goal = goal + self.obstacles = obstacles + self.dwa_params = dwa_params + self.state_size = 6 + self.action_size = 3 + self.agent = DQNAgent(self.state_size, self.action_size) + self.reset() + + def reset(self): + self.state = np.array(self.start + (0.0, 0.0, 0.0)) + return self.state.reshape(1, -1) + + def step(self, action): + velocity_change, yaw_change = self.map_action_to_control(action) + next_state, reward, done = self.perform_action(velocity_change, yaw_change) + return next_state, reward, done + + def train(self, episodes, batch_size=64): # Set your batch size here + for episode in range(episodes): + state = self.reset() + total_reward = 0 + done = False + + while not done: + action = self.agent.act(state) + next_state, reward, done = self.step(action) + total_reward += reward + self.agent.remember(state, action, reward, next_state, done) + state = next_state + + # Replay experiences with the set batch size + self.agent.replay(batch_size) + + print(f"Episode: {episode + 1}/{episodes}, Total Reward: {total_reward}") + + + def compute_reward(self, state, path): + # Distance to the goal + distance_to_goal = np.linalg.norm(np.array(state[:2]) - np.array(self.goal[:2])) + + # Obstacle clearance + obstacle_clearance = self.min_obstacle_clearance(path, self.obstacles) + + # Path smoothness (penalizing sharp turns) + path_smoothness = self.path_smoothness(path) + + # Energy usage (penalizing high energy usage) + energy_usage = self.compute_energy_usage(path, state[3]) # state[3] is the current velocity + + # Calculate the reward + reward = -distance_to_goal # Encourage getting closer to the goal + + # Penalize getting too close to obstacles + if obstacle_clearance < 1.0: # 1.0 is a threshold, you can adjust this + reward -= 100 * (1.0 - obstacle_clearance) + + # Penalize sharp turns in the path + reward -= 10 * path_smoothness + + # Penalize high energy usage + reward -= 0.1 * energy_usage + + # Small positive reward for staying alive (not crashing) + reward += 1.0 + + return reward + + + def perform_action(self, velocity_change, yaw_change): + state = self.state + new_velocity = state[3] + velocity_change + new_yaw_rate = state[4] + yaw_change + + # Ensure the state vector always has 6 elements + state = np.array([state[0], state[1], state[2], new_velocity, new_yaw_rate, state[5]]) # Adjust the last value as needed + + dwa = DWA(self.dwa_params) + local_path = dwa.plan(state, self.goal, self.X, self.obstacles) + + if not local_path: + reward = -100 # Collision penalty + done = True + updated_state = state # Maintain the current state + else: + # Ensure the state from local_path[-1] is extended to 6 elements + updated_state = np.array(local_path[-1]) + if len(updated_state) == 5: + updated_state = np.append(updated_state, state[5]) # Maintain the last element (e.g., energy usage or other feature) + + reward = self.compute_reward(updated_state, local_path) + done = np.linalg.norm(np.array(updated_state[:3]) - np.array(self.goal[:3])) < 1.0 # Check if the goal is reached + + print("State shape after update:", updated_state.shape) + return updated_state.reshape(1, -1), reward, done # This return should be inside the function + + +# Ensure all other parts of the code that handle the state are consistent with the 6-element structure. + + def min_obstacle_clearance(self, path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + def path_smoothness(self, path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i][:3]) - np.array(path[i-1][:3]) # Use the first 3 elements if these are the coordinates + vec2 = np.array(path[i+1][:3]) - np.array(path[i][:3]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + def compute_energy_usage(self, path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i][:3]) - np.array(path[i-1][:3])) + energy += distance * velocity + return energy + + + + def map_action_to_control(self, action): + # Simplified action space + velocity_change = [-0.1, 0.0, 0.1][action % 3] # Example action mapping + yaw_change = 0 # Keep yaw_change static for simplicity + return velocity_change, yaw_change + diff --git a/rrt_algorithms/dqn_algorithm/DQN1.py b/rrt_algorithms/dqn_algorithm/DQN1.py new file mode 100644 index 0000000..abb7329 --- /dev/null +++ b/rrt_algorithms/dqn_algorithm/DQN1.py @@ -0,0 +1,209 @@ +import numpy as np +import tensorflow as tf +from tensorflow.keras.models import Sequential +from tensorflow.keras.layers import Dense +from tensorflow.keras.optimizers import Adam +from collections import deque +import random +import sys +import uuid +import skfuzzy as fuzz +from skfuzzy import control as ctrl +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.rrt.rrt import RRT +from rrt_algorithms.search_space.search_space2 import SearchSpace +from rrt_algorithms.utilities.plotting2 import Plot +from rrt_algorithms.dwa_algorithm.DWA3 import DWA +from rrt_algorithms.dwa_algorithm.DWA3 import FuzzyController +from rrt_algorithms.utilities.obstacle_generation2 import generate_random_cylindrical_obstacles + +class DQNAgent: + def __init__(self, state_size, action_size): + self.state_size = state_size + self.action_size = action_size + self.memory = deque(maxlen=2000) + self.gamma = 0.95 # discount rate + self.epsilon = 1.0 # exploration rate + self.epsilon_min = 0.01 + self.epsilon_decay = 0.995 + self.learning_rate = 0.001 + self.model = self._build_model() + + def _build_model(self): + model = Sequential() + model.add(Dense(24, input_dim=self.state_size, activation='relu')) + model.add(Dense(24, activation='relu')) + model.add(Dense(self.action_size, activation='linear')) + model.compile(loss='mse', optimizer=Adam(learning_rate=self.learning_rate)) + return model + + def act(self, state): + """Selects an action using an epsilon-greedy strategy.""" + state = np.reshape(state, [1, self.state_size]) + if np.random.rand() <= self.epsilon: + return random.randrange(self.action_size) + act_values = self.model.predict(state) + return np.argmax(act_values[0]) + + + + def remember(self, state, action, reward, next_state, done): + self.memory.append((state, action, reward, next_state, done)) + + + + + + def replay(self, batch_size): + if len(self.memory) < batch_size: + return + + minibatch = random.sample(self.memory, batch_size) + for state, action, reward, next_state, done in minibatch: + target = reward + if not done: + next_state = np.reshape(next_state, [1, self.state_size]) # Reshape next_state + target = (reward + self.gamma * np.amax(self.model.predict(next_state)[0])) + state = np.reshape(state, [1, self.state_size]) # Reshape state + target_f = self.model.predict(state) + target_f[0][action] = target + self.model.fit(state, target_f, epochs=1, verbose=0) + + if self.epsilon > self.epsilon_min: + self.epsilon *= self.epsilon_decay + +class DroneEnv: + def __init__(self, search_space, start_position, goal_position, obstacles, dwa_params): + self.search_space = search_space + self.start_position = start_position + self.goal_position = goal_position + self.obstacles = obstacles + self.dwa_params = dwa_params + self.state_size = 5 # Make sure this matches the state size + self.action_size = 9 # Assuming 3 velocity changes and 3 yaw changes + self.agent = DQNAgent(self.state_size, self.action_size) # Ensure the DQNAgent is correctly initialized + self.goal_threshold = 1.0 # Set the threshold for considering the goal as "reached" + self.reset() + + def reset(self): + self.current_state = np.array(list(self.start_position) + [0.0, 0.0]) # [x, y, z, velocity, yaw_rate] + return self.current_state + + + def step(self, action): + velocity_change, yaw_change = self.map_action_to_control(action) + updated_state, reward, done = self.perform_action(velocity_change, yaw_change) + + # Check if the drone is close enough to the goal to consider the episode done + if np.linalg.norm(updated_state[:3] - np.array(self.goal_position[:3])) < 1.0: + done = True # Set the done flag when the goal is reached + + return updated_state, reward, done + + + def map_action_to_control(self, action): + """ + Map an action index to a specific velocity change and yaw change. + """ + velocity_change = [-0.1, 0.0, 0.1][action % 3] # Modulo 3 to cycle through velocity options + yaw_change = [-0.1, 0.0, 0.1][(action // 3) % 3] # Integer division by 3 and modulo 3 for yaw options + + return velocity_change, yaw_change + + + def perform_action(self, velocity_change, yaw_change): + # Use the current state (already stored in the class) + state = self.current_state # Access the current state + new_velocity = state[3] + velocity_change + new_yaw_rate = state[4] + yaw_change + + # Update the state + new_state = np.array([state[0], state[1], state[2], new_velocity, new_yaw_rate]) + + # Calculate reward based on the new state + reward = self.compute_reward(new_state) + + # Check if the goal is reached or there was a collision + done = self.is_goal_reached(new_state) + + return new_state, reward, done + + + def compute_reward(self, state): + # Calculate the distance to the goal + distance_to_goal = np.linalg.norm(state[:3] - self.goal_position[:3]) + + # Reward for reaching the goal + if distance_to_goal < self.goal_threshold: + return 1000 # Large reward for reaching the goal + + # Penalize based on the distance to the goal + reward = -distance_to_goal + + # Optionally, add additional penalties for collisions or other constraints + + return reward + + def is_goal_reached(self, state): + # Check if the drone is within the threshold distance to the goal + return np.linalg.norm(state[:3] - self.goal_position[:3]) < self.goal_threshold + + + + + + # Rest of the code remains the same + + def min_obstacle_clearance(self, path, obstacles): + min_clearance = float('inf') + for point in path: + for obs in obstacles: + clearance = np.linalg.norm(np.array(point[:2]) - np.array(obs[:2])) - obs[4] + if clearance < min_clearance: + min_clearance = clearance + return min_clearance + + def path_smoothness(self, path): + total_curvature = 0.0 + for i in range(1, len(path) - 1): + vec1 = np.array(path[i][:3]) - np.array(path[i-1][:3]) + vec2 = np.array(path[i+1][:3]) - np.array(path[i][:3]) + angle = np.arccos(np.dot(vec1, vec2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))) + total_curvature += angle + return total_curvature + + def compute_energy_usage(self, path, velocity): + energy = 0.0 + for i in range(1, len(path)): + distance = np.linalg.norm(np.array(path[i][:3]) - np.array(path[i-1][:3])) + energy += distance * velocity + return energy + + + + + def map_action_to_control(self, action): + # Map the action to a velocity and yaw rate change + velocity_change = [-0.1, 0.0, 0.1][action % 3] # Modulo 3 for velocity options + yaw_change = [-0.1, 0.0, 0.1][(action // 3) % 3] # Integer division for yaw options + return velocity_change, yaw_change + + def train(self, episodes, batch_size=64): + for episode in range(episodes): + state = self.reset() + total_reward = 0 + done = False + + while not done: + action = self.agent.act(state) # Ensure `act` is properly called here + next_state, reward, done = self.step(action) + total_reward += reward + self.agent.remember(state, action, reward, next_state, done) + state = next_state + + # Replay experiences with the set batch size + self.agent.replay(batch_size) + + print(f"Episode: {episode + 1}/{episodes}, Total Reward: {total_reward}") + diff --git a/rrt_algorithms/dqn_algorithm/__pycache__/DQN.cpython-38.pyc b/rrt_algorithms/dqn_algorithm/__pycache__/DQN.cpython-38.pyc new file mode 100644 index 0000000..bf9b8a1 Binary files /dev/null and b/rrt_algorithms/dqn_algorithm/__pycache__/DQN.cpython-38.pyc differ diff --git a/rrt_algorithms/dwa_algorithm/DWA.py b/rrt_algorithms/dwa_algorithm/DWA.py new file mode 100644 index 0000000..be3e894 --- /dev/null +++ b/rrt_algorithms/dwa_algorithm/DWA.py @@ -0,0 +1,102 @@ +import numpy as np + +class DWA: + def __init__(self, config): + self.config = config + + def plan(self, current_position, goal_position, X, obstacles): + # Generate the dynamic window based on current speed and robot limitations + dw = self.calc_dynamic_window(current_position) + + # Evaluate trajectories in the dynamic window + best_u = None + min_cost = float('inf') + best_trajectory = [] + + for v, w in dw: + trajectory = self.generate_trajectory(current_position, v, w) + cost = self.calc_cost(trajectory, goal_position, obstacles) + + if cost < min_cost: + min_cost = cost + best_u = (v, w) + best_trajectory = trajectory + + return best_trajectory + + def calc_dynamic_window(self, current_position): + # Dynamic window calculation + v_min = max(self.config['min_speed'], current_position[3] - self.config['max_accel'] * self.config['dt']) + v_max = min(self.config['max_speed'], current_position[3] + self.config['max_accel'] * self.config['dt']) + w_min = max(-self.config['max_yaw_rate'], current_position[4] - self.config['max_accel'] * self.config['dt']) + w_max = min(self.config['max_yaw_rate'], current_position[4] + self.config['max_accel'] * self.config['dt']) + + return [(v, w) for v in np.arange(v_min, v_max, self.config['v_reso']) for w in np.arange(w_min, w_max, self.config['yaw_rate_reso'])] + + def generate_trajectory(self, current_position, v, w): + # Generate trajectory for a given velocity and yaw rate + trajectory = [current_position] + time = 0 + + while time <= self.config['predict_time']: + next_position = self.motion_model(trajectory[-1], v, w) + trajectory.append(next_position) + time += self.config['dt'] + + return trajectory + + def motion_model(self, state, v, w): + # Update the state [x, y, theta, v, w] given current velocities + x, y, theta, _, _ = state + x += v * np.cos(theta) * self.config['dt'] + y += v * np.sin(theta) * self.config['dt'] + theta += w * self.config['dt'] + return [x, y, theta, v, w] + + def calc_cost(self, trajectory, goal_position, obstacles): + # Cost function combining distance to goal, speed, and distance to obstacles + to_goal_cost = self.config['to_goal_cost_gain'] * self.calc_to_goal_cost(trajectory[-1], goal_position) + speed_cost = self.config['speed_cost_gain'] * (self.config['max_speed'] - trajectory[-1][3]) + obstacle_cost = self.config['obstacle_cost_gain'] * self.calc_obstacle_cost(trajectory, obstacles) + + return to_goal_cost + speed_cost + obstacle_cost + + def calc_to_goal_cost(self, state, goal_position): + # Euclidean distance to the goal + return np.linalg.norm([state[0] - goal_position[0], state[1] - goal_position[1]]) + + def calc_obstacle_cost(self, trajectory, obstacles): + # Calculate the distance to the closest obstacle + min_dist = float('inf') + for point in trajectory: + for obs in obstacles: + dist = np.linalg.norm([point[0] - obs[0], point[1] - obs[1]]) + if dist < min_dist: + min_dist = dist + return min_dist + +# Sample usage of DWA with RRT +if __name__ == "__main__": + 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, + } + + dwa = DWA(dwa_params) + current_position = [0, 0, 0, 0, 0] # Example starting position [x, y, theta, v, w] + goal_position = [10, 10] # Example goal position [x, y] + X = None # The search space; not used in this basic example + obstacles = [(5, 5), (7, 8)] # Example obstacles; replace with actual obstacle positions + + trajectory = dwa.plan(current_position, goal_position, X, obstacles) + print("Best trajectory:", trajectory) + diff --git a/rrt_algorithms/dwa_algorithm/DWA1.py b/rrt_algorithms/dwa_algorithm/DWA1.py new file mode 100644 index 0000000..8ddc95a --- /dev/null +++ b/rrt_algorithms/dwa_algorithm/DWA1.py @@ -0,0 +1,108 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +import numpy as np + +class DWA: + def __init__(self, config): + self.config = config + + def plan(self, start_point, end_point, X, obstacles): + # Generate dynamic window and evaluate possible trajectories + dw = self.calc_dynamic_window(start_point) + best_trajectory = None + min_cost = float('inf') + + for v in np.arange(dw[0], dw[1], self.config['v_reso']): + for y in np.arange(dw[2], dw[3], self.config['yaw_rate_reso']): + trajectory = self.generate_trajectory(start_point, v, y) + if X.collision_free(trajectory[0], trajectory[-1], self.config['dt']): + cost = self.calc_cost(trajectory, end_point, obstacles) + if cost < min_cost: + min_cost = cost + best_trajectory = trajectory + + return best_trajectory + + def calc_dynamic_window(self, current_position): + # Dynamic window calculation, returning individual scalar values instead of a list of tuples + v_min = max(self.config['min_speed'], current_position[3] - self.config['max_accel'] * self.config['dt']) + v_max = min(self.config['max_speed'], current_position[3] + self.config['max_accel'] * self.config['dt']) + w_min = max(-self.config['max_yaw_rate'], current_position[4] - self.config['max_accel'] * self.config['dt']) + w_max = min(self.config['max_yaw_rate'], current_position[4] + self.config['max_accel'] * self.config['dt']) + + return [v_min, v_max, w_min, w_max] + + def generate_trajectory(self, current_position, v, w): + # Generate trajectory for a given velocity and yaw rate + trajectory = [current_position] + time = 0 + + while time <= self.config['predict_time']: + next_position = self.motion_model(trajectory[-1], v, w) + trajectory.append(next_position) + time += self.config['dt'] + + return trajectory + + def motion_model(self, state, v, w): + # Update the state [x, y, theta, v, w] given current velocities + x, y, theta, _, _ = state + x += v * np.cos(theta) * self.config['dt'] + y += v * np.sin(theta) * self.config['dt'] + theta += w * self.config['dt'] + return [x, y, theta, v, w] + + def calc_cost(self, trajectory, goal_position, obstacles): + # Cost function combining distance to goal, speed, and distance to obstacles + to_goal_cost = self.config['to_goal_cost_gain'] * self.calc_to_goal_cost(trajectory[-1], goal_position) + speed_cost = self.config['speed_cost_gain'] * (self.config['max_speed'] - trajectory[-1][3]) + obstacle_cost = self.config['obstacle_cost_gain'] * self.calc_obstacle_cost(trajectory, obstacles) + + return to_goal_cost + speed_cost + obstacle_cost + + def calc_to_goal_cost(self, state, goal_position): + # Euclidean distance to the goal + return np.linalg.norm([state[0] - goal_position[0], state[1] - goal_position[1]]) + + def calc_obstacle_cost(self, trajectory, obstacles): + # Calculate the distance to the closest obstacle + min_dist = float('inf') + for point in trajectory: + for obs in obstacles: + dist = np.linalg.norm([point[0] - obs[0], point[1] - obs[1]]) + if dist < min_dist: + min_dist = dist + return min_dist + +# Sample usage of DWA with RRT +""" +if __name__ == "__main__": + 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, + } + + dwa = DWA(dwa_params) + current_position = [0, 0, 0, 0, 0] # Example starting position [x, y, theta, v, w] + goal_position = [10, 10] # Example goal position [x, y] + X = None # The search space; not used in this basic example + obstacles = [(5, 5), (7, 8)] # Example obstacles; replace with actual obstacle positions + + trajectory = dwa.plan(current_position, goal_position, X, obstacles) + print("Best trajectory:", trajectory) +""" diff --git a/rrt_algorithms/dwa_algorithm/DWA2.py b/rrt_algorithms/dwa_algorithm/DWA2.py new file mode 100644 index 0000000..5a6e071 --- /dev/null +++ b/rrt_algorithms/dwa_algorithm/DWA2.py @@ -0,0 +1,164 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +import sys +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl + +# Update the DWA plan method to use the fuzzy controller +class FuzzyController: + def __init__(self): + # Define fuzzy variables + self.distance_to_goal = ctrl.Antecedent(np.arange(0, 101, 1), 'distance_to_goal') + self.distance_to_obstacle = ctrl.Antecedent(np.arange(0, 101, 1), 'distance_to_obstacle') + + self.to_goal_gain = ctrl.Consequent(np.arange(0, 11, 1), 'to_goal_gain') + self.speed_gain = ctrl.Consequent(np.arange(0, 11, 1), 'speed_gain') + self.obstacle_gain = ctrl.Consequent(np.arange(0, 11, 1), 'obstacle_gain') + + # Auto-membership function with 3 levels: 'poor', 'average', 'good' + self.distance_to_goal.automf(3) + self.distance_to_obstacle.automf(3) + + self.to_goal_gain.automf(3) + self.speed_gain.automf(3) + self.obstacle_gain.automf(3) + + # Define fuzzy rules to cover all cases + rule1 = ctrl.Rule(self.distance_to_goal['poor'] & self.distance_to_obstacle['good'], + (self.to_goal_gain['good'], self.speed_gain['good'], self.obstacle_gain['poor'])) + rule2 = ctrl.Rule(self.distance_to_goal['average'] & self.distance_to_obstacle['average'], + (self.to_goal_gain['average'], self.speed_gain['average'], self.obstacle_gain['average'])) + rule3 = ctrl.Rule(self.distance_to_goal['good'] & self.distance_to_obstacle['poor'], + (self.to_goal_gain['poor'], self.speed_gain['poor'], self.obstacle_gain['good'])) + + # Create control system + self.control_system = ctrl.ControlSystem([rule1, rule2, rule3]) + self.simulation = ctrl.ControlSystemSimulation(self.control_system) + + def compute_gains(self, dist_to_goal, dist_to_obstacle): + # Set inputs + self.simulation.input['distance_to_goal'] = dist_to_goal + self.simulation.input['distance_to_obstacle'] = dist_to_obstacle + + # Perform fuzzy computation + self.simulation.compute() + + # Ensure the fuzzy system has outputted the necessary gains + return (self.simulation.output.get('to_goal_gain', 0), + self.simulation.output.get('speed_gain', 0), + self.simulation.output.get('obstacle_gain', 0)) + +class DWA: + def __init__(self, config): + self.config = config + self.fuzzy_controller = FuzzyController() + + def plan(self, start_point, end_point, X, obstacles): + # Compute distance to goal and nearest obstacle + dist_to_goal = np.linalg.norm(np.array(start_point[:2]) - np.array(end_point[:2])) + dist_to_obstacle = min([np.linalg.norm(np.array(start_point[:2]) - np.array(obs[:2])) - obs[4] for obs in obstacles]) + + # Get fuzzy-adjusted weights + to_goal_gain, speed_gain, obstacle_gain = self.fuzzy_controller.compute_gains(dist_to_goal, dist_to_obstacle) + # Generate dynamic window and evaluate possible trajectories + dw = self.calc_dynamic_window(start_point) + best_trajectory = None + min_cost = float('inf') + + for v in np.arange(dw[0], dw[1], self.config['v_reso']): + for y in np.arange(dw[2], dw[3], self.config['yaw_rate_reso']): + trajectory = self.generate_trajectory(start_point, v, y) + if X.collision_free(trajectory[0], trajectory[-1], self.config['dt']): + cost = (to_goal_gain * self.calc_to_goal_cost(trajectory[-1], end_point) + + speed_gain * (self.config['max_speed'] - trajectory[-1][3]) + + obstacle_gain * self.calc_obstacle_cost(trajectory, obstacles)) + if cost < min_cost: + min_cost = cost + best_trajectory = trajectory + + return best_trajectory + + def calc_dynamic_window(self, current_position): + # Dynamic window calculation, returning individual scalar values instead of a list of tuples + v_min = max(self.config['min_speed'], current_position[3] - self.config['max_accel'] * self.config['dt']) + v_max = min(self.config['max_speed'], current_position[3] + self.config['max_accel'] * self.config['dt']) + w_min = max(-self.config['max_yaw_rate'], current_position[4] - self.config['max_accel'] * self.config['dt']) + w_max = min(self.config['max_yaw_rate'], current_position[4] + self.config['max_accel'] * self.config['dt']) + + return [v_min, v_max, w_min, w_max] + + def generate_trajectory(self, current_position, v, w): + # Generate trajectory for a given velocity and yaw rate + trajectory = [current_position] + time = 0 + + while time <= self.config['predict_time']: + next_position = self.motion_model(trajectory[-1], v, w) + trajectory.append(next_position) + time += self.config['dt'] + + return trajectory + + def motion_model(self, state, v, w): + # Update the state [x, y, theta, v, w] given current velocities + x, y, theta, _, _ = state + x += v * np.cos(theta) * self.config['dt'] + y += v * np.sin(theta) * self.config['dt'] + theta += w * self.config['dt'] + return [x, y, theta, v, w] + + def calc_cost(self, trajectory, goal_position, obstacles): + # Cost function combining distance to goal, speed, and distance to obstacles + to_goal_cost = self.config['to_goal_cost_gain'] * self.calc_to_goal_cost(trajectory[-1], goal_position) + speed_cost = self.config['speed_cost_gain'] * (self.config['max_speed'] - trajectory[-1][3]) + obstacle_cost = self.config['obstacle_cost_gain'] * self.calc_obstacle_cost(trajectory, obstacles) + + return to_goal_cost + speed_cost + obstacle_cost + + def calc_to_goal_cost(self, state, goal_position): + # Euclidean distance to the goal + return np.linalg.norm([state[0] - goal_position[0], state[1] - goal_position[1]]) + + def calc_obstacle_cost(self, trajectory, obstacles): + # Calculate the distance to the closest obstacle + min_dist = float('inf') + for point in trajectory: + for obs in obstacles: + dist = np.linalg.norm([point[0] - obs[0], point[1] - obs[1]]) + if dist < min_dist: + min_dist = dist + return min_dist + +# Sample usage of DWA with RRT +""" +if __name__ == "__main__": + 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, + } + + dwa = DWA(dwa_params) + current_position = [0, 0, 0, 0, 0] # Example starting position [x, y, theta, v, w] + goal_position = [10, 10] # Example goal position [x, y] + X = None # The search space; not used in this basic example + obstacles = [(5, 5), (7, 8)] # Example obstacles; replace with actual obstacle positions + + trajectory = dwa.plan(current_position, goal_position, X, obstacles) + print("Best trajectory:", trajectory) +""" diff --git a/rrt_algorithms/dwa_algorithm/DWA3.py b/rrt_algorithms/dwa_algorithm/DWA3.py new file mode 100644 index 0000000..2af0e0a --- /dev/null +++ b/rrt_algorithms/dwa_algorithm/DWA3.py @@ -0,0 +1,164 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +import sys +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl + +# Update the DWA plan method to use the fuzzy controller +class FuzzyController: + def __init__(self): + # Define fuzzy variables + self.distance_to_goal = ctrl.Antecedent(np.arange(0, 101, 1), 'distance_to_goal') + self.distance_to_obstacle = ctrl.Antecedent(np.arange(0, 101, 1), 'distance_to_obstacle') + + self.to_goal_gain = ctrl.Consequent(np.arange(0, 11, 1), 'to_goal_gain') + self.speed_gain = ctrl.Consequent(np.arange(0, 11, 1), 'speed_gain') + self.obstacle_gain = ctrl.Consequent(np.arange(0, 11, 1), 'obstacle_gain') + + # Auto-membership function with 3 levels: 'poor', 'average', 'good' + self.distance_to_goal.automf(3) + self.distance_to_obstacle.automf(3) + + self.to_goal_gain.automf(3) + self.speed_gain.automf(3) + self.obstacle_gain.automf(3) + + # Define fuzzy rules to cover all cases + rule1 = ctrl.Rule(self.distance_to_goal['poor'] & self.distance_to_obstacle['good'], + (self.to_goal_gain['good'], self.speed_gain['good'], self.obstacle_gain['poor'])) + rule2 = ctrl.Rule(self.distance_to_goal['average'] & self.distance_to_obstacle['average'], + (self.to_goal_gain['average'], self.speed_gain['average'], self.obstacle_gain['average'])) + rule3 = ctrl.Rule(self.distance_to_goal['good'] & self.distance_to_obstacle['poor'], + (self.to_goal_gain['poor'], self.speed_gain['poor'], self.obstacle_gain['good'])) + + # Create control system + self.control_system = ctrl.ControlSystem([rule1, rule2, rule3]) + self.simulation = ctrl.ControlSystemSimulation(self.control_system) + + def compute_gains(self, dist_to_goal, dist_to_obstacle): + # Set inputs + self.simulation.input['distance_to_goal'] = dist_to_goal + self.simulation.input['distance_to_obstacle'] = dist_to_obstacle + + # Perform fuzzy computation + self.simulation.compute() + + # Ensure the fuzzy system has outputted the necessary gains + return (self.simulation.output.get('to_goal_gain', 0), + self.simulation.output.get('speed_gain', 0), + self.simulation.output.get('obstacle_gain', 0)) + +class DWA: + def __init__(self, config): + self.config = config + self.fuzzy_controller = FuzzyController() + + def plan(self, start_point, end_point, X, obstacles): + # Compute distance to goal and nearest obstacle + dist_to_goal = np.linalg.norm(np.array(start_point[:2]) - np.array(end_point[:2])) + dist_to_obstacle = min([np.linalg.norm(np.array(start_point[:2]) - np.array(obs[:2])) - obs[4] for obs in obstacles]) + + # Get fuzzy-adjusted weights + to_goal_gain, speed_gain, obstacle_gain = self.fuzzy_controller.compute_gains(dist_to_goal, dist_to_obstacle) + # Generate dynamic window and evaluate possible trajectories + dw = self.calc_dynamic_window(start_point) + best_trajectory = None + min_cost = float('inf') + + for v in np.arange(dw[0], dw[1], self.config['v_reso']): + for y in np.arange(dw[2], dw[3], self.config['yaw_rate_reso']): + trajectory = self.generate_trajectory(start_point, v, y) + if X.collision_free(trajectory[0][:5], trajectory[-1][:5], self.config['dt']): + cost = (to_goal_gain * self.calc_to_goal_cost(trajectory[-1], end_point) + + speed_gain * (self.config['max_speed'] - trajectory[-1][3]) + + obstacle_gain * self.calc_obstacle_cost(trajectory, obstacles)) + if cost < min_cost: + min_cost = cost + best_trajectory = trajectory + + return best_trajectory + + def calc_dynamic_window(self, current_position): + # Dynamic window calculation, returning individual scalar values instead of a list of tuples + v_min = max(self.config['min_speed'], current_position[3] - self.config['max_accel'] * self.config['dt']) + v_max = min(self.config['max_speed'], current_position[3] + self.config['max_accel'] * self.config['dt']) + w_min = max(-self.config['max_yaw_rate'], current_position[4] - self.config['max_accel'] * self.config['dt']) + w_max = min(self.config['max_yaw_rate'], current_position[4] + self.config['max_accel'] * self.config['dt']) + + return [v_min, v_max, w_min, w_max] + + def generate_trajectory(self, current_position, v, w): + # Generate trajectory for a given velocity and yaw rate + trajectory = [current_position] + time = 0 + + while time <= self.config['predict_time']: + next_position = self.motion_model(trajectory[-1], v, w) + trajectory.append(next_position) + time += self.config['dt'] + + return trajectory + + def motion_model(self, state, v, w): + # Update the state [x, y, theta, v, w] given current velocities + x, y, theta, _, _ = state[:5] # Ensure only the first 5 elements are considered + x += v * np.cos(theta) * self.config['dt'] + y += v * np.sin(theta) * self.config['dt'] + theta += w * self.config['dt'] + return [x, y, theta, v, w] + + def calc_cost(self, trajectory, goal_position, obstacles): + # Cost function combining distance to goal, speed, and distance to obstacles + to_goal_cost = self.config['to_goal_cost_gain'] * self.calc_to_goal_cost(trajectory[-1], goal_position) + speed_cost = self.config['speed_cost_gain'] * (self.config['max_speed'] - trajectory[-1][3]) + obstacle_cost = self.config['obstacle_cost_gain'] * self.calc_obstacle_cost(trajectory, obstacles) + + return to_goal_cost + speed_cost + obstacle_cost + + def calc_to_goal_cost(self, state, goal_position): + # Euclidean distance to the goal + return np.linalg.norm([state[0] - goal_position[0], state[1] - goal_position[1]]) + + def calc_obstacle_cost(self, trajectory, obstacles): + # Calculate the distance to the closest obstacle + min_dist = float('inf') + for point in trajectory: + for obs in obstacles: + dist = np.linalg.norm([point[0] - obs[0], point[1] - obs[1]]) + if dist < min_dist: + min_dist = dist + return min_dist + +# Sample usage of DWA with RRT +""" +if __name__ == "__main__": + 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, + } + + dwa = DWA(dwa_params) + current_position = [0, 0, 0, 0, 0] # Example starting position [x, y, theta, v, w] + goal_position = [10, 10] # Example goal position [x, y] + X = None # The search space; not used in this basic example + obstacles = [(5, 5), (7, 8)] # Example obstacles; replace with actual obstacle positions + + trajectory = dwa.plan(current_position, goal_position, X, obstacles) + print("Best trajectory:", trajectory) +""" diff --git a/rrt_algorithms/dwa_algorithm/__pycache__/DWA.cpython-38.pyc b/rrt_algorithms/dwa_algorithm/__pycache__/DWA.cpython-38.pyc new file mode 100644 index 0000000..66b5a32 Binary files /dev/null and b/rrt_algorithms/dwa_algorithm/__pycache__/DWA.cpython-38.pyc differ diff --git a/rrt_algorithms/dwa_algorithm/__pycache__/DWA1.cpython-38.pyc b/rrt_algorithms/dwa_algorithm/__pycache__/DWA1.cpython-38.pyc new file mode 100644 index 0000000..0a9ef0b Binary files /dev/null and b/rrt_algorithms/dwa_algorithm/__pycache__/DWA1.cpython-38.pyc differ diff --git a/rrt_algorithms/dwa_algorithm/__pycache__/DWA2.cpython-38.pyc b/rrt_algorithms/dwa_algorithm/__pycache__/DWA2.cpython-38.pyc new file mode 100644 index 0000000..7578e0d Binary files /dev/null and b/rrt_algorithms/dwa_algorithm/__pycache__/DWA2.cpython-38.pyc differ diff --git a/rrt_algorithms/dwa_algorithm/__pycache__/DWA3.cpython-38.pyc b/rrt_algorithms/dwa_algorithm/__pycache__/DWA3.cpython-38.pyc new file mode 100644 index 0000000..88aaa50 Binary files /dev/null and b/rrt_algorithms/dwa_algorithm/__pycache__/DWA3.cpython-38.pyc differ diff --git a/rrt_algorithms/rrt/rrt.py b/rrt_algorithms/rrt/rrt.py index 34fa05a..c295202 100644 --- a/rrt_algorithms/rrt/rrt.py +++ b/rrt_algorithms/rrt/rrt.py @@ -25,7 +25,9 @@ def rrt_search(self): self.add_edge(0, self.x_init, None) while True: + print(f"q: {self.q}") x_new, x_nearest = self.new_and_near(0, self.q) + print(f"x_new: {x_new}, x_nearest: {x_nearest}") if x_new is None: continue @@ -36,3 +38,4 @@ def rrt_search(self): solution = self.check_solution() if solution[0]: return solution[1] + diff --git a/rrt_algorithms/rrt/rrt_base 1.py b/rrt_algorithms/rrt/rrt_base 1.py new file mode 100644 index 0000000..eaa19e9 --- /dev/null +++ b/rrt_algorithms/rrt/rrt_base 1.py @@ -0,0 +1,186 @@ +import random + +import numpy as np +import sys +sys.path.append('/home/dell/rrt-algorithms') + + +from rrt_algorithms.search_space.search_space1 import SearchSpace +from rrt_algorithms.rrt.tree import Tree +from rrt_algorithms.utilities.geometry import steer + + +class RRTBase(object): + def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01): + """ + Template RRT planner + :param X: Search Space + :param q: length of edges added to tree + :param x_init: tuple, initial location + :param x_goal: tuple, goal location + :param max_samples: max number of samples to take + :param r: resolution of points to sample along edge when checking for collisions + :param prc: probability of checking whether there is a solution + """ + self.X = X + self.samples_taken = 0 + self.max_samples = max_samples + self.q = q + self.r = r + self.prc = prc + self.x_init = x_init + self.x_goal = x_goal + self.trees = [] # list of all trees + self.add_tree() # add initial tree + + def add_tree(self): + """ + Create an empty tree and add to trees + """ + self.trees.append(Tree(self.X)) + + def add_vertex(self, tree, v): + """ + Add vertex to corresponding tree + :param tree: int, tree to which to add vertex + :param v: tuple, vertex to add + """ + self.trees[tree].V.insert(0, v + v, v) + self.trees[tree].V_count += 1 # increment number of vertices in tree + self.samples_taken += 1 # increment number of samples taken + + def add_edge(self, tree, child, parent): + """ + Add edge to corresponding tree + :param tree: int, tree to which to add vertex + :param child: tuple, child vertex + :param parent: tuple, parent vertex + """ + self.trees[tree].E[child] = parent + + def nearby(self, tree, x, n): + """ + Return nearby vertices + :param tree: int, tree being searched + :param x: tuple, vertex around which searching + :param n: int, max number of neighbors to return + :return: list of nearby vertices + """ + return self.trees[tree].V.nearest(x, num_results=n, objects="raw") + + def get_nearest(self, tree, x): + """ + Return vertex nearest to x + :param tree: int, tree being searched + :param x: tuple, vertex around which searching + :return: tuple, nearest vertex to x + """ + return next(self.nearby(tree, x, 1)) + + def new_and_near(self, tree, q): + """ + Return a new steered vertex and the vertex in tree that is nearest + :param tree: int, tree being searched + :param q: length of edge when steering + :return: vertex, new steered vertex, vertex, nearest vertex in tree to new vertex + """ + x_rand = self.X.sample_free() + print(f"Random sample: {x_rand}") + + x_nearest = self.get_nearest(tree, x_rand) + print(f"Nearest point: {x_nearest}") + x_new = self.bound_point(steer(x_nearest, x_rand, q)) + print(f"Steered new point: {x_new}") + # check if new point is in X_free and not already in V + if not self.trees[0].V.count(x_new) == 0 or not self.X.obstacle_free(x_new): + print(f"New point {x_new} is invalid or in obstacle.") + return None, None + self.samples_taken += 1 + return x_new, x_nearest + + def connect_to_point(self, tree, x_a, x_b): + """ + Connect vertex x_a in tree to vertex x_b + :param tree: int, tree to which to add edge + :param x_a: tuple, vertex + :param x_b: tuple, vertex + :return: bool, True if able to add edge, False if prohibited by an obstacle + """ + if self.trees[tree].V.count(x_b) == 0 and self.X.collision_free(x_a, x_b, self.r): + self.add_vertex(tree, x_b) + self.add_edge(tree, x_b, x_a) + return True + return False + + def can_connect_to_goal(self, tree): + """ + Check if the goal can be connected to the graph + :param tree: rtree of all Vertices + :return: True if can be added, False otherwise + """ + x_nearest = self.get_nearest(tree, self.x_goal) + if self.x_goal in self.trees[tree].E and x_nearest in self.trees[tree].E[self.x_goal]: + # tree is already connected to goal using nearest vertex + return True + # check if obstacle-free + if self.X.collision_free(x_nearest, self.x_goal, self.r): + return True + return False + + def get_path(self): + """ + Return path through tree from start to goal + :return: path if possible, None otherwise + """ + if self.can_connect_to_goal(0): + print("Can connect to goal") + self.connect_to_goal(0) + return self.reconstruct_path(0, self.x_init, self.x_goal) + print("Could not connect to goal") + return None + + def connect_to_goal(self, tree): + """ + Connect x_goal to graph + (does not check if this should be possible, for that use: can_connect_to_goal) + :param tree: rtree of all Vertices + """ + x_nearest = self.get_nearest(tree, self.x_goal) + self.trees[tree].E[self.x_goal] = x_nearest + + def reconstruct_path(self, tree, x_init, x_goal): + """ + Reconstruct path from start to goal + :param tree: int, tree in which to find path + :param x_init: tuple, starting vertex + :param x_goal: tuple, ending vertex + :return: sequence of vertices from start to goal + """ + path = [x_goal] + current = x_goal + if x_init == x_goal: + return path + while not self.trees[tree].E[current] == x_init: + path.append(self.trees[tree].E[current]) + current = self.trees[tree].E[current] + path.append(x_init) + path.reverse() + return path + + def check_solution(self): + # probabilistically check if solution found + if self.prc and random.random() < self.prc: + print("Checking if can connect to goal at", str(self.samples_taken), "samples") + path = self.get_path() + if path is not None: + return True, path + # check if can connect to goal after generating max_samples + if self.samples_taken >= self.max_samples: + return True, self.get_path() + return False, None + + def bound_point(self, point): + # if point is out-of-bounds, set to bound + point = np.maximum(point, self.X.dimension_lengths[:, 0]) + point = np.minimum(point, self.X.dimension_lengths[:, 1]) + return tuple(point) diff --git a/rrt_algorithms/rrt/rrt_base.py b/rrt_algorithms/rrt/rrt_base.py index 8d147fa..e19d617 100644 --- a/rrt_algorithms/rrt/rrt_base.py +++ b/rrt_algorithms/rrt/rrt_base.py @@ -1,7 +1,11 @@ import random import numpy as np +import sys +sys.path.append('/home/dell/rrt-algorithms') + +from rrt_algorithms.search_space.search_space import SearchSpace from rrt_algorithms.rrt.tree import Tree from rrt_algorithms.utilities.geometry import steer @@ -81,10 +85,15 @@ def new_and_near(self, tree, q): :return: vertex, new steered vertex, vertex, nearest vertex in tree to new vertex """ x_rand = self.X.sample_free() + print(f"Random sample: {x_rand}") + x_nearest = self.get_nearest(tree, x_rand) + print(f"Nearest point: {x_nearest}") x_new = self.bound_point(steer(x_nearest, x_rand, q)) + print(f"Steered new point: {x_new}") # check if new point is in X_free and not already in V if not self.trees[0].V.count(x_new) == 0 or not self.X.obstacle_free(x_new): + print(f"New point {x_new} is invalid or in obstacle.") return None, None self.samples_taken += 1 return x_new, x_nearest diff --git a/rrt_algorithms/search_space/search_space.py b/rrt_algorithms/search_space/search_space.py index 16c3d42..10cd2ee 100644 --- a/rrt_algorithms/search_space/search_space.py +++ b/rrt_algorithms/search_space/search_space.py @@ -7,71 +7,75 @@ from rrt_algorithms.utilities.geometry import es_points_along_line from rrt_algorithms.utilities.obstacle_generation import obstacle_generator +class SearchSpace: + def __init__(self, dimension_lengths, obstacles=None): + self.dimensions = len(dimension_lengths) + self.dimension_lengths = dimension_lengths + self.obstacles = obstacles if obstacles is not None else [] -class SearchSpace(object): - def __init__(self, dimension_lengths, O=None): + def obstacle_free(self, point): """ - Initialize Search Space - :param dimension_lengths: range of each dimension - :param O: list of obstacles + Check if the point is free from obstacles. + :param point: Tuple (x, y, z) + :return: True if the point is free of obstacles, False otherwise. """ - # sanity check - if len(dimension_lengths) < 2: - raise Exception("Must have at least 2 dimensions") - self.dimensions = len(dimension_lengths) # number of dimensions - # sanity checks - if any(len(i) != 2 for i in dimension_lengths): - raise Exception("Dimensions can only have a start and end") - if any(i[0] >= i[1] for i in dimension_lengths): - raise Exception("Dimension start must be less than dimension end") - self.dimension_lengths = dimension_lengths # length of each dimension - p = index.Property() - p.dimension = self.dimensions - if O is None: - self.obs = index.Index(interleaved=True, properties=p) - else: - # r-tree representation of obstacles - # sanity check - if any(len(o) / 2 != len(dimension_lengths) for o in O): - raise Exception("Obstacle has incorrect dimension definition") - if any(o[i] >= o[int(i + len(o) / 2)] for o in O for i in range(int(len(o) / 2))): - raise Exception("Obstacle start must be less than obstacle end") - self.obs = index.Index(obstacle_generator(O), interleaved=True, properties=p) + for obstacle in self.obstacles: + x_center, y_center, z_min, z_max, radius = obstacle + + # Check if the point is within the cylinder's height bounds + if z_min <= point[2] <= z_max: + # Check if the point is within the radius of the cylinder + distance_to_axis = np.sqrt((point[0] - x_center) ** 2 + (point[1] - y_center) ** 2) + if distance_to_axis <= radius: + return False # The point is inside the obstacle + return True # The point is free of obstacles - def obstacle_free(self, x): + def collision_free(self, x_a, x_b, r): """ - Check if a location resides inside of an obstacle - :param x: location to check - :return: True if not inside an obstacle, False otherwise + Check if a line segment between x_a and x_b is free from obstacles. + :param x_a: Starting point of the line segment. + :param x_b: Ending point of the line segment. + :param r: Resolution for sampling along the line. + :return: True if the line segment does not intersect any obstacles, False otherwise. """ - return self.obs.count(x) == 0 + # Generate points along the line segment between x_a and x_b + points = self.es_points_along_line(x_a, x_b, r) + for point in points: + if not self.obstacle_free(point): + return False # Collision detected + return True # No collision - def sample_free(self): + + + def es_points_along_line(self, x_a, x_b, r): """ - Sample a location within X_free - :return: random location within X_free + Generate points along a line segment between x_a and x_b. + :param x_a: Starting point of the line segment. + :param x_b: Ending point of the line segment. + :param r: Resolution for sampling points. + :return: List of points along the line segment. """ - while True: # sample until not inside of an obstacle - x = self.sample() - if self.obstacle_free(x): - return x + distance = np.linalg.norm(np.array(x_b) - np.array(x_a)) + num_points = int(distance / r) + 1 + return [tuple(np.array(x_a) + t * (np.array(x_b) - np.array(x_a))) for t in np.linspace(0, 1, num_points)] + - def collision_free(self, start, end, r): + + def sample(self): """ - Check if a line segment intersects an obstacle - :param start: starting point of line - :param end: ending point of line - :param r: resolution of points to sample along edge when checking for collisions - :return: True if line segment does not intersect an obstacle, False otherwise + Return a random location within the search space dimensions. + :return: A tuple representing a random location within the search space. """ - points = es_points_along_line(start, end, r) - coll_free = all(map(self.obstacle_free, points)) - return coll_free + return tuple(np.random.uniform(low, high) for low, high in self.dimension_lengths) - def sample(self): + def sample_free(self): """ - Return a random location within X - :return: random location within X (not necessarily X_free) + Sample a random location within the search space that is free from obstacles. + :return: A tuple representing a random location within the search space that is obstacle-free. """ - x = np.random.uniform(self.dimension_lengths[:, 0], self.dimension_lengths[:, 1]) - return tuple(x) + while True: + point = self.sample() + if self.obstacle_free(point): + return point + + diff --git a/rrt_algorithms/search_space/search_space0.py b/rrt_algorithms/search_space/search_space0.py new file mode 100644 index 0000000..16c3d42 --- /dev/null +++ b/rrt_algorithms/search_space/search_space0.py @@ -0,0 +1,77 @@ +# 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 +from rtree import index + +from rrt_algorithms.utilities.geometry import es_points_along_line +from rrt_algorithms.utilities.obstacle_generation import obstacle_generator + + +class SearchSpace(object): + def __init__(self, dimension_lengths, O=None): + """ + Initialize Search Space + :param dimension_lengths: range of each dimension + :param O: list of obstacles + """ + # sanity check + if len(dimension_lengths) < 2: + raise Exception("Must have at least 2 dimensions") + self.dimensions = len(dimension_lengths) # number of dimensions + # sanity checks + if any(len(i) != 2 for i in dimension_lengths): + raise Exception("Dimensions can only have a start and end") + if any(i[0] >= i[1] for i in dimension_lengths): + raise Exception("Dimension start must be less than dimension end") + self.dimension_lengths = dimension_lengths # length of each dimension + p = index.Property() + p.dimension = self.dimensions + if O is None: + self.obs = index.Index(interleaved=True, properties=p) + else: + # r-tree representation of obstacles + # sanity check + if any(len(o) / 2 != len(dimension_lengths) for o in O): + raise Exception("Obstacle has incorrect dimension definition") + if any(o[i] >= o[int(i + len(o) / 2)] for o in O for i in range(int(len(o) / 2))): + raise Exception("Obstacle start must be less than obstacle end") + self.obs = index.Index(obstacle_generator(O), interleaved=True, properties=p) + + def obstacle_free(self, x): + """ + Check if a location resides inside of an obstacle + :param x: location to check + :return: True if not inside an obstacle, False otherwise + """ + return self.obs.count(x) == 0 + + def sample_free(self): + """ + Sample a location within X_free + :return: random location within X_free + """ + while True: # sample until not inside of an obstacle + x = self.sample() + if self.obstacle_free(x): + return x + + def collision_free(self, start, end, r): + """ + Check if a line segment intersects an obstacle + :param start: starting point of line + :param end: ending point of line + :param r: resolution of points to sample along edge when checking for collisions + :return: True if line segment does not intersect an obstacle, False otherwise + """ + points = es_points_along_line(start, end, r) + coll_free = all(map(self.obstacle_free, points)) + return coll_free + + def sample(self): + """ + Return a random location within X + :return: random location within X (not necessarily X_free) + """ + x = np.random.uniform(self.dimension_lengths[:, 0], self.dimension_lengths[:, 1]) + return tuple(x) diff --git a/rrt_algorithms/search_space/search_space1.py b/rrt_algorithms/search_space/search_space1.py new file mode 100644 index 0000000..fc4194d --- /dev/null +++ b/rrt_algorithms/search_space/search_space1.py @@ -0,0 +1,116 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +import numpy as np +from rtree import index + +from rrt_algorithms.utilities.geometry import es_points_along_line +from rrt_algorithms.utilities.obstacle_generation1 import obstacle_generator + +from rtree import index + +class SearchSpace: + def __init__(self, dimension_lengths, obstacles=None): + self.dimension_lengths = dimension_lengths + self.obstacles = obstacles if obstacles is not None else [] + self.dimensions = len(dimension_lengths) # Add this line to define the number of dimensions + self.build_bvh() + self.build_obs_index() + + def build_bvh(self): + # Create a BVH tree for fast collision detection + p = index.Property() + p.dimension = 3 # Set dimension to 3 for 3D bounding boxes + self.bvh = index.Index(properties=p) + for i, obstacle in enumerate(self.obstacles): + aabb = self.compute_aabb(obstacle) + self.bvh.insert(i, aabb) + + def build_obs_index(self): + # Create an R-tree index for obstacles + p = index.Property() + p.dimension = 3 # Set dimension to 3 for 3D obstacles + self.obs = index.Index(properties=p) + for i, obstacle in enumerate(self.obstacles): + aabb = self.compute_aabb(obstacle) + self.obs.insert(i, aabb) + + def compute_aabb(self, obstacle): + # Compute the Axis-Aligned Bounding Box (AABB) for the obstacle + x_center, y_center, z_min, z_max, radius = obstacle + x_min = x_center - radius + x_max = x_center + radius + y_min = y_center - radius + y_max = y_center + radius + z_min = z_min # Lower z-bound + z_max = z_max # Upper z-bound + return (x_min, y_min, z_min, x_max, y_max, z_max) + + def sample(self): + """ + Return a random location within the search space dimensions. + :return: A tuple representing a random location within the search space. + """ + return tuple(np.random.uniform(low, high) for low, high in self.dimension_lengths) + def collision_free(self, start, end, r): + direction = np.array(end) - np.array(start) + length = np.linalg.norm(direction) + direction = direction / length + steps = int(length / r) + for i in range(steps + 1): + point = np.array(start) + i * r * direction + if not self.obstacle_free(point): + return False + return True + + + + def is_point_in_obstacle(self, point, obstacle): + # Detailed check if the point is inside the actual obstacle + x_center, y_center, z_min, z_max, radius = obstacle + dist = np.linalg.norm(np.array(point[:2]) - np.array([x_center, y_center])) + return dist <= radius and z_min <= point[2] <= z_max + + + + def es_points_along_line(self, x_a, x_b, r): + """ + Generate points along a line segment between x_a and x_b. + :param x_a: Starting point of the line segment. + :param x_b: Ending point of the line segment. + :param r: Resolution for sampling points. + :return: List of points along the line segment. + """ + distance = np.linalg.norm(np.array(x_b) - np.array(x_a)) + num_points = int(distance / r) + 1 + return [tuple(np.array(x_a) + t * (np.array(x_b) - np.array(x_a))) for t in np.linspace(0, 1, num_points)] + + + + + def obstacle_free(self, point): + """ + Check if a point is free from obstacles. + :param point: A tuple representing a point in the search space. + :return: True if the point is free from obstacles, False otherwise. + """ + nearby_obstacles = list(self.bvh.intersection((point[0], point[1], point[2], point[0], point[1], point[2]))) + for obs_idx in nearby_obstacles: + if self.is_point_in_obstacle(point, self.obstacles[obs_idx]): + return False + return True + + def sample_free(self): + """ + Sample a random location within the search space that is free from obstacles. + :return: A tuple representing a random location within the search space that is obstacle-free. + """ + while True: + point = self.sample() + if self.obstacle_free(point): + return point diff --git a/rrt_algorithms/search_space/search_space2.py b/rrt_algorithms/search_space/search_space2.py new file mode 100644 index 0000000..3744ebf --- /dev/null +++ b/rrt_algorithms/search_space/search_space2.py @@ -0,0 +1,145 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#The drone is modeled as a sphere with a 10 cm radius (0.1 meters). This radius is added to the radius of each obstacle during collision detection. +#Expanded Obstacles: By expanding each obstacle's radius during collision checks, we ensure that the drone is considered in the obstacle avoidance logic, avoiding potential collisions. +#all_obstacles as a combination of the obstacles list (static obstacles) and Obstacles (randomly generated obstacles). +import numpy as np +from rtree import index + +from rrt_algorithms.utilities.geometry import es_points_along_line +from rrt_algorithms.utilities.obstacle_generation2 import obstacle_generator + +from rtree import index + +class SearchSpace: + def __init__(self, dimension_lengths, obstacles=None): + self.dimension_lengths = dimension_lengths + self.obstacles = obstacles if obstacles is not None else [] + self.dimensions = len(dimension_lengths) + self.drone_radius = 0.10 # Drone radius in meters (10 cm for a 20 cm diameter drone) + self.build_bvh() + + def build_bvh(self): + p = index.Property() + p.dimension = 3 # Set dimension to 3 for 3D bounding boxes + self.bvh = index.Index(properties=p) + for i, obstacle in enumerate(self.obstacles): + aabb = self.compute_aabb(obstacle) + self.bvh.insert(i, aabb) + + def compute_aabb(self, obstacle): + x_center, y_center, z_min, z_max, radius = obstacle + radius += self.drone_radius # Expand the obstacle's radius by the drone's radius + x_min = x_center - radius + x_max = x_center + radius + y_min = y_center - radius + y_max = y_center + radius + z_min = z_min # Lower z-bound + z_max = z_max # Upper z-bound + return (x_min, y_min, z_min, x_max, y_max, z_max) + + def is_point_in_obstacle(self, point, obstacle): + +# Check if a point is inside an obstacle. + # :param point: The point (x, y, z). + # :param obstacle: The obstacle defined as (x_center, y_center, z_min, z_max, radius). + # :param drone_radius: The radius of the drone. + #:return: True if the point is inside the obstacle, False otherwise. + + x_center, y_center, z_min, z_max, radius = obstacle + effective_radius = radius + self.drone_radius + + # Check if the point is within the cylindrical obstacle's radius in the xy-plane + dist_xy = np.linalg.norm(np.array(point[:2]) - np.array([x_center, y_center])) + + # Check if the point is within the z bounds of the obstacle + within_z_bounds = z_min <= point[2] <= z_max + + # The point is inside the obstacle if it's within the effective radius and within the z bounds + return dist_xy <= effective_radius and within_z_bounds + + + def obstacle_free(self, point): + # Ensure that point has at least three elements for x, y, z + if len(point) < 3: + raise ValueError(f"Point {point} is incorrectly formatted, expected at least 3 elements for (x, y, z).") + + # Extract spatial coordinates + point = point[:3] + + nearby_obstacles = list(self.bvh.intersection((point[0], point[1], point[2], point[0], point[1], point[2]))) + for obs_idx in nearby_obstacles: + if self.is_point_in_obstacle(point, self.obstacles[obs_idx]): + return False + return True + + def collision_free(self, start, end, r): + # Ensure that start and end have at least three elements for x, y, z + if len(start) < 3 or len(end) < 3: + raise ValueError(f"Start {start} or end {end} point is incorrectly formatted, expected at least 3 elements for (x, y, z).") + + # Extract spatial coordinates + start = start[:3] + end = end[:3] + + direction = np.array(end) - np.array(start) + length = np.linalg.norm(direction) + direction = direction / length + steps = int(length / r) + for i in range(steps + 1): + point = np.array(start) + i * r * direction + if not self.obstacle_free(point): + return False + return True + + + def build_obs_index(self): + # Create an R-tree index for obstacles + p = index.Property() + p.dimension = 3 # Set dimension to 3 for 3D obstacles + self.obs = index.Index(properties=p) + for i, obstacle in enumerate(self.obstacles): + aabb = self.compute_aabb(obstacle) + self.obs.insert(i, aabb) + + + + def sample(self): + """ + Return a random location within the search space dimensions. + :return: A tuple representing a random location within the search space. + """ + return tuple(np.random.uniform(low, high) for low, high in self.dimension_lengths) + + + + + def es_points_along_line(self, x_a, x_b, r): + """ + Generate points along a line segment between x_a and x_b. + :param x_a: Starting point of the line segment. + :param x_b: Ending point of the line segment. + :param r: Resolution for sampling points. + :return: List of points along the line segment. + """ + distance = np.linalg.norm(np.array(x_b) - np.array(x_a)) + num_points = int(distance / r) + 1 + return [tuple(np.array(x_a) + t * (np.array(x_b) - np.array(x_a))) for t in np.linspace(0, 1, num_points)] + + + + + + def sample_free(self): + while True: + point = self.sample() + # Ensure that the sampled point is correctly formatted before checking if it's obstacle-free + if isinstance(point, (list, tuple, np.ndarray)) and len(point) >= 3: + if self.obstacle_free(point): + return point + else: + raise ValueError(f"Sampled point {point} is incorrectly formatted.") diff --git a/rrt_algorithms/utilities/fuzy_logic.py b/rrt_algorithms/utilities/fuzy_logic.py new file mode 100644 index 0000000..d78576a --- /dev/null +++ b/rrt_algorithms/utilities/fuzy_logic.py @@ -0,0 +1,61 @@ +import numpy as np +import skfuzzy as fuzz +from skfuzzy import control as ctrl + +class FuzzyController: + def __init__(self): + # Define fuzzy variables + self.dist_to_goal = ctrl.Antecedent(np.arange(0, 101, 1), 'dist_to_goal') + self.dist_to_obstacle = ctrl.Antecedent(np.arange(0, 101, 1), 'dist_to_obstacle') + + self.to_goal_gain = ctrl.Consequent(np.arange(0, 2.1, 0.1), 'to_goal_gain') + self.speed_gain = ctrl.Consequent(np.arange(0, 2.1, 0.1), 'speed_gain') + self.obstacle_gain = ctrl.Consequent(np.arange(0, 2.1, 0.1), 'obstacle_gain') + + # Membership functions for distance to goal + self.dist_to_goal['close'] = fuzz.trimf(self.dist_to_goal.universe, [0, 0, 30]) + self.dist_to_goal['medium'] = fuzz.trimf(self.dist_to_goal.universe, [20, 50, 80]) + self.dist_to_goal['far'] = fuzz.trimf(self.dist_to_goal.universe, [70, 100, 100]) + + # Membership functions for distance to obstacles + self.dist_to_obstacle['close'] = fuzz.trimf(self.dist_to_obstacle.universe, [0, 0, 30]) + self.dist_to_obstacle['medium'] = fuzz.trimf(self.dist_to_obstacle.universe, [20, 50, 80]) + self.dist_to_obstacle['far'] = fuzz.trimf(self.dist_to_obstacle.universe, [70, 100, 100]) + + # Membership functions for gain adjustments + self.to_goal_gain['low'] = fuzz.trimf(self.to_goal_gain.universe, [0, 0, 1]) + self.to_goal_gain['medium'] = fuzz.trimf(self.to_goal_gain.universe, [0.5, 1.0, 1.5]) + self.to_goal_gain['high'] = fuzz.trimf(self.to_goal_gain.universe, [1.0, 2.0, 2.0]) + + self.speed_gain['low'] = fuzz.trimf(self.speed_gain.universe, [0, 0, 1]) + self.speed_gain['medium'] = fuzz.trimf(self.speed_gain.universe, [0.5, 1.0, 1.5]) + self.speed_gain['high'] = fuzz.trimf(self.speed_gain.universe, [1.0, 2.0, 2.0]) + + self.obstacle_gain['low'] = fuzz.trimf(self.obstacle_gain.universe, [0, 0, 1]) + self.obstacle_gain['medium'] = fuzz.trimf(self.obstacle_gain.universe, [0.5, 1.0, 1.5]) + self.obstacle_gain['high'] = fuzz.trimf(self.obstacle_gain.universe, [1.0, 2.0, 2.0]) + + # Define fuzzy rules + self.rule1 = ctrl.Rule(self.dist_to_goal['close'] & self.dist_to_obstacle['close'], + [self.to_goal_gain['low'], self.speed_gain['medium'], self.obstacle_gain['high']]) + self.rule2 = ctrl.Rule(self.dist_to_goal['medium'] & self.dist_to_obstacle['medium'], + [self.to_goal_gain['medium'], self.speed_gain['medium'], self.obstacle_gain['medium']]) + self.rule3 = ctrl.Rule(self.dist_to_goal['far'] & self.dist_to_obstacle['far'], + [self.to_goal_gain['high'], self.speed_gain['high'], self.obstacle_gain['low']]) + self.rule4 = ctrl.Rule(self.dist_to_goal['far'] & self.dist_to_obstacle['close'], + [self.to_goal_gain['medium'], self.speed_gain['low'], self.obstacle_gain['high']]) + self.rule5 = ctrl.Rule(self.dist_to_goal['close'] & self.dist_to_obstacle['far'], + [self.to_goal_gain['high'], self.speed_gain['high'], self.obstacle_gain['low']]) + + self.control_system = ctrl.ControlSystem([self.rule1, self.rule2, self.rule3, self.rule4, self.rule5]) + self.simulation = ctrl.ControlSystemSimulation(self.control_system) + + def compute_gains(self, dist_to_goal, dist_to_obstacle): + self.simulation.input['dist_to_goal'] = dist_to_goal + self.simulation.input['dist_to_obstacle'] = dist_to_obstacle + self.simulation.compute() + return (self.simulation.output['to_goal_gain'], + self.simulation.output['speed_gain'], + self.simulation.output['obstacle_gain']) + + diff --git a/rrt_algorithms/utilities/obstacle_generation1.py b/rrt_algorithms/utilities/obstacle_generation1.py new file mode 100644 index 0000000..9acb30a --- /dev/null +++ b/rrt_algorithms/utilities/obstacle_generation1.py @@ -0,0 +1,64 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +from __future__ import annotations + +import random +import uuid + +import numpy as np + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from rrt_algorithms.search_space.search_space1 import SearchSpace + + + +def generate_random_cylindrical_obstacles(X: SearchSpace, start, end, n): + """ + Generates n random cylindrical obstacles without disrupting world connectivity. + It also respects start and end points so that they don't lie inside an obstacle. + """ + i = 0 + obstacles = [] + while i < n: + scollision = True + fcollision = True + + x_center = random.uniform(X.dimension_lengths[0][0], X.dimension_lengths[0][1]) + y_center = random.uniform(X.dimension_lengths[1][0], X.dimension_lengths[1][1]) + z_min = random.uniform(X.dimension_lengths[2][0], X.dimension_lengths[2][1] - 10) # Ensure some height + z_max = z_min + random.uniform(5, 15) # Give a random height between 5 and 15 + radius = random.uniform(2, 10) # Random radius between 2 and 10 + + if np.linalg.norm([start[0] - x_center, start[1] - y_center]) > radius: + scollision = False + if np.linalg.norm([end[0] - x_center, end[1] - y_center]) > radius: + fcollision = False + + obstacle = (x_center, y_center, z_min, z_max, radius) + + if scollision or fcollision: + continue + + obstacles.append(obstacle) + X.obs.insert(uuid.uuid4().int, X.compute_aabb(obstacle), X.compute_aabb(obstacle)) + i += 1 + + return obstacles + + + +def obstacle_generator(obstacles): + """ + Add obstacles to r-tree + :param obstacles: list of obstacles + """ + for obstacle in obstacles: + yield (uuid.uuid4().int, obstacle, obstacle) diff --git a/rrt_algorithms/utilities/obstacle_generation2.py b/rrt_algorithms/utilities/obstacle_generation2.py new file mode 100644 index 0000000..e790dd9 --- /dev/null +++ b/rrt_algorithms/utilities/obstacle_generation2.py @@ -0,0 +1,65 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +#the RRT and DWA algorithms function with the optimized collision checking using Bounding Volume Hierarchies (BVH). +#BVH Implementation: We use an R-tree (a type of BVH) to store axis-aligned bounding boxes (AABB) for each obstacle. This allows us to quickly identify which obstacles might intersect with a given point or path segment. +#Efficient Collision Checking: Instead of checking every obstacle for collision, we first use the BVH to narrow down the list to only those obstacles whose bounding boxes intersect with the point or path segment in question. +#Integration with DWA: The code remains the same in terms of how paths are generated and optimized, but now the collision checking is more efficient, leading to faster overall performance. +#All obstacles (both random and predefined) need to be properly registered in the SearchSpace so that the collision checking functions (obstacle_free and collision_free) are aware of all obstacles. +# the script avoids both static and dynamic obstacles, it starts from orange dot and its final goal is green. +from __future__ import annotations + +import random +import uuid + +import numpy as np + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from rrt_algorithms.search_space.search_space2 import SearchSpace + + +def generate_random_cylindrical_obstacles(X: SearchSpace, start: Tuple[float, float, float], end: Tuple[float, float, float], n: int) -> List[Tuple[float, float, float, float, float]]: + """ + Generates n random cylindrical obstacles without disrupting world connectivity. + It also respects start and end points so that they don't lie inside an obstacle. + """ + obstacles = [] + i = 0 + while i < n: + scollision = True + fcollision = True + + x_center = random.uniform(X.dimension_lengths[0][0], X.dimension_lengths[0][1]) + y_center = random.uniform(X.dimension_lengths[1][0], X.dimension_lengths[1][1]) + z_min = random.uniform(X.dimension_lengths[2][0], X.dimension_lengths[2][1] - 10) # Ensure some height + z_max = z_min + random.uniform(5, 15) # Give a random height between 5 and 15 + radius = random.uniform(2, 10) # Random radius between 2 and 10 + + if np.linalg.norm([start[0] - x_center, start[1] - y_center]) > radius: + scollision = False + if np.linalg.norm([end[0] - x_center, end[1] - y_center]) > radius: + fcollision = False + + obstacle = (x_center, y_center, z_min, z_max, radius) + + if scollision or fcollision: + continue + + obstacles.append(obstacle) + X.obstacles.append(obstacle) # Add to the SearchSpace's obstacles list + i += 1 + + # Rebuild the BVH with the new obstacles + X.build_bvh() + + return obstacles + + +def obstacle_generator(obstacles): + """ + Add obstacles to r-tree + :param obstacles: list of obstacles + """ + for obstacle in obstacles: + yield (uuid.uuid4().int, obstacle, obstacle) diff --git a/rrt_algorithms/utilities/plotting.py b/rrt_algorithms/utilities/plotting.py index 86e149e..87eafcb 100644 --- a/rrt_algorithms/utilities/plotting.py +++ b/rrt_algorithms/utilities/plotting.py @@ -1,13 +1,10 @@ -# This file is subject to the terms and conditions defined in -# file 'LICENSE', which is part of this source code package. from pathlib import Path - import plotly as py from plotly import graph_objs as go +import numpy as np colors = ['darkblue', 'teal'] - class Plot(object): def __init__(self, filename): """ @@ -20,8 +17,7 @@ def __init__(self, filename): self.filename = str(self.filename) self.data = [] self.layout = {'title': 'Plot', - 'showlegend': False - } + 'showlegend': False} self.fig = {'data': self.data, 'layout': self.layout} @@ -76,52 +72,111 @@ def plot_tree_3d(self, trees): ) self.data.append(trace) - def plot_obstacles(self, X, O): + def plot_obstacles(self, X, obstacles): + """ + Plot obstacles in the search space. + :param X: Search Space + :param obstacles: list of obstacles (cylindrical in this case) + """ + if X.dimensions == 3: # plot in 3D + for obstacle in obstacles: + x_center, y_center, z_min, z_max, radius = obstacle + + # Create the mesh grid for the cylindrical surface + z = np.linspace(z_min, z_max, 50) + theta = np.linspace(0, 2 * np.pi, 50) + theta_grid, z_grid = np.meshgrid(theta, z) + x_grid = x_center + radius * np.cos(theta_grid) + y_grid = y_center + radius * np.sin(theta_grid) + + # Plot the cylindrical surface + obs = go.Surface( + x=x_grid, + y=y_grid, + z=z_grid, + colorscale='Viridis', + opacity=0.7 + ) + self.data.append(obs) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def draw(self, auto_open=True): + """ + Render the plot to a file + """ + py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) + def plot_start(self, X, x_init): """ - Plot obstacles + Plot starting point :param X: Search Space - :param O: list of obstacles + :param x_init: starting location """ if X.dimensions == 2: # plot in 2D - self.layout['shapes'] = [] - for O_i in O: - # noinspection PyUnresolvedReferences - self.layout['shapes'].append( - { - 'type': 'rect', - 'x0': O_i[0], - 'y0': O_i[1], - 'x1': O_i[2], - 'y1': O_i[3], - 'line': { - 'color': 'purple', - 'width': 4, - }, - 'fillcolor': 'purple', - 'opacity': 0.70 - }, - ) + trace = go.Scatter( + x=[x_init[0]], + y=[x_init[1]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) elif X.dimensions == 3: # plot in 3D - for O_i in O: - obs = go.Mesh3d( - x=[O_i[0], O_i[0], O_i[3], O_i[3], O_i[0], O_i[0], O_i[3], O_i[3]], - y=[O_i[1], O_i[4], O_i[4], O_i[1], O_i[1], O_i[4], O_i[4], O_i[1]], - z=[O_i[2], O_i[2], O_i[2], O_i[2], O_i[5], O_i[5], O_i[5], O_i[5]], - i=[7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2], - j=[3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3], - k=[0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6], - color='purple', - opacity=0.70 - ) - self.data.append(obs) + trace = go.Scatter3d( + x=[x_init[0]], + y=[x_init[1]], + z=[x_init[2]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_goal(self, X, x_goal, color="green"): + """ + Plot goal point + :param X: Search Space + :param x_goal: goal location + :param color: color of the goal point + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_goal[0]], + y=[x_goal[1]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_goal[0]], + y=[x_goal[1]], + z=[x_goal[2]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) else: # can't plot in higher dimensions print("Cannot plot in > 3 dimensions") - def plot_path(self, X, path): + def plot_path(self, X, path, color='red'): """ Plot path through Search Space :param X: Search Space :param path: path through space given as a sequence of points + :param color: color of the path to be plotted """ if X.dimensions == 2: # plot in 2D x, y = [], [] @@ -132,7 +187,7 @@ def plot_path(self, X, path): x=x, y=y, line=dict( - color="red", + color=color, width=4 ), mode="lines" @@ -150,86 +205,45 @@ def plot_path(self, X, path): y=y, z=z, line=dict( - color="red", + color=color, width=4 ), mode="lines" ) self.data.append(trace) - else: # can't plot in higher dimensions + else: print("Cannot plot in > 3 dimensions") - def plot_start(self, X, x_init): - """ - Plot starting point - :param X: Search Space - :param x_init: starting location - """ - if X.dimensions == 2: # plot in 2D - trace = go.Scatter( - x=[x_init[0]], - y=[x_init[1]], - line=dict( - color="orange", - width=10 - ), - mode="markers" - ) - self.data.append(trace) - elif X.dimensions == 3: # plot in 3D - trace = go.Scatter3d( - x=[x_init[0]], - y=[x_init[1]], - z=[x_init[2]], - line=dict( - color="orange", - width=10 - ), - mode="markers" - ) - - self.data.append(trace) - else: # can't plot in higher dimensions - print("Cannot plot in > 3 dimensions") - - def plot_goal(self, X, x_goal): + def plot_intermediate_goal(self, X, x_intermediate): """ - Plot goal point + Plot intermediate goal point :param X: Search Space - :param x_goal: goal location + :param x_intermediate: intermediate goal location """ if X.dimensions == 2: # plot in 2D trace = go.Scatter( - x=[x_goal[0]], - y=[x_goal[1]], - line=dict( - color="green", - width=10 + x=[x_intermediate[0]], + y=[x_intermediate[1]], + marker=dict( + color="pink", + size=10 ), mode="markers" ) - self.data.append(trace) elif X.dimensions == 3: # plot in 3D trace = go.Scatter3d( - x=[x_goal[0]], - y=[x_goal[1]], - z=[x_goal[2]], - line=dict( - color="green", - width=10 + x=[x_intermediate[0]], + y=[x_intermediate[1]], + z=[x_intermediate[2]], + marker=dict( + color="pink", + size=10 ), mode="markers" ) - self.data.append(trace) else: # can't plot in higher dimensions - print("Cannot plot in > 3 dimensions") - - def draw(self, auto_open=True): - """ - Render the plot to a file - """ - py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) + print("Cannot plot in > 3 dimensions") diff --git a/rrt_algorithms/utilities/plotting0.py b/rrt_algorithms/utilities/plotting0.py new file mode 100644 index 0000000..86e149e --- /dev/null +++ b/rrt_algorithms/utilities/plotting0.py @@ -0,0 +1,235 @@ +# This file is subject to the terms and conditions defined in +# file 'LICENSE', which is part of this source code package. +from pathlib import Path + +import plotly as py +from plotly import graph_objs as go + +colors = ['darkblue', 'teal'] + + +class Plot(object): + def __init__(self, filename): + """ + Create a plot + :param filename: filename + """ + self.filename = Path(__file__).parent / "../../output/visualizations/" / f"{filename}.html" + if not self.filename.parent.exists(): + self.filename.parent.mkdir(parents=True, exist_ok=True) + self.filename = str(self.filename) + self.data = [] + self.layout = {'title': 'Plot', + 'showlegend': False + } + + self.fig = {'data': self.data, + 'layout': self.layout} + + def plot_tree(self, X, trees): + """ + Plot tree + :param X: Search Space + :param trees: list of trees + """ + if X.dimensions == 2: # plot in 2D + self.plot_tree_2d(trees) + elif X.dimensions == 3: # plot in 3D + self.plot_tree_3d(trees) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_tree_2d(self, trees): + """ + Plot 2D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter( + x=[start[0], end[0]], + y=[start[1], end[1]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_tree_3d(self, trees): + """ + Plot 3D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter3d( + x=[start[0], end[0]], + y=[start[1], end[1]], + z=[start[2], end[2]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_obstacles(self, X, O): + """ + Plot obstacles + :param X: Search Space + :param O: list of obstacles + """ + if X.dimensions == 2: # plot in 2D + self.layout['shapes'] = [] + for O_i in O: + # noinspection PyUnresolvedReferences + self.layout['shapes'].append( + { + 'type': 'rect', + 'x0': O_i[0], + 'y0': O_i[1], + 'x1': O_i[2], + 'y1': O_i[3], + 'line': { + 'color': 'purple', + 'width': 4, + }, + 'fillcolor': 'purple', + 'opacity': 0.70 + }, + ) + elif X.dimensions == 3: # plot in 3D + for O_i in O: + obs = go.Mesh3d( + x=[O_i[0], O_i[0], O_i[3], O_i[3], O_i[0], O_i[0], O_i[3], O_i[3]], + y=[O_i[1], O_i[4], O_i[4], O_i[1], O_i[1], O_i[4], O_i[4], O_i[1]], + z=[O_i[2], O_i[2], O_i[2], O_i[2], O_i[5], O_i[5], O_i[5], O_i[5]], + i=[7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2], + j=[3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3], + k=[0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6], + color='purple', + opacity=0.70 + ) + self.data.append(obs) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_path(self, X, path): + """ + Plot path through Search Space + :param X: Search Space + :param path: path through space given as a sequence of points + """ + if X.dimensions == 2: # plot in 2D + x, y = [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + trace = go.Scatter( + x=x, + y=y, + line=dict( + color="red", + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + x, y, z = [], [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + z.append(i[2]) + trace = go.Scatter3d( + x=x, + y=y, + z=z, + line=dict( + color="red", + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_start(self, X, x_init): + """ + Plot starting point + :param X: Search Space + :param x_init: starting location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_init[0]], + y=[x_init[1]], + line=dict( + color="orange", + width=10 + ), + mode="markers" + ) + + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_init[0]], + y=[x_init[1]], + z=[x_init[2]], + line=dict( + color="orange", + width=10 + ), + mode="markers" + ) + + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_goal(self, X, x_goal): + """ + Plot goal point + :param X: Search Space + :param x_goal: goal location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_goal[0]], + y=[x_goal[1]], + line=dict( + color="green", + width=10 + ), + mode="markers" + ) + + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_goal[0]], + y=[x_goal[1]], + z=[x_goal[2]], + line=dict( + color="green", + width=10 + ), + mode="markers" + ) + + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def draw(self, auto_open=True): + """ + Render the plot to a file + """ + py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) diff --git a/rrt_algorithms/utilities/plotting1.py b/rrt_algorithms/utilities/plotting1.py new file mode 100644 index 0000000..ffb485b --- /dev/null +++ b/rrt_algorithms/utilities/plotting1.py @@ -0,0 +1,239 @@ +from pathlib import Path +import plotly as py +from plotly import graph_objs as go +import numpy as np + +colors = ['darkblue', 'teal'] + +class Plot(object): + def __init__(self, filename): + """ + Create a plot + :param filename: filename + """ + self.filename = Path(__file__).parent / "../../output/visualizations/" / f"{filename}.html" + if not self.filename.parent.exists(): + self.filename.parent.mkdir(parents=True, exist_ok=True) + self.filename = str(self.filename) + self.data = [] + self.layout = {'title': 'Plot', + 'showlegend': False} + + self.fig = {'data': self.data, + 'layout': self.layout} + + def plot_tree(self, X, trees): + """ + Plot tree + :param X: Search Space + :param trees: list of trees + """ + if X.dimensions == 2: # plot in 2D + self.plot_tree_2d(trees) + elif X.dimensions == 3: # plot in 3D + self.plot_tree_3d(trees) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_tree_2d(self, trees): + """ + Plot 2D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter( + x=[start[0], end[0]], + y=[start[1], end[1]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_tree_3d(self, trees): + """ + Plot 3D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter3d( + x=[start[0], end[0]], + y=[start[1], end[1]], + z=[start[2], end[2]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_obstacles(self, X, obstacles): + if X.dimensions == 3: # Plot in 3D + for obstacle in obstacles: + x_center, y_center, z_min, z_max, radius = obstacle + + z = np.linspace(z_min, z_max, 50) + theta = np.linspace(0, 2 * np.pi, 50) + theta_grid, z_grid = np.meshgrid(theta, z) + x_grid = x_center + radius * np.cos(theta_grid) + y_grid = y_center + radius * np.sin(theta_grid) + + obs = go.Surface( + x=x_grid, + y=y_grid, + z=z_grid, + colorscale='Viridis', + opacity=0.7 + ) + self.data.append(obs) + def draw(self, auto_open=True): + """ + Render the plot to a file + """ + py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) + def plot_start(self, X, x_init): + """ + Plot starting point + :param X: Search Space + :param x_init: starting location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_init[0]], + y=[x_init[1]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_init[0]], + y=[x_init[1]], + z=[x_init[2]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_goal(self, X, x_goal, color="green"): + """ + Plot goal point + :param X: Search Space + :param x_goal: goal location + :param color: color of the goal point + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_goal[0]], + y=[x_goal[1]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_goal[0]], + y=[x_goal[1]], + z=[x_goal[2]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_path(self, X, path, color='red'): + """ + Plot path through Search Space + :param X: Search Space + :param path: path through space given as a sequence of points + :param color: color of the path to be plotted + """ + if X.dimensions == 2: # plot in 2D + x, y = [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + trace = go.Scatter( + x=x, + y=y, + line=dict( + color=color, + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + x, y, z = [], [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + z.append(i[2]) + trace = go.Scatter3d( + x=x, + y=y, + z=z, + line=dict( + color=color, + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + else: + print("Cannot plot in > 3 dimensions") + + + def plot_intermediate_goal(self, X, x_intermediate): + """ + Plot intermediate goal point + :param X: Search Space + :param x_intermediate: intermediate goal location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_intermediate[0]], + y=[x_intermediate[1]], + marker=dict( + color="pink", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_intermediate[0]], + y=[x_intermediate[1]], + z=[x_intermediate[2]], + marker=dict( + color="pink", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") diff --git a/rrt_algorithms/utilities/plotting2.py b/rrt_algorithms/utilities/plotting2.py new file mode 100644 index 0000000..62e1278 --- /dev/null +++ b/rrt_algorithms/utilities/plotting2.py @@ -0,0 +1,239 @@ +from pathlib import Path +import plotly as py +from plotly import graph_objs as go +import numpy as np + +colors = ['lightblue', 'teal'] + +class Plot(object): + def __init__(self, filename): + """ + Create a plot + :param filename: filename + """ + self.filename = Path(__file__).parent / "../../output/visualizations/" / f"{filename}.html" + if not self.filename.parent.exists(): + self.filename.parent.mkdir(parents=True, exist_ok=True) + self.filename = str(self.filename) + self.data = [] + self.layout = {'title': 'Plot', + 'showlegend': False} + + self.fig = {'data': self.data, + 'layout': self.layout} + + def plot_tree(self, X, trees): + """ + Plot tree + :param X: Search Space + :param trees: list of trees + """ + if X.dimensions == 2: # plot in 2D + self.plot_tree_2d(trees) + elif X.dimensions == 3: # plot in 3D + self.plot_tree_3d(trees) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_tree_2d(self, trees): + """ + Plot 2D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter( + x=[start[0], end[0]], + y=[start[1], end[1]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_tree_3d(self, trees): + """ + Plot 3D trees + :param trees: trees to plot + """ + for i, tree in enumerate(trees): + for start, end in tree.E.items(): + if end is not None: + trace = go.Scatter3d( + x=[start[0], end[0]], + y=[start[1], end[1]], + z=[start[2], end[2]], + line=dict( + color=colors[i] + ), + mode="lines" + ) + self.data.append(trace) + + def plot_obstacles(self, X, obstacles): + if X.dimensions == 3: # Plot in 3D + for obstacle in obstacles: + x_center, y_center, z_min, z_max, radius = obstacle + + z = np.linspace(z_min, z_max, 50) + theta = np.linspace(0, 2 * np.pi, 50) + theta_grid, z_grid = np.meshgrid(theta, z) + x_grid = x_center + radius * np.cos(theta_grid) + y_grid = y_center + radius * np.sin(theta_grid) + + obs = go.Surface( + x=x_grid, + y=y_grid, + z=z_grid, + colorscale='Viridis', + opacity=0.7 + ) + self.data.append(obs) + def draw(self, auto_open=True): + """ + Render the plot to a file + """ + py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) + def plot_start(self, X, x_init): + """ + Plot starting point + :param X: Search Space + :param x_init: starting location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_init[0]], + y=[x_init[1]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_init[0]], + y=[x_init[1]], + z=[x_init[2]], + marker=dict( + color="orange", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_goal(self, X, x_goal, color="green"): + """ + Plot goal point + :param X: Search Space + :param x_goal: goal location + :param color: color of the goal point + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_goal[0]], + y=[x_goal[1]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_goal[0]], + y=[x_goal[1]], + z=[x_goal[2]], + marker=dict( + color=color, + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions") + + def plot_path(self, X, path, color='red'): + """ + Plot path through Search Space + :param X: Search Space + :param path: path through space given as a sequence of points + :param color: color of the path to be plotted + """ + if X.dimensions == 2: # plot in 2D + x, y = [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + trace = go.Scatter( + x=x, + y=y, + line=dict( + color=color, + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + x, y, z = [], [], [] + for i in path: + x.append(i[0]) + y.append(i[1]) + z.append(i[2]) + trace = go.Scatter3d( + x=x, + y=y, + z=z, + line=dict( + color=color, + width=4 + ), + mode="lines" + ) + + self.data.append(trace) + else: + print("Cannot plot in > 3 dimensions") + + + def plot_intermediate_goal(self, X, x_intermediate): + """ + Plot intermediate goal point + :param X: Search Space + :param x_intermediate: intermediate goal location + """ + if X.dimensions == 2: # plot in 2D + trace = go.Scatter( + x=[x_intermediate[0]], + y=[x_intermediate[1]], + marker=dict( + color="pink", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + elif X.dimensions == 3: # plot in 3D + trace = go.Scatter3d( + x=[x_intermediate[0]], + y=[x_intermediate[1]], + z=[x_intermediate[2]], + marker=dict( + color="pink", + size=10 + ), + mode="markers" + ) + self.data.append(trace) + else: # can't plot in higher dimensions + print("Cannot plot in > 3 dimensions")