-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathplot.py
127 lines (95 loc) · 3.74 KB
/
plot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
# # import numpy as np
# # import matplotlib.pyplot as plt
# # from agent import Agent
# # n_steps = 20000
# # agent = Agent(position=[0,0], goal=np.array([700,500]), color="blue")
# # obstacle_p = [200,230]
# # att_fs = []
# # rep_fs = []
# # for _ in range(n_steps):
# # att_f = np.linalg.norm(agent.Attractive_force())
# # rep_f = np.linalg.norm(agent.Repulsive_force(obstacle_p))
# # print(rep_f)
# # att_fs.append(att_f)
# # rep_fs.append(rep_f)
# # agent.move()
# # # Create time array
# # time = np.arange(n_steps)
# # # Plot attractive and repulsive forces over time
# # plt.plot(time, att_fs, label='Attractive Force')
# # plt.plot(time, rep_fs, label='Repulsive Force')
# # plt.xlabel('Time Step')
# # plt.ylabel('Force Magnitude')
# # plt.title('Attractive and Repulsive Forces over Time')
# # plt.legend()
# # plt.grid(True)
# # plt.show()
# import numpy as np
# import matplotlib.pyplot as plt
# from agent import Agent
# import json
# def parse_color(color_str):
# color_dict = {
# "red": (255, 0, 0),
# "green": (0, 255, 0),
# "blue": (0, 0, 255),
# "yellow": (255, 255, 0),
# "purple": (128, 0, 128),
# "orange": (255, 165, 0),
# "pink": (255, 192, 203),
# "black": (0, 0, 0),
# "white": (255, 255, 255)
# }
# return color_dict.get(color_str, (0, 0, 0))
# def visualize_potential_field():
# # Load agent and obstacle data
# with open('agent.json') as f:
# agent_data = json.load(f)
# with open('obs.json') as f:
# obs_data = json.load(f)
# def visualize_potential_field():
# # Load agent and obstacle data
# with open('agent.json') as f:
# agent_data = json.load(f)
# with open('obs.json') as f:
# obs_data = json.load(f)
# # Create agent object
# agent = Agent(position=(agent_data['position']['x'], agent_data['position']['y']),
# color=agent_data['color'],
# radius=agent_data['length'],
# max_speed=0.5)
# # Extract obstacle position and radius and convert to integers
# obs_pos = (int(obs_data['position']['x']), int(obs_data['position']['y']))
# obs_radius = int(obs_data['length'])
# # Extract goal position
# goal_pos = (agent.goal[0], agent.goal[1])
# # Normalize color values to the range [0, 1]
# obs_color = tuple(c / 255 for c in parse_color(obs_data['color']))
# goal_color = 'green' # You can choose the color for the goal position
# # Define grid for potential field visualization
# x = np.linspace(0, 800, 50)
# y = np.linspace(0, 600, 50)
# X, Y = np.meshgrid(x, y)
# # Calculate potential field at each point in the grid
# F_total = np.zeros_like(X)
# for i in range(len(x)):
# for j in range(len(y)):
# position = np.array([x[i], y[j]])
# F_total[j, i] = np.linalg.norm(agent.Repulsive_force(position) + agent.Attractive_force())
# # Plot potential field with obstacles
# plt.figure(figsize=(8, 6))
# levels = np.linspace(0, np.max(F_total), 50) # Adjust levels for better visibility
# plt.contourf(X, Y, F_total, levels=levels, cmap='viridis')
# plt.colorbar(label='Potential Field Magnitude')
# plt.scatter(*agent.p, color=parse_color(agent.color), label='Agent')
# circle = plt.Circle(obs_pos, obs_radius, color=obs_color, fill=True, alpha=0.5)
# plt.gca().add_patch(circle)
# plt.scatter(*goal_pos, color=goal_color, label='Goal') # Plotting the goal position
# plt.xlabel('X Position')
# plt.ylabel('Y Position')
# plt.title('Static Visualization of Potential Field')
# plt.legend()
# plt.grid(True)
# plt.show()
# if __name__ == "__main__":
# visualize_potential_field()