class GraphicsComponent():
def __init__(self, color, fill, edge_color):
self.color = color
self.edge_color = edge_color
self.fill = fill
self.patch = None
def update(self, geometry_component, canvas):
if self.patch is None:
self.patch = PolygonPatch(geometry_component.polygon,
fc=self.color,
ec=self.edge_color,
fill=self.fill)
#print(geometry_component._position)
#print(geometry_component.polygon.representative_point())
canvas.axes.add_patch(self.patch)
else:
path = PolygonPatch(geometry_component.polygon,
fc=self.color,
fill=self.fill).get_path()
self.patch._path = path
self.patch.set_facecolor(self.color)
def remove(self):
self.patch.remove()
class Circuit(object):
def __init__(self, points, n_cars, width=0.3, num_checkpoints=100):
self.n_cars = n_cars
if isinstance(points, np.ndarray):
points = points.tolist()
self.points = points
if self.points[0] != self.points[-1]:
self.points.append(points[0])
# Compute circuit's geometry
self.line = geom.LineString(self.points)
self.width = width
self.circuit = self.line.buffer(self.width, cap_style=1)
# For numerical stabilities when checking if something is inside the
# circuit.
self.dilated = self.line.buffer(self.width * 1.01, cap_style=1)
# Where the start line is
self.define_start()
# Define the checkpoints
self.make_checkpoints(n=num_checkpoints)
def define_start(self):
"""The start line is in the middle of the longest horizontal segment."""
last = geom.Point(*self.line.coords[0])
self.start = last
maxDistance = 0
for x, y in self.line.coords[1:]:
curr = geom.Point((x, y))
if curr.distance(last) > maxDistance and curr.y == last.y:
maxDistance = curr.distance(last)
self.start = geom.Point((0.5 * (x + last.x)),
0.5 * (y + last.y))
last = curr
self.start_line = geom.LineString([
(self.start.x, self.start.y - self.width),
(self.start.x, self.start.y + self.width)
])
def make_checkpoints(self, n):
step_ext = self.circuit.exterior.length / n
step_int = self.circuit.interiors[0].length / n
self.checklines = []
for i in range(n):
self.checklines.append(
geom.LineString([
self.circuit.exterior.interpolate(step_ext * (n - i)),
self.circuit.interiors[0].interpolate(step_int * i)
], ))
self.reset()
def reset(self):
self.checkpoints = np.zeros((self.n_cars, len(self.checklines)),
dtype=np.bool)
self.laps = np.zeros(self.n_cars, dtype=np.int32)
self.progression = np.zeros(self.n_cars, dtype=np.float)
self.chicken_dinner = False
self.half_chicken_dinner = False
def update_checkpoints(self, start, stop):
x_start, y_start = start
x_stop, y_stop = stop
for car_id in range(self.n_cars):
x_str, y_str = x_start[car_id], y_start[car_id]
x_stp, y_stp = x_stop[car_id], y_stop[car_id]
traj = geom.LineString([(x_str, y_str), (x_stp, y_stp)])
checkpoints = self.checkpoints[car_id]
if not np.all(checkpoints):
for idx, line in enumerate(self.checklines):
if line.intersects(traj):
checkpoints[idx] = True
else:
self.checkpoints[car_id] = np.zeros(len(self.checklines),
dtype=np.bool)
self.laps[car_id] += 1
self.progression[car_id] = np.sum(checkpoints) / len(checkpoints)
def __contains__(self, shape):
return self.dilated.contains(shape)
def plot(self, ax, color='gray', skeleton=True):
title = 'Let the best AI win !'
ax.set_title(title, fontname='Lucida Console', fontsize=32)
if skeleton:
self.skeleton_patch = ax.plot(self.line.xy[0],
self.line.xy[1],
color='white',
linewidth=3,
solid_capstyle='round',
zorder=3,
linestyle='--')
self.start_line_patch = ax.plot(self.start_line.xy[0],
self.start_line.xy[1],
color='red',
linewidth=3,
linestyle='-',
zorder=3)
self.circuit_patch = PolygonPatch(self.circuit,
fc=color,
ec='red',
alpha=0.5,
zorder=2)
ax.add_patch(self.circuit_patch)
offset_x = (self.circuit.bounds[2] - self.circuit.bounds[0]) * 0.15
offset_y = (self.circuit.bounds[3] - self.circuit.bounds[1]) * 0.3
self.x_min, self.x_max = self.circuit.bounds[
0], self.circuit.bounds[2] + offset_x
self.y_min, self.y_max = self.circuit.bounds[
1], self.circuit.bounds[3] + offset_y
self.scoreboard_text = ax.text(self.x_max,
self.y_max,
'SCOREBOARD',
fontname='Lucida Console',
fontsize=20,
ha='left',
va='top')
self.time_text = ax.text(self.x_min,
self.y_max,
"Step: 0",
alpha=0.3,
fontname='Lucida Console',
fontsize=20,
ha='left',
va='top')
self.texts = []
for rank in range(min(10, self.n_cars)):
x_text_pos = self.x_max
y_text_pos = self.y_min + 0.8 * (self.y_max - self.y_min) * (
1 - rank / min(10, self.n_cars))
text = ax.text(x_text_pos,
y_text_pos,
" ",
fontname='Lucida Console',
ha='left')
self.texts.append(text)
ax.set_xlim(self.x_min, self.x_max)
ax.set_ylim(self.y_min, self.y_max)
ax.set_aspect(1)
def update_plot(self, ax, cars, time):
self.time_text.set_text(f"Step: {time}")
crashed = cars.render_locked
names = cars.names
prog_total = self.progression + self.laps
ranks = np.argsort(prog_total)
for k, car_id in enumerate(ranks):
rank = self.n_cars - k
max_name_len = 16
true_name = names[car_id][:max_name_len]
name_len = len(true_name)
if name_len < max_name_len:
name = ' ' * (max_name_len - name_len) + true_name
else:
name = true_name
lap = self.laps[car_id]
if lap == 1:
bbox = dict(facecolor=(1, 1, 0, 0.3),
edgecolor='none',
boxstyle='round,pad=0.5')
if not self.half_chicken_dinner:
title = f'{true_name} is on fire !'
self.half_chicken_dinner = True
ax.set_title(title,
fontname='Lucida Console',
color='orange',
fontsize=32)
elif lap >= 2:
bbox = dict(facecolor=(0, 1, 0, 0.3),
edgecolor='none',
boxstyle='round,pad=0.5')
if not self.chicken_dinner:
title = f'A winner is {true_name} ({car_id})!'
self.chicken_dinner = True
ax.set_title(title,
fontname='Lucida Console',
color='green',
fontsize=32)
if lap < 2:
if crashed[car_id]:
bbox = dict(facecolor=(1, 0, 0, 0.3),
edgecolor='none',
boxstyle='round,pad=0.5')
else:
bbox = dict(facecolor='none', edgecolor='none')
if rank <= 10:
if rank == 1:
rank_text = '1er'
elif rank == 2:
rank_text = '2nd'
elif rank == 3:
rank_text = '3rd'
else:
rank_text = f'{rank}e'
if len(rank_text) < 3:
rank_text += ' '
if len(ranks) == 1:
rank_text = ''
text = f'{rank_text} {name} - Lap {lap+1} - {self.progression[car_id]:2.0%} - {car_id}'
text_patch = self.texts[rank - 1]
text_patch.set_visible(True)
text_patch.set_text(text)
text_patch.set_color(cars.colors[car_id])
text_patch.set_bbox(bbox)
def remove_plot(self, ax):
self.start_line_patch[0].remove()
if hasattr(self, 'skeleton_patch'):
self.skeleton_patch[0].remove()
self.circuit_patch.remove()
self.scoreboard_text.remove()
self.time_text.remove()
for text in self.texts:
text.remove()
def main():
n = 2 # Nr of agents
global dv, size_field, max_velocity, max_acceleration, t # change this later
size_field = 10
max_velocity = 2 ##0.5 these works for smaller radiuses, also produces the dancing thingy mentioned in the paper
max_acceleration = 2 ##0.5
dv = 0.1 #0.1 # Step size when looking for new velocities
t = 1 # timestep I guess
simulation_time = 200
radius = 1
agentA = Agents([1, 1], [0.5, 0.5], [8, 8], radius,
'r') # position, velocity, goal, radius, color
agentB = Agents([8, 8], [1, -0.5], [1, 1], radius, 'b')
agentC = Agents([1, 8], [1, 2], [8, 1], radius, 'y')
agentD = Agents([8, 1], [-1, 2], [1, 8], radius, 'g')
agentF = Agents([1, 5], [1, 2], [8, 5], radius, 'b')
agentG = Agents([8, 5], [-1, 2], [1, 5], radius, 'b')
agents = [agentA, agentB, agentC, agentD, agentF, agentG]
fig, ax = plt.subplots(
) # Fig ska man aldrig bry sig om om man inte vill ändra själva plotrutan
time = 0
while time < simulation_time:
for agent in agents:
VOs = agent.calculate_velocity_obstacles(agents)
preffered_vel = agent.find_preffered_velocity()
#print("preffered velocity", preffered_vel)
new_velocity = agent.avoidance_strategy(preffered_vel)
if agent == agentA:
diff_x = new_velocity[0] * t
diff_y = new_velocity[1] * t
print(time)
#print("NEW X", diff_x)
#print("NEW Y", diff_y)
print("\n")
agent.pos = [
agent.pos[0] + new_velocity[0] * t,
agent.pos[1] + new_velocity[1] * t
]
agent.vel = new_velocity
#velocity_ob = agentA.calculate_velocity_obstacles([agentB])
if time == 0:
anim1, anim2, anim3, anim4, anim5, anim6 = ax.plot(
agentA.pos[0], agentA.pos[1], 'go', agentB.pos[0],
agentB.pos[1], 'bo', agentC.pos[0], agentC.pos[1], 'ro',
agentD.pos[0], agentD.pos[1], 'yo', agentF.pos[0],
agentF.pos[1], 'yo', agentG.pos[0], agentG.pos[1], 'yo')
patch = PolygonPatch(agentA.velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch)
patch1a = PolygonPatch(agentA.velocity_objects[1],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch1a)
patch2 = PolygonPatch(agentA.velocity_objects[2],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch2)
patch3a = PolygonPatch(agentA.velocity_objects[3],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch3a)
patch4 = PolygonPatch(agentA.velocity_objects[4],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch4)
# patch2 = PolygonPatch(agentB.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch2)
# patch3 = PolygonPatch(agentC.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch3)
# patch4 = PolygonPatch(agentD.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch4)
ax.axis([-0, size_field, -0, size_field], 'equal')
f = ax.add_artist(agentA.shape)
f2 = ax.add_artist(agentB.shape)
f3 = ax.add_artist(agentC.shape)
f4 = ax.add_artist(agentD.shape)
f5 = ax.add_artist(agentF.shape)
f6 = ax.add_artist(agentG.shape)
vel1 = ax.quiver(
agentB.pos[0],
agentB.pos[1],
agentB.vel[0],
agentB.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel2 = ax.quiver(
agentA.pos[0],
agentA.pos[1],
agentA.vel[0],
agentA.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel3 = ax.quiver(
agentC.pos[0],
agentC.pos[1],
agentC.vel[0],
agentC.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel4 = ax.quiver(
agentD.pos[0],
agentD.pos[1],
agentD.vel[0],
agentD.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
else:
anim1.set_data(agentA.pos[0], agentA.pos[1])
anim2.set_data(agentB.pos[0], agentB.pos[1])
anim3.set_data(agentC.pos[0], agentC.pos[1])
anim4.set_data(agentD.pos[0], agentD.pos[1])
anim5.set_data(agentF.pos[0], agentF.pos[1])
anim6.set_data(agentG.pos[0], agentG.pos[1])
#velocity_object = Polygon([pos11, tp22, tp11])
#s11 = patch.get_path()
#print(s11)
#patch.bounds= (ps11, tp22, tp11)# = PolygonPatch(agentA.velocity_objects[0], facecolor='#ff3333', edgecolor='#6699cc', alpha=0.5, zorder=2)
#print(type(patch))
patch.remove()
patch = PolygonPatch(agentA.velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch)
patch1a.remove()
patch1a = PolygonPatch(agentA.velocity_objects[1],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch1a)
patch2.remove()
patch2 = PolygonPatch(agentA.velocity_objects[2],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch2)
patch3a.remove()
patch3a = PolygonPatch(agentA.velocity_objects[3],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch3a)
patch4.remove()
patch4 = PolygonPatch(agentA.velocity_objects[4],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch4)
# patch2.remove()
# patch2 = PolygonPatch(agentB.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch2)
# patch3.remove()
# patch3 = PolygonPatch(agentC.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch3)
# patch4.remove()
# patch4 = PolygonPatch(agentD.velocity_objects[0], facecolor='#FFFF00', edgecolor='#FFFF00', alpha=0.5, zorder=2)
# ax.add_patch(patch4)
f.center = agentA.pos[0], agentA.pos[1]
f2.center = agentB.pos[0], agentB.pos[1]
f3.center = agentC.pos[0], agentC.pos[1]
f4.center = agentD.pos[0], agentD.pos[1]
f5.center = agentF.pos[0], agentF.pos[1]
f6.center = agentG.pos[0], agentG.pos[1]
vel1.remove()
vel2.remove()
vel3.remove()
vel4.remove()
vel1 = ax.quiver(
agentB.pos[0],
agentB.pos[1],
agentB.vel[0],
agentB.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel2 = ax.quiver(
agentA.pos[0],
agentA.pos[1],
agentA.vel[0],
agentA.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel3 = ax.quiver(
agentC.pos[0],
agentC.pos[1],
agentC.vel[0],
agentC.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
vel4 = ax.quiver(
agentD.pos[0],
agentD.pos[1],
agentD.vel[0],
agentD.vel[1],
scale=6,
scale_units='width') ##in case we want velocity vector
#f = ax.add_artist(agentA.shape)
#f2 = ax.add_artist(agentB.shape)
#ax.quiver(agentB.pos[0], agentB.pos[1], agentB.vel[0], agentB.vel[1], scale=6, scale_units ='width') ##in case we want velocity vector
#ax.quiver(agentA.pos[0], agentA.pos[1], agentA.vel[0], agentA.vel[1], scale=6, scale_units ='width') ##in case we want velocity vector
ax.plot(agentA.goal[0], agentA.goal[1], 'r*')
ax.plot(agentB.goal[0], agentB.goal[1], 'g*')
ax.plot(agentC.goal[0], agentC.goal[1], 'b*')
ax.plot(agentD.goal[0], agentD.goal[1], 'y*')
ax.plot(agentF.goal[0], agentF.goal[1], 'b*')
ax.plot(agentG.goal[0], agentG.goal[1], 'b*')
time += 1
plt.pause(0.0000000000001)
def main():
n = 2 # Nr of agents
global dv, size_field, max_velocity, max_acceleration, t # change this later
size_field = 40
max_velocity = 6 ##0.5 these works for smaller radiuses, also produces the dancing thingy mentioned in the paper
max_acceleration = 2 ##0.5
dv = 0.05 #0.1 # Step size when looking for new velocities
t = 1 # timestep I guess
simulation_time = 250
radius = 1
pos = []
goal = []
# generate start and end positions
# for t in range(n):
# for i in range(n):
# x = np.random.uniform(low=(size_field/2-5), high =(size_field/2+5))
# y = size_field+10
# goal.append((x,y))
pos = []
# for x in range(5,35,3):
# if x ==5:
# pos.append((x-2.5,2))
# else:
# pos.append((x-2.5,2))
# pos.append((2.5,x-2.5))
for degree in range(
0, 360, 30
): #181): INCREASED THIS TO Make the agents avoid each other more
theta = degree * 0.0174533
r = 15
x = 16 + r * math.cos(theta)
y = 16 + r * math.sin(theta)
#vx = np.random.uniform(low=-max_velocity, high=max_velocity)
#vy = np.random.uniform(low=-max_velocity, high=max_velocity)
pos.append((x, y))
#random.shuffle(pos)
goal = rotate(pos, 6)
#goal = pos[::-1]
agents = create_agents(n, radius, pos, goal)
fig, ax = plt.subplots(
) # Fig ska man aldrig bry sig om om man inte vill ändra själva plotrutan
#boundary_polygons = init_map(size_field, ax, radius)
# Consider the obstacles as agents with zero velocity! Don't consider these when updating velocities etc
#obstacle_agents = create_fake_agents(len(boundary_polygons), radius, boundary_polygons, 1)
time = 0
#avoid = [agents+obstacle_agents] # want to send in both agents and obstacles when creating VOs
save_trajectories = [[] for i in range(n)]
k = 0
while time < simulation_time:
for agent in agents:
VOs = agent.calculate_velocity_obstacles(
agents) #, boundary_polygons)
preffered_vel = agent.find_preffered_velocity()
#print("preffered velocity", preffered_vel)
new_velocity = agent.avoidance_strategy(preffered_vel)
agent.pos = [
agent.pos[0] + new_velocity[0] * t,
agent.pos[1] + new_velocity[1] * t
]
agent.vel = new_velocity
save_trajectories[k].append(agent.pos) # Keep going from here!
k += 1
k = 0
if time == 0:
anims = []
patches = []
fs = []
for agent in agents:
dd = ax.plot(agent.pos[0], agent.pos[1], 'go')
anims.append(dd)
oo = ax.add_artist(agent.shape)
fs.append(oo)
ax.axis([-0, size_field, -0, size_field], 'equal')
patcha = PolygonPatch(agents[0].velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patcha)
patch1 = PolygonPatch(agents[1].velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch1)
else:
for (agent, anim, f) in zip(agents, anims, fs):
anim[0].set_data(agent.pos[0], agent.pos[1])
f.center = agent.pos[0], agent.pos[1]
ax.plot(agent.goal[0], agent.goal[1], 'r*')
patcha.remove()
patcha = PolygonPatch(agents[0].velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patcha)
patch1.remove()
patch1 = PolygonPatch(agents[1].velocity_objects[0],
facecolor='#ff3333',
edgecolor='#6699cc',
alpha=0.5,
zorder=2)
ax.add_patch(patch1)
time += 1
print("time:", time)
plt.pause(0.1)
if time == 1:
ddd = input("Tryck")
#print(save_trajectories[0])
fig, ax = plt.subplots()
colors = [
'r-', 'b-', 'g-', 'c-', 'm-', 'y-', 'k-', 'w*', 'b*', 'g*', 'r*', 'c*',
'm*', 'y*', 'k*', 'w*'
]
for agent in save_trajectories:
#color = colors.pop()
x = []
y = []
for spot in agent:
x.append(spot[0]) #ax.plot(spot[0],spot[1])#,color)
y.append(spot[1])
ax.plot(x, y) #,color)
plt.show()
