├── pso ├── __init__.py ├── utils.py ├── pso.py └── main.py ├── .gitignore ├── imgs ├── Sectors.png ├── LapTimeEvolution.png └── RacingLineEvolution.gif ├── LICENSE ├── README.md └── data └── tracks.json /pso/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ -------------------------------------------------------------------------------- /imgs/Sectors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ParsaD23/Racing-Line-Optimization-with-PSO/HEAD/imgs/Sectors.png -------------------------------------------------------------------------------- /imgs/LapTimeEvolution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ParsaD23/Racing-Line-Optimization-with-PSO/HEAD/imgs/LapTimeEvolution.png -------------------------------------------------------------------------------- /imgs/RacingLineEvolution.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ParsaD23/Racing-Line-Optimization-with-PSO/HEAD/imgs/RacingLineEvolution.gif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Parsa Dahesh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /pso/utils.py: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------------------+ 2 | # 3 | # Parsa Dahesh (dinoco.parsa23@gmail.com or parsa.dahesh@studio.unibo.it) 4 | # Racing Line Optimization with PSO 5 | # MIT License, Copyright (c) 2021 Parsa Dahesh 6 | # 7 | #------------------------------------------------------------------------------+ 8 | 9 | import math 10 | import matplotlib.pyplot as plt 11 | 12 | def plot_lines(lines): 13 | for l in lines: 14 | x, y = l.xy 15 | plt.plot(x,y) 16 | 17 | def get_closet_points(point, array): 18 | '''Closest point 19 | 20 | Given a point and an array of points, returns the array point with the lower euclidean distance to the original point. 21 | 22 | Parameters 23 | ---------- 24 | point : list 25 | Point coordinates 26 | array : list 27 | List of coordinates 28 | 29 | Returns 30 | ------- 31 | result : list 32 | Point coordinate in the array list 33 | ''' 34 | 35 | result = [] 36 | distance = 1000 37 | for i in range(len(array)): 38 | temp = math.sqrt((point[0]-array[i][0])**2+(point[1]-array[i][1])**2) 39 | if tempRacing Line Optimization with PSO 2 | 3 | This repository contains a racing line optimization algorithm in python that uses **Particle Swarm Optimization**. 4 | 5 | ## Requirements 6 | 7 | This version was developed and tested with ```python==3.8.12```. The following modules are required: 8 | * ```matplotlib``` 9 | * ```numpy``` 10 | * ```scipy``` 11 | * ```shapely``` 12 | 13 | ## How does it work? 14 | 15 | ### 1. Input Parameters 16 | 17 | The **input** parameters are: 18 | - `track_layout`: array of $(x,y)$ coordinates that represent the layout of the track. 19 | - `width`: width of the track. We assume it is constant along the track. 20 | They are stored in the `./data/tracks.json` file. 21 | 22 | ### 2. Define the search space 23 | 24 | The **track borders** are obtained by adding an offset (i.e. half of the `width`) to the `track_layout` in both directions (left and right). 25 | 26 | Now, we define the **search space** of the algorithm, namely the sectors. Sectors are equally distanced segments that go from the outer border to inner border of the track. The points through which the racing line passes, will move along these segments: 27 | 28 | ![](imgs/Sectors.png) 29 | 30 | ### 3. Compute racing line 31 | 32 | To find the racing line, the algorithm will fit a cubic spline to the sector points. The vehicles's speed at each point $i$ of the racing line is computed as: 33 | 34 | $$ v_i = \sqrt{\mu * r_i * 9.81} $$ 35 | 36 | where $\mu$ is the coeffcient of friction (set to $0.13$) and $r$ is the radius of curvature which is computed as: 37 | 38 | $$ r = \frac{1}{k} \quad \text{with} \quad k = \frac{|x'y''-y'x''|}{(x'^2+y'^2)^{3/2}} $$ 39 | 40 | where $x$ and $y$ are the coordinates of each point of the spline. 41 | 42 | The algorithm's **objective** is to compute the fastest racing line around the track based on the laptime. Having the speed and the distance between each pair of points computed by the spline, we can compute the laptime. 43 | 44 | ## Run the algorithm 45 | 46 | Run the `main.py` script to see the optimizer work. Inside the main function you will have the possibility to change the hyper-parameters of the PSO algorithm. 47 | 48 | ![](imgs/RacingLineEvolution.gif) 49 | 50 | ![](imgs/LapTimeEvolution.png) 51 | 52 | ## License 53 | 54 | This project is under the MIT license. See [LICENSE](https://github.com/ParsaD23/Racing-Line-Optimization-with-PSO/blob/master/LICENSE) for more information. 55 | -------------------------------------------------------------------------------- /data/tracks.json: -------------------------------------------------------------------------------- 1 | { 2 | "test_track": { 3 | "layout": [[218, 57], [219, 55], [221, 54], [223, 53], [224, 52], [226, 51], [228, 50], [230, 49], [232, 49],[234, 48], [236, 48], [237, 47], [239, 47], [241, 47], [242, 47], [244, 47], [245, 47], [247, 48], [248, 48], [249, 49], [250, 50], [251, 51], [251, 52], [252, 53], [252, 54], [252, 56], [252, 57], [253, 59], [253, 61],[252, 62], [252, 64], [252, 66], [252, 68], [252, 70], [251, 72], [251, 74], [251, 76], [250, 78], [250, 80],[250, 81], [249, 83], [249, 85], [249, 87], [249, 89], [249, 90], [249, 92], [248, 94], [248, 95], [248, 97],[248, 98], [248, 100], [248, 102], [248, 103], [249, 105], [249, 106], [249, 108], [249, 110], [249, 111],[249, 113], [249, 115], [249, 116], [250, 118], [250, 120], [250, 122], [250, 124], [250, 126], [251, 128],[251, 130], [251, 132], [251, 134], [252, 136], [252, 138], [252, 140], [252, 142], [252, 144], [253, 147],[253, 149], [253, 151], [253, 153], [253, 155], [253, 157], [254, 160], [254, 162], [254, 164], [254, 166],[254, 168], [254, 170], [254, 173], [254, 175], [254, 177], [254, 179], [254, 181], [254, 183], [254, 185],[254, 187], [254, 189], [254, 191], [254, 193], [254, 195], [254, 197], [253, 198], [253, 200], [253, 202], [253, 204], [252, 205], [252, 207], [251, 208], [251, 210], [250, 211], [250, 213], [249, 214], [249, 215], [248, 216], [248, 218], [247, 219], [246, 220], [245, 221], [244, 221], [244, 222], [243, 223], [242, 224], [241, 224], [240, 225], [239, 225], [237, 225], [236, 225], [235, 225], [234, 226], [232, 225], [231, 225], [229, 225], [228, 225], [226, 224], [225, 224], [223, 223], [221, 223], [220, 222], [218, 221], [216, 221], [214, 220], [212, 219], [211, 218], [209, 217], [207, 216], [205, 215], [203, 214], [201, 213], [199, 212], [197, 211], [195, 210], [194, 209], [192, 208], [190, 207], [188, 206], [186, 205], [184, 204], [182, 203], [180, 202], [178, 201], [177, 200], [175, 199], [173, 198], [171, 197], [169, 196], [168, 196], [166, 195], [164, 195], [163, 194], [161, 194], [160, 193], [158, 193], [157, 193], [156, 193], [154, 192], [153, 192], [152, 193], [151, 193], [150, 193], [149, 194], [148, 194], [147, 195], [146, 196], [145, 196], [145, 197], [144, 199], [143, 200], [143, 201], [143, 203], [142, 204], [142, 206], [142, 208], [141, 210], [141, 211], [141, 213], [141, 215], [141, 217], [140, 219], [140, 221], [140, 223], [140, 225], [140, 227], [139, 229], [139, 231], [139, 233], [138, 235], [138, 237], [137, 238], [137, 240], [136, 241], [135, 243], [135, 244], [134, 245], [133, 246], [131, 247], [130, 248], [129, 248], [127, 249], [126, 249], [124, 249], [122, 250], [120, 250], [119, 250], [117, 250], [115, 250], [113, 251], [112, 251], [110, 251], [108, 251], [106, 252], [105, 252], [103, 252], [101, 252], [99, 252], [98, 253], [96, 253], [94, 253], [92, 253], [90, 253], [89, 254], [87, 254], [85, 254], [84, 254], [82, 254], [80, 254], [78, 254], [77, 254], [75, 254], [73, 254], [72, 254], [70, 254], [68, 254], [67, 254], [65, 254], [64, 254], [62, 253], [61, 253], [59, 253], [58, 252], [56, 252], [55, 252], [53, 251], [52, 251], [51, 250], [49, 249], [48, 249], [47, 248], [45, 247], [44, 246], [43, 246], [42, 245], [41, 244], [39, 243], [38, 242], [37, 241], [36, 239], [35, 238], [34, 237], [33, 236], [32, 235], [31, 233], [30, 232], [30, 231], [29, 229], [28, 228], [27, 226], [26, 225], [26, 223], [25, 222], [24, 220], [24, 219], [23, 217], [22, 215], [22, 214], [21, 212], [21, 210], [20, 208], [20, 207], [19, 205], [19, 203], [18, 201], [18, 199], [18, 197], [17, 195], [17, 193], [17, 192], [17, 190], [16, 188], [16, 186], [16, 184], [16, 182], [16, 180], [16, 178], [16, 176], [16, 174], [16, 172], [16, 170], [16, 168], [16, 166], [16, 163], [16, 161], [16, 159], [16, 157], [16, 155], [17, 153], [17, 151], [17, 149], [17, 147], [18, 145], [18, 143], [19, 141], [19, 139], [19, 137], [20, 135], [20, 133], [21, 131], [21, 129], [22, 127], [22, 125], [23, 123], [24, 121], [24, 119], [25, 118], [26, 116], [27, 114], [27, 112], [28, 110], [29, 108], [30, 107], [31, 105], [32, 103], [32, 101], [33, 100], [34, 98], [35, 97], [36, 95], [37, 93], [39, 92], [40, 90], [41, 89], [42, 87],[43, 86], [44, 85], [46, 83], [47, 82], [48, 81], [49, 80], [51, 78], [52, 77], [53, 76], [55, 75], [56, 74], [58, 73], [59, 72], [61, 71], [62, 71], [64, 70], [65, 69], [67, 68], [68, 68], [70, 67], [72, 67], [73, 66], [75, 66], [76, 66], [78, 66], [80, 65], [81, 65], [83, 65], [85, 66], [87, 66], [88, 66], [90, 66], [92, 67], [93, 67], [95, 68], [97, 68], [99, 69], [100, 70], [102, 70], [104, 71], [106, 72], [107, 73], [109, 74], [111, 75], [113, 76], [114, 77], [116, 78], [118, 79], [119, 80], [121, 81], [123, 83], [125, 84], [126, 85], [128, 86], [130, 88], [131, 89], [133, 90], [135, 92], [136, 93], [138, 94], [140, 96], [141, 97], [143, 98], [144, 99], [146, 101], [147, 102], [149, 103], [150, 104], [152, 106], [153, 107], [155, 108], [156, 109], [158, 110], [159, 111], [161, 112], [162, 113], [163, 114], [165, 115], [166, 116], [167, 117], [168, 117], [170, 118], [171, 119], [172, 119], [173, 120], [174, 120], [175, 120], [176, 121], [177, 121], [178, 121], [179, 121], [180, 121], [181, 121], [182, 121], [183, 120], [184, 120], [184, 120], [185, 119], [186, 118], [186, 118], [187, 117], [188, 116], [188, 115], [189, 114], [189, 113], [190, 111], [190, 110], [191, 109], [191, 107], [192, 106], [192, 104], [193, 103], [193, 101], [194, 100], [194, 98], [195, 96], [195, 95], [196, 93], [197, 91], [197, 89], [198, 88], [198, 86], [199, 84], [200, 82], [200, 80], [201, 79], [202, 77], [203, 75], [204, 73], [205, 72], [206, 70], [207, 68], [208, 67], [209, 65], [210, 64], [212, 62], [213, 61], [214, 59], [216, 58], [218, 56]], 4 | "width": 20 5 | } 6 | } -------------------------------------------------------------------------------- /pso/pso.py: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------------------+ 2 | # 3 | # Parsa Dahesh (dinoco.parsa23@gmail.com or parsa.dahesh@studio.unibo.it) 4 | # Racing Line Optimization with PSO 5 | # MIT License, Copyright (c) 2021 Parsa Dahesh 6 | # 7 | #------------------------------------------------------------------------------+ 8 | 9 | import random 10 | import time 11 | 12 | class Particle: 13 | def __init__(self, n_dimensions, boundaries): 14 | self.position = [] 15 | self.best_position = [] 16 | self.velocity = [] 17 | 18 | for i in range(n_dimensions): 19 | self.position.append(random.uniform(0, boundaries[i])) 20 | self.velocity.append(random.uniform(-boundaries[i], boundaries[i])) 21 | 22 | self.best_position = self.position 23 | 24 | def update_position(self, newVal): 25 | self.position = newVal 26 | 27 | def update_best_position(self, newVal): 28 | self.best_position = newVal 29 | 30 | def update_velocity(self, newVal): 31 | self.velocity = newVal 32 | 33 | def optimize(cost_func, n_dimensions, boundaries, n_particles, n_iterations, w, cp, cg, verbose=False): 34 | ''' Particle Swarm Optimization 35 | 36 | This function will minimize the cost function 37 | 38 | Parameters 39 | ---------- 40 | cost_func : function 41 | A function that will evaluate a given input, return a float value 42 | n_dimension : int 43 | Dimensionality of the problem 44 | boundaries : list[float] 45 | Problem's search space boundaries 46 | n_particles : int 47 | Number of particles 48 | n_iteration : int 49 | Number of iterations 50 | w : float 51 | Inertia parameter 52 | cp : float 53 | Constant parameter influencing the cognitive component (how much the current particle's best position will influnce its next iteration) 54 | cg : float 55 | Constant parameter influencing the social component (how much the global solution will influnce its next iteration of a particle) 56 | verbose : bool 57 | Flag to turn on output prints (default is False) 58 | 59 | Returns 60 | ------- 61 | global_solution : 62 | Solution of the optimization 63 | gs_eval : 64 | Evaluation of global_solution with cost_func 65 | gs_history : 66 | List of the global solution at each iteration of the algorithm 67 | gs_eval_history : 68 | List of the global solution's evaluation at each iteration of the algorithm 69 | ''' 70 | 71 | particles = [] 72 | global_solution = [] 73 | gs_eval = [] 74 | gs_history = [] 75 | gs_eval_history = [] 76 | 77 | if verbose: 78 | print() 79 | print("------------------ PARAMETERS -----------------") 80 | print("Number of dimensions:", n_dimensions) 81 | print("Number of iterations:", n_iterations) 82 | print("Number of particles:", n_particles) 83 | print("w: {}\tcp: {}\tcg: {}".format(w,cp,cg)) 84 | print() 85 | print("----------------- OPTIMIZATION ----------------") 86 | print("Population initialization...") 87 | 88 | for i in range(n_particles): 89 | particles.append(Particle(n_dimensions, boundaries)) 90 | 91 | global_solution = particles[0].position 92 | gs_eval = cost_func(global_solution) 93 | for p in particles: 94 | p_eval = cost_func(p.best_position) 95 | if p_eval < gs_eval: 96 | global_solution = p.best_position 97 | gs_eval = cost_func(global_solution) 98 | 99 | gs_history.append(global_solution) 100 | gs_eval_history.append(gs_eval) 101 | 102 | if verbose: 103 | print("Start of optimization...") 104 | printProgressBar(0, n_iterations, prefix = 'Progress:', suffix = 'Complete', length = 50) 105 | 106 | start_time = time.time_ns() 107 | 108 | for k in range(n_iterations): 109 | for p in particles: 110 | rp = random.uniform(0,1) 111 | rg = random.uniform(0,1) 112 | 113 | velocity = [] 114 | new_position =[] 115 | for i in range(n_dimensions): 116 | velocity.append(w * p.velocity[i] + \ 117 | cp * rp * ( p.best_position[i] - p.position[i] ) + \ 118 | cg * rg * ( global_solution[i] - p.position[i] )) 119 | 120 | if velocity[i] < -boundaries[i]: 121 | velocity[i] = -boundaries[i] 122 | elif velocity[i] > boundaries[i]: 123 | velocity[i] = boundaries[i] 124 | 125 | new_position.append(p.position[i] + velocity[i]) 126 | if new_position[i] < 0.0: 127 | new_position[i] = 0.0 128 | elif new_position[i] > boundaries[i]: 129 | new_position[i] = boundaries[i] 130 | 131 | p.update_velocity(velocity) 132 | p.update_position(new_position) 133 | 134 | p_eval = cost_func(p.position) 135 | if p_eval < cost_func(p.best_position): 136 | p.update_best_position(p.position) 137 | if p_eval < gs_eval: 138 | global_solution = p.position 139 | gs_eval = p_eval 140 | 141 | gs_eval_history.append(gs_eval) 142 | gs_history.append(global_solution) 143 | 144 | if verbose: 145 | printProgressBar(k+1, n_iterations, prefix = 'Progress:', suffix = 'Complete', length = 50) 146 | 147 | finish_time = time.time_ns() 148 | elapsed_time = (finish_time-start_time)/10e8 149 | 150 | if verbose: 151 | time.sleep(0.2) 152 | print("End of optimization...") 153 | print() 154 | print("------------------- RESULTS -------------------") 155 | print("Optimization elapsed time: {:.2f} s".format(elapsed_time)) 156 | print("Solution evaluation: {:.5f}".format(gs_eval)) 157 | 158 | return global_solution, gs_eval, gs_history, gs_eval_history 159 | 160 | def printProgressBar (iteration, total, prefix = '', suffix = '', decimals = 1, length = 100, fill = '█', printEnd = "\r"): 161 | """ 162 | Call in a loop to create terminal progress bar 163 | @params: 164 | iteration - Required : current iteration (Int) 165 | total - Required : total iterations (Int) 166 | prefix - Optional : prefix string (Str) 167 | suffix - Optional : suffix string (Str) 168 | decimals - Optional : positive number of decimals in percent complete (Int) 169 | length - Optional : character length of bar (Int) 170 | fill - Optional : bar fill character (Str) 171 | printEnd - Optional : end character (e.g. "\r", "\r\n") (Str) 172 | """ 173 | percent = ("{0:." + str(decimals) + "f}").format(100 * (iteration / float(total))) 174 | filledLength = int(length * iteration // total) 175 | bar = fill * filledLength + '-' * (length - filledLength) 176 | print(f'\r{prefix} |{bar}| {percent}% {suffix}', end = printEnd) 177 | # Print New Line on Complete 178 | if iteration == total: 179 | print() 180 | -------------------------------------------------------------------------------- /pso/main.py: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------------------+ 2 | # 3 | # Parsa Dahesh (dinoco.parsa23@gmail.com or parsa.dahesh@studio.unibo.it) 4 | # Racing Line Optimization with PSO 5 | # MIT License, Copyright (c) 2021 Parsa Dahesh 6 | # 7 | #------------------------------------------------------------------------------+ 8 | 9 | import json 10 | import math 11 | import matplotlib.pyplot as plt 12 | import numpy as np 13 | 14 | import pso 15 | 16 | from scipy import interpolate 17 | from shapely.geometry import LineString 18 | from utils import plot_lines, get_closet_points 19 | 20 | def main(): 21 | # PARAMETERS 22 | N_SECTORS = 40 23 | N_PARTICLES = 60 24 | N_ITERATIONS = 150 25 | W = -0.2256 26 | CP = -0.1564 27 | CG = 3.8876 28 | PLOT = True 29 | 30 | # Read tracks from json file 31 | json_data = None 32 | with open('data/tracks.json') as file: 33 | json_data = json.load(file) 34 | 35 | track_layout = json_data['test_track']['layout'] 36 | track_width = json_data['test_track']['width'] 37 | 38 | # Compute inner and outer tracks borders 39 | center_line = LineString(track_layout) 40 | inside_line = LineString(center_line.parallel_offset(track_width/2, 'left')) 41 | outside_line = LineString(center_line.parallel_offset(track_width/2, 'right')) 42 | 43 | if PLOT: 44 | plt.title("Track Layout Points") 45 | for p in track_layout: 46 | plt.plot(p[0], p[1], 'r.') 47 | plt.show() 48 | plt.title("Track layout") 49 | plot_lines([outside_line, inside_line]) 50 | plt.show() 51 | 52 | # Define sectors' extreme points (in coordinates). Each sector segment is defined by an inside point and an outside point. 53 | inside_points, outside_points = define_sectors(center_line, inside_line, outside_line, N_SECTORS) 54 | 55 | if PLOT: 56 | plt.title("Sectors") 57 | for i in range(N_SECTORS): 58 | plt.plot([inside_points[i][0], outside_points[i][0]], [inside_points[i][1], outside_points[i][1]]) 59 | plot_lines([outside_line, inside_line]) 60 | plt.show() 61 | 62 | # Define the boudaries that will be passed to the pso algorithm [0 - trackwidth]. 63 | # 0 correspond to the inner border, trackwidth to the outer border. 64 | boundaries = [] 65 | for i in range(N_SECTORS): 66 | boundaries.append(np.linalg.norm(inside_points[i]-outside_points[i])) 67 | 68 | def myCostFunc(sectors): 69 | return get_lap_time(sectors_to_racing_line(sectors, inside_points, outside_points)) 70 | 71 | global_solution, gs_eval, gs_history, gs_eval_history = pso.optimize(cost_func = myCostFunc, 72 | n_dimensions=N_SECTORS, 73 | boundaries=boundaries, 74 | n_particles=N_PARTICLES, 75 | n_iterations=N_ITERATIONS, 76 | w=W, cp=CP, cg=CG, 77 | verbose=True) 78 | 79 | _, v, x, y = get_lap_time(sectors_to_racing_line(global_solution, inside_points, outside_points), return_all=True) 80 | 81 | if PLOT: 82 | plt.title("Racing Line History") 83 | plt.ion() 84 | for i in range(0, len(np.array(gs_history)), int(N_ITERATIONS/100)): 85 | lth, vh, xh, yh = get_lap_time(sectors_to_racing_line(gs_history[i], inside_points, outside_points), return_all=True) 86 | plt.scatter(xh, yh, marker='.', c = vh, cmap='RdYlGn') 87 | plot_lines([outside_line, inside_line]) 88 | plt.draw() 89 | plt.pause(0.00000001) 90 | plt.clf() 91 | plt.ioff() 92 | 93 | plt.title("Final racing line") 94 | rl = np.array(sectors_to_racing_line(global_solution, inside_points, outside_points)) 95 | plt.plot(rl[:,0], rl[:,1], 'ro') 96 | plt.scatter(x, y, marker='.', c = v, cmap='RdYlGn') 97 | for i in range(N_SECTORS): 98 | plt.plot([inside_points[i][0], outside_points[i][0]], [inside_points[i][1], outside_points[i][1]]) 99 | plot_lines([outside_line, inside_line]) 100 | plt.show() 101 | 102 | plt.title("Global solution history") 103 | plt.ylabel("lap_time (s)") 104 | plt.xlabel("n_iteration") 105 | plt.plot(gs_eval_history) 106 | plt.show() 107 | 108 | def sectors_to_racing_line(sectors:list, inside_points:list, outside_points:list): 109 | '''Sectors to racing line coordinate 110 | 111 | This function converts the sector values (numeric value from 0 to the track's width) to cartesian coordinates using a parametric function. 112 | 113 | Parameters 114 | ---------- 115 | sectors : list 116 | Position value of the sector inside the sector segment 117 | inside_points : list 118 | List coordinates corresponding to the internal point of each sector segment 119 | outside_points : list 120 | List coordinates corresponding to the external point of each sector segment 121 | 122 | Returns 123 | ------- 124 | racing_line : list 125 | List of coordinates corresponding to the sectors' position 126 | ''' 127 | 128 | racing_line = [] 129 | for i in range(len(sectors)): 130 | x1, y1 = inside_points[i][0], inside_points[i][1] 131 | x2, y2 = outside_points[i][0], outside_points[i][1] 132 | m = (y2-y1)/(x2-x1) 133 | 134 | a = math.cos(math.atan(m)) # angle with x axis 135 | b = math.sin(math.atan(m)) # angle with x axis 136 | 137 | xp = x1 - sectors[i]*a 138 | yp = y1 - sectors[i]*b 139 | 140 | if abs(math.dist(inside_points[i], [xp,yp])) + abs(math.dist(outside_points[i], [xp,yp])) - \ 141 | abs(math.dist(outside_points[i], inside_points[i])) > 0.1: 142 | xp = x1 + sectors[i]*a 143 | yp = y1 + sectors[i]*b 144 | 145 | racing_line.append([xp, yp]) 146 | return racing_line 147 | 148 | def get_lap_time(racing_line:list, return_all=False): 149 | '''Fitness function 150 | 151 | Computes the laptime given a racing line made of sector points. 152 | This function computes a racing line passing through the sector points and compute the vehicle speed in each point of the racing line. 153 | 154 | Parameters 155 | ---------- 156 | racing_line : array 157 | Racing line in sector points 158 | return_all : boolean 159 | A flag to return the optional values (default is False) 160 | 161 | Returns 162 | ------- 163 | lap_time : float 164 | Lap time in seconds 165 | v : list[float], optional 166 | Speed value for each point in the computed racing line 167 | x : int, optional 168 | x coordinate for each point of the computed racing line 169 | y : int, optional 170 | y coordinate for each point of the computed racing line 171 | ''' 172 | # Computing the spline function passing throught the racing_line points 173 | rl = np.array(racing_line) 174 | tck, _ = interpolate.splprep([rl[:,0], rl[:,1]], s=0.0, per=0) 175 | x, y = interpolate.splev(np.linspace(0, 1, 1000), tck) 176 | 177 | # Computing derivatives 178 | dx, dy = np.gradient(x), np.gradient(y) 179 | d2x, d2y = np.gradient(dx), np.gradient(dy) 180 | 181 | curvature = np.abs(dx * d2y - d2x * dy) / (dx * dx + dy * dy)**1.5 182 | radius = [1/c for c in curvature] 183 | 184 | us = 0.13 # Coefficient of friction 185 | 186 | # Computing speed at each point 187 | v = [min(10,math.sqrt(us*r*9.81)) for r in radius] 188 | 189 | # Computing lap time around the track 190 | lap_time = sum( [math.sqrt((x[i]-x[i+1])**2 + (y[i]-y[i+1])**2)/v[i] for i in range(len(x)-1)]) 191 | 192 | if return_all: 193 | return lap_time, v, x, y 194 | return lap_time 195 | 196 | def define_sectors(center_line : LineString, inside_line : LineString, outside_line : LineString, n_sectors : int): 197 | '''Defines sectors' search space 198 | 199 | Parameters 200 | ---------- 201 | center_line : LineString 202 | Center line of the track 203 | inside_line : LineString 204 | Inside line of the track 205 | outside_line : LineString 206 | Outside line of the track 207 | n_secotrs : int 208 | Number of sectors 209 | 210 | Returns 211 | ------- 212 | inside_points : list 213 | List coordinates corresponding to the internal point of each sector segment 214 | outside_points : list 215 | List coordinates corresponding to the external point of each sector segment 216 | ''' 217 | 218 | distances = np.linspace(0, center_line.length, n_sectors) 219 | center_points_temp = [center_line.interpolate(distance) for distance in distances] 220 | center_points = np.array([[center_points_temp[i].x, center_points_temp[i].y] for i in range(len(center_points_temp)-1)]) 221 | center_points = np.append(center_points, [center_points[0]], axis=0) 222 | 223 | distances = np.linspace(0,inside_line.length, 1000) 224 | inside_border = [inside_line.interpolate(distance) for distance in distances] 225 | inside_border = np.array([[e.x, e.y] for e in inside_border]) 226 | inside_points = np.array([get_closet_points([center_points[i][0],center_points[i][1]], inside_border) for i in range(len(center_points))]) 227 | 228 | distances = np.linspace(0,outside_line.length, 1000) 229 | outside_border = [outside_line.interpolate(distance) for distance in distances] 230 | outside_border = np.array([[e.x, e.y] for e in outside_border]) 231 | outside_points = np.array([get_closet_points([center_points[i][0],center_points[i][1]], outside_border) for i in range(len(center_points))]) 232 | 233 | return inside_points, outside_points 234 | 235 | if __name__ == "__main__": 236 | main() 237 | --------------------------------------------------------------------------------