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()