├── .DS_Store ├── .gitignore ├── Data ├── TAL_vs_baseline_reward.png ├── animation.gif ├── animation.webp ├── tal_calculation.png └── tal_progress.png ├── LICENSE.md ├── README.md ├── TrajectoryAidedLearning ├── DataTools │ ├── AnalysePerformance │ │ ├── CalculateAverages.py │ │ ├── CalculateAveragesMaps.py │ │ ├── CalculateStatistics.py │ │ ├── CalculateStatisticsMaps.py │ │ ├── GenerateBarPlots.py │ │ └── GenerateVelocityProfiles.py │ ├── MapData.py │ ├── PlotPerformance │ │ ├── CthVsProgress_Barplot.py │ │ ├── Cth_speeds_Barplot.py │ │ ├── LiteratureComparison.py │ │ ├── TAL_Cth_maps_Barplot.py │ │ ├── TAL_Cth_maps_BarplotMaps.py │ │ ├── TAL_Cth_speeds_Barplot.py │ │ ├── TAL_pp_maps_Barplot.py │ │ └── TAL_speeds_AvgProgress.py │ ├── ProfileAnalysis │ │ ├── Cth_speeds_Profiles.py │ │ └── TAL_speedProfiles.py │ ├── TrainingGraphs │ │ ├── CthVsProgress_TrainingGraph.py │ │ ├── Cth_TAL_maps_RewardTrainingGraph.py │ │ ├── Cth_TAL_speeds_TrainingAnimation.py │ │ ├── Cth_TAL_speeds_TrainingGraph.py │ │ ├── Cth_speeds_TrainingGraph.py │ │ ├── TAL_Cth_maps_Training.py │ │ ├── TAL_maps_TrainingGraphs.py │ │ ├── TAL_speeds_TrainingGraph.py │ │ └── TrainingUtils.py │ └── plotting_utils.py ├── Planners │ ├── AgentPlanners.py │ ├── PurePursuit.py │ └── __init__.py ├── TestSimulation.py ├── TestSimulationMaps.py ├── TrainAgents.py ├── Utils │ ├── HistoryStructs.py │ ├── RacingTrack.py │ ├── RewardSignals.py │ ├── RewardUtils.py │ ├── StdTrack.py │ ├── TD3.py │ ├── __init__.py │ └── utils.py ├── __init__.py └── f110_gym │ ├── __init__.py │ ├── base_classes.py │ ├── collision_models.py │ ├── dynamic_models.py │ ├── f110_env.py │ ├── laser_models.py │ └── rendering.py ├── config ├── CthVsProgress.yaml ├── Cth_maps.yaml ├── Cth_maps2.yaml ├── Cth_speedMaps.yaml ├── Cth_speeds.yaml ├── PP_maps8.yaml ├── PP_speeds.yaml ├── TAL_maps.yaml ├── TAL_maps2.yaml ├── TAL_speeds.yaml └── config_file.yaml ├── maps ├── .gitkeep ├── f1_aut.png ├── f1_aut.yaml ├── f1_aut_centerline.csv ├── f1_aut_raceline.csv ├── f1_aut_wide.png ├── f1_aut_wide.yaml ├── f1_aut_wide_centerline.csv ├── f1_aut_wide_raceline.csv ├── f1_esp.png ├── f1_esp.yaml ├── f1_esp_centerline.csv ├── f1_esp_raceline.csv ├── f1_gbr.png ├── f1_gbr.yaml ├── f1_gbr_centerline.csv ├── f1_gbr_raceline.csv ├── f1_mco.png ├── f1_mco.yaml ├── f1_mco_centerline.csv └── f1_mco_raceline.csv └── setup.py /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/.DS_Store -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Django stuff: 58 | *.log 59 | local_settings.py 60 | db.sqlite3 61 | db.sqlite3-journal 62 | 63 | # Flask stuff: 64 | instance/ 65 | .webassets-cache 66 | 67 | # Scrapy stuff: 68 | .scrapy 69 | 70 | # Sphinx documentation 71 | docs/_build/ 72 | 73 | # PyBuilder 74 | target/ 75 | 76 | # Jupyter Notebook 77 | .ipynb_checkpoints 78 | 79 | # IPython 80 | profile_default/ 81 | ipython_config.py 82 | 83 | # pyenv 84 | .python-version 85 | 86 | # pipenv 87 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 88 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 89 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 90 | # install all needed dependencies. 91 | #Pipfile.lock 92 | 93 | # celery beat schedule file 94 | celerybeat-schedule 95 | 96 | # SageMath parsed files 97 | *.sage.py 98 | 99 | # Environments 100 | .env 101 | .venv 102 | env/ 103 | venv/ 104 | ENV/ 105 | env.bak/ 106 | venv.bak/ 107 | 108 | # Spyder project settings 109 | .spyderproject 110 | .spyproject 111 | 112 | # Rope project settings 113 | .ropeproject 114 | 115 | # mkdocs documentation 116 | /site 117 | 118 | # mypy 119 | .mypy_cache/ 120 | .dmypy.json 121 | dmypy.json 122 | 123 | # Pyre type checker 124 | .pyre/ 125 | 126 | Data/Vehicles/ 127 | 128 | Data/Images/ 129 | 130 | 131 | Data/UploadImgs2/ 132 | -------------------------------------------------------------------------------- /Data/TAL_vs_baseline_reward.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/Data/TAL_vs_baseline_reward.png -------------------------------------------------------------------------------- /Data/animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/Data/animation.gif -------------------------------------------------------------------------------- /Data/animation.webp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/Data/animation.webp -------------------------------------------------------------------------------- /Data/tal_calculation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/Data/tal_calculation.png -------------------------------------------------------------------------------- /Data/tal_progress.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/Data/tal_progress.png -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2022 BDEvan5 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TrajectoryAidedLearning 2 | 3 | This repo contains the source code for the paper entitled, "[High-speed Autonomous Racing using Trajectory-aided Deep Reinforcement Learning](https://ieeexplore.ieee.org/document/10182327)" 4 | 5 | We present a reward signal that incorporates an optimal trajectory to train deep reinforcement learning agents for high-speed autonomous racing. 6 | 7 | ![](Data/tal_calculation.png) 8 | 9 | Training agents with our reward signal results in significatly improved training performance. 10 | The most noteable performance difference is at high-speeds where previous rewards failed. 11 | 12 | ![](Data/TAL_vs_baseline_reward.png) 13 | 14 | The improved training results in higher average progrresses at high speeds. 15 | 16 | ![](Data/tal_progress.png) 17 | 18 | # Result Generation 19 | 20 | The results in the paper are generated through a two step process of: 21 | 1. Train and test the agents 22 | 2. Process and plot the data 23 | 24 | For every test: 25 | - Run calculate_statistics 26 | - Run calculate_averages 27 | 28 | ## Tests: 29 | 30 | ### Maximum Speed Investigation 31 | 32 | - Aim: Understand how performance changes with different speeds. 33 | - Config files: CthSpeeds, TAL_speeds 34 | - Results: 35 | - Training graph: Cth_TAL_speeds_TrainingGraph 36 | - Lap times and % success: Cth_TAL_speeds_Barplot 37 | 38 | ### 6 m/s Performance Comparision 39 | 40 | - Aim: Compare the baseline and TAL on different maps with a maximum speed of 6 m/s. 41 | - Config file: Cth_maps, TAL_maps 42 | - Results: 43 | - Training graphs: TAL_Cth_maps_TrainingGraph 44 | - Lap times and success bar plot: TAL_Cth_maps_Barplot 45 | 46 | ### Speed Profile Analysis 47 | 48 | - Aim: Study the speed profiles 49 | - Requires the pure pursuit (PP_speeds) results 50 | - Results: 51 | - Trajectories: GenerateVelocityProfiles, set the folder to TAL_speeds 52 | - Speed profile pp TAL: TAL_speed_profiles 53 | - Speed profile x3: TAL_speed_profiles 54 | - Slip profile: TAL_speed_profiles 55 | 56 | ### Comparison with Literatures 57 | 58 | - Aim: Compare our method with the literature 59 | - Results: 60 | - Bar plot: LiteratureComparison 61 | - Note that the results from the literature are hard coded. 62 | 63 | ![](Data/animation.gif) 64 | 65 | 66 | ## Citation 67 | 68 | If you find this work useful, please consider citing: 69 | ``` 70 | @ARTICLE{10182327, 71 | author={Evans, Benjamin David and Engelbrecht, Herman Arnold and Jordaan, Hendrik Willem}, 72 | journal={IEEE Robotics and Automation Letters}, 73 | title={High-Speed Autonomous Racing Using Trajectory-Aided Deep Reinforcement Learning}, 74 | year={2023}, 75 | volume={8}, 76 | number={9}, 77 | pages={5353-5359}, 78 | doi={10.1109/LRA.2023.3295252} 79 | } 80 | ``` -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/CalculateAverages.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | import glob 4 | import os 5 | 6 | 7 | class VehicleData: 8 | def __init__(self, vehicle_id, n=3, prefix="Data/Vehicles/Cth_speedMaps/"): 9 | self.vehicle_id = vehicle_id 10 | self.prefix = prefix = prefix 11 | 12 | self.times = [] 13 | self.success_rates = [] 14 | self.avg_progresses = [] 15 | 16 | 17 | for i in range(n): 18 | self.process_folder(vehicle_id, i) 19 | 20 | self.save_data() 21 | 22 | def process_folder(self, name, n): 23 | folder = self.prefix + name + "_" + str(n) 24 | 25 | #open summary stats 26 | try: 27 | with open(f"{folder}/SummaryStatistics.txt", 'r') as file: 28 | lines = file.readlines() 29 | line = lines[2] # first lap is heading 30 | line = line.split(',') 31 | 32 | time = float(line[8]) 33 | if not np.isnan(time): 34 | self.times.append(time) 35 | success = float(line[12]) 36 | self.success_rates.append(success) 37 | 38 | avg_progress = float(line[7]) 39 | self.avg_progresses.append(avg_progress) 40 | except: 41 | print(f"File not opened: {folder}") 42 | 43 | def save_data(self): 44 | functions = [np.mean, np.std, np.amin, np.amax] 45 | names = ["Mean", "Std", "Min", "Max"] 46 | 47 | times = np.array(self.times) 48 | success_rates = np.array(self.success_rates) 49 | progresses = np.array(self.avg_progresses) 50 | 51 | 52 | with open(self.prefix + "Results_" + self.vehicle_id + ".txt", 'w') as file: 53 | file.write(f"Metric , Time , Success Rate , Avg Progress \n") 54 | for i in range(len(names)): 55 | file.write(f"{names[i]}".ljust(10)) 56 | if len(times) == 1: 57 | file.write(f", {times[0]:14.4f}") 58 | file.write(f", {success_rates[0]:14.4f}") 59 | elif len(times) == 0: 60 | file.write(f", nan".rjust(14)) 61 | file.write(f", nan".rjust(14)) 62 | else: 63 | file.write(f", {functions[i](times):14.4f}") 64 | file.write(f", {functions[i](success_rates):14.4f}") 65 | 66 | file.write(f", {functions[i](progresses):14.4f} \n") 67 | 68 | 69 | 70 | 71 | 72 | 73 | def aggregate_runs(path): 74 | vehicle_folders = glob.glob(f"{path}*/") 75 | vehicle_folders.sort() 76 | 77 | print(f"{len(vehicle_folders)} folders found") 78 | 79 | id_list = [] 80 | for j, folder in enumerate(vehicle_folders): 81 | print(f"Vehicle folder being opened: {folder}") 82 | 83 | vehicle_name = folder.split("/")[-2] 84 | vehicle_id = vehicle_name[:-2] 85 | print(vehicle_id) 86 | 87 | if not vehicle_id in id_list:# and not vehicle_id[0:2] == "PP": 88 | id_list.append(vehicle_id) 89 | 90 | for i in range(len(id_list)): 91 | v = VehicleData(id_list[i], n=5, prefix=path) 92 | # v = VehicleData(id_list[i], n=3, prefix=path) 93 | 94 | 95 | 96 | 97 | # aggregate_runs("Data/Vehicles/Cth_speedMaps/") 98 | # aggregate_runs("Data/Vehicles/Cth_speeds/") 99 | # aggregate_runs("Data/Vehicles/TAL_speeds/") 100 | # aggregate_runs("Data/Vehicles/CthVsProgress/") 101 | 102 | # aggregate_runs("Data/Vehicles/TAL_maps/") 103 | # aggregate_runs("Data/Vehicles/TAL_maps8/") 104 | aggregate_runs("Data/Vehicles/PP_maps8/") 105 | # aggregate_runs("Data/Vehicles/Cth_maps/") -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/CalculateAveragesMaps.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | import glob 4 | import os 5 | 6 | 7 | class VehicleData: 8 | def __init__(self, vehicle_id, n=3, prefix="Data/Vehicles/Cth_speedMaps/"): 9 | self.vehicle_id = vehicle_id 10 | self.prefix = prefix = prefix 11 | 12 | map_names = ["f1_aut", "f1_esp", "f1_gbr", "f1_mco"] 13 | for map_name in map_names: 14 | 15 | self.times = [] 16 | self.success_rates = [] 17 | self.avg_progresses = [] 18 | 19 | for i in range(n): 20 | self.process_folder(vehicle_id, i, map_name) 21 | 22 | self.save_data(map_name) 23 | 24 | def process_folder(self, name, n, map_name): 25 | folder = self.prefix + name + "_" + str(n) 26 | 27 | #open summary stats 28 | try: 29 | with open(f"{folder}/SummaryStatistics{map_name[-3:].upper()}.txt", 'r') as file: 30 | lines = file.readlines() 31 | line = lines[2] # first lap is heading 32 | line = line.split(',') 33 | 34 | time = float(line[8]) 35 | if not np.isnan(time): 36 | self.times.append(time) 37 | success = float(line[12]) 38 | self.success_rates.append(success) 39 | 40 | avg_progress = float(line[7]) 41 | self.avg_progresses.append(avg_progress) 42 | except: 43 | print(f"File not opened: {folder}") 44 | 45 | def save_data(self, map_name): 46 | functions = [np.mean, np.std, np.amin, np.amax] 47 | names = ["Mean", "Std", "Min", "Max"] 48 | 49 | times = np.array(self.times) 50 | success_rates = np.array(self.success_rates) 51 | progresses = np.array(self.avg_progresses) 52 | 53 | 54 | with open(self.prefix + "Results_" + self.vehicle_id + f"_test{map_name[-3:].upper()}.txt", 'w') as file: 55 | file.write(f"Metric , Time , Success Rate , Avg Progress \n") 56 | for i in range(len(names)): 57 | file.write(f"{names[i]}".ljust(10)) 58 | if len(times) == 1: 59 | file.write(f", {times[0]:14.4f}") 60 | file.write(f", {success_rates[0]:14.4f}") 61 | elif len(times) == 0: 62 | file.write(f", nan".rjust(14)) 63 | file.write(f", nan".rjust(14)) 64 | else: 65 | file.write(f", {functions[i](times):14.4f}") 66 | file.write(f", {functions[i](success_rates):14.4f}") 67 | 68 | file.write(f", {functions[i](progresses):14.4f} \n") 69 | 70 | 71 | 72 | 73 | 74 | 75 | def aggregate_runs(path): 76 | vehicle_folders = glob.glob(f"{path}*/") 77 | vehicle_folders.sort() 78 | 79 | print(f"{len(vehicle_folders)} folders found") 80 | 81 | id_list = [] 82 | for j, folder in enumerate(vehicle_folders): 83 | print(f"Vehicle folder being opened: {folder}") 84 | 85 | vehicle_name = folder.split("/")[-2] 86 | vehicle_id = vehicle_name[:-2] 87 | print(vehicle_id) 88 | 89 | map_name = vehicle_id[-7:-4] 90 | if not vehicle_id in id_list and map_name == "gbr": 91 | # if not vehicle_id in id_list and map_name == "esp": 92 | id_list.append(vehicle_id) 93 | 94 | for i in range(len(id_list)): 95 | v = VehicleData(id_list[i], n=5, prefix=path) 96 | # v = VehicleData(id_list[i], n=3, prefix=path) 97 | 98 | 99 | 100 | 101 | 102 | # aggregate_runs("Data/Vehicles/TAL_maps/") 103 | aggregate_runs("Data/Vehicles/Cth_maps3/") -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/CalculateStatistics.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | import glob 4 | import os 5 | 6 | from PIL import Image 7 | import glob 8 | import trajectory_planning_helpers as tph 9 | from matplotlib.ticker import PercentFormatter 10 | from matplotlib.collections import LineCollection 11 | 12 | from TrajectoryAidedLearning.DataTools.MapData import MapData 13 | from TrajectoryAidedLearning.RewardSignals.StdTrack import StdTrack 14 | from TrajectoryAidedLearning.RewardSignals.RacingTrack import RacingTrack 15 | from TrajectoryAidedLearning.Utils.utils import * 16 | from matplotlib.ticker import MultipleLocator 17 | 18 | # SAVE_PDF = False 19 | SAVE_PDF = True 20 | 21 | 22 | def ensure_path_exists(folder): 23 | if not os.path.exists(folder): 24 | os.makedirs(folder) 25 | 26 | 27 | class AnalyseTestLapData: 28 | def __init__(self): 29 | self.path = None 30 | self.vehicle_name = None 31 | self.map_name = None 32 | self.states = None 33 | self.actions = None 34 | self.map_data = None 35 | self.std_track = None 36 | self.summary_path = None 37 | self.lap_n = 0 38 | 39 | def explore_folder(self, path): 40 | # vehicle_folders = glob.glob(f"{path}PP*/") 41 | vehicle_folders = glob.glob(f"{path}*/") 42 | # vehicle_folders = glob.glob(f"{path}Agent*/") 43 | print(vehicle_folders) 44 | print(f"{len(vehicle_folders)} folders found") 45 | 46 | for j, folder in enumerate(vehicle_folders): 47 | print(f"Vehicle folder being opened: {folder}") 48 | 49 | # if os.path.exists(folder + "Statistics.txt"): 50 | # continue 51 | 52 | self.process_folder(folder) 53 | 54 | def process_folder(self, folder): 55 | self.path = folder 56 | 57 | 58 | with open(self.path + "Statistics.txt", "w") as file: 59 | file.write(f"Name: {self.path}\n") 60 | file.write("Lap" + "Steering".rjust(16) + "Total Distance".rjust(16) + "Mean Curvature".rjust(16) + "Total Curvature".rjust(16) + "Mean Deviation".rjust(16) + "Total Deviation".rjust(16) + "Progress".rjust(16) + "Time".rjust(16) + "Avg Velocity".rjust(16) + "Mean R Deviation".rjust(16) + "Total R Deviation".rjust(16) + "\n") 61 | 62 | self.vehicle_name = self.path.split("/")[-2] 63 | self.map_name = self.vehicle_name.split("_")[4] 64 | if self.map_name == "f1": 65 | self.map_name += "_" + self.vehicle_name.split("_")[5] 66 | self.map_data = MapData(self.map_name) 67 | self.std_track = StdTrack(self.map_name) 68 | self.racing_track = RacingTrack(self.map_name) 69 | 70 | for self.lap_n in range(100): 71 | if not self.load_lap_data(): break # no more laps 72 | self.calculate_lap_statistics() 73 | 74 | self.generate_summary_stats() 75 | 76 | def load_lap_data(self): 77 | try: 78 | data = np.load(self.path + "Testing/" + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 79 | # data = np.load(self.path + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 80 | except Exception as e: 81 | print(e) 82 | print(f"No data for: " + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 83 | return 0 84 | self.states = data[:, :7] 85 | self.actions = data[:, 7:] 86 | 87 | return 1 # to say success 88 | 89 | def calculate_lap_statistics(self): 90 | if not self.load_lap_data(): return 91 | 92 | steering = np.abs(self.actions[:, 0]) 93 | rms_steering = np.mean(np.abs(steering)) 94 | 95 | pts = self.states[:, 0:2] 96 | ss = np.linalg.norm(np.diff(pts, axis=0), axis=1) 97 | total_distance = np.sum(ss) 98 | 99 | ths, ks = tph.calc_head_curv_num.calc_head_curv_num(pts, ss, False) 100 | mean_curvature = np.mean(np.abs(ks)) 101 | total_curvature = np.sum(np.abs(ks)) 102 | 103 | hs = [] 104 | for point in pts: 105 | idx, dists = self.std_track.get_trackline_segment(point) 106 | x, h = self.std_track.interp_pts(idx, dists) 107 | hs.append(h) 108 | 109 | hs = np.array(hs) 110 | mean_deviation = np.mean(hs) 111 | total_deviation = np.sum(hs) 112 | hs = [] 113 | for point in pts: 114 | idx, dists = self.racing_track.get_trackline_segment(point) 115 | x, h = self.racing_track.interp_pts(idx, dists) 116 | hs.append(h) 117 | 118 | hs = np.array(hs) 119 | mean_race_deviation = np.mean(hs) 120 | total_race_deviation = np.sum(hs) 121 | 122 | time = len(pts) /10 123 | vs = self.states[:, 3] 124 | avg_velocity = np.mean(vs) 125 | 126 | progress = self.std_track.calculate_progress(pts[-1])/self.std_track.total_s 127 | if progress < 0.01 or progress > 0.99: 128 | progress = 1 # it is finished 129 | 130 | with open(self.path + "Statistics.txt", "a") as file: 131 | file.write(f"{self.lap_n}, {rms_steering:14.4f}, {total_distance:14.4f}, {mean_curvature:14.4f}, {total_curvature:14.4f}, {mean_deviation:14.4f}, {total_deviation:14.4f}, {progress:14.4f}, {time:14.2f}, {avg_velocity:14.4f}, {mean_race_deviation:14.2f}, {total_race_deviation:14.2f}\n") 132 | 133 | def generate_summary_stats(self): 134 | progress_ind = 7 135 | n_values = 12 136 | data = [] 137 | for i in range(n_values): 138 | data.append([]) 139 | 140 | n_success, n_total = 0, 0 141 | progresses = [] 142 | with open(self.path + "Statistics.txt", 'r') as file: 143 | lines = file.readlines() 144 | if len(lines) < 3: return 145 | 146 | for lap_n in range(len(lines)-2): 147 | line = lines[lap_n+2] # first lap is heading 148 | line = line.split(',') 149 | progress = float(line[progress_ind]) 150 | n_total += 1 151 | progresses.append(progress) 152 | if progress < 0.01 or progress > 0.99: 153 | n_success += 1 154 | for i in range(n_values): 155 | data[i].append(float(line[i])) 156 | else: 157 | continue 158 | 159 | progresses = np.array(progresses) 160 | data = np.array(data) 161 | with open(self.path + "SummaryStatistics.txt", "w") as file: 162 | file.write(lines[0]) 163 | file.write(lines[1]) 164 | file.write("0") 165 | for i in range(1, n_values): 166 | if i == progress_ind: 167 | file.write(f", {np.mean(progresses*100):14.4f}") 168 | else: 169 | avg = np.mean(data[i]) 170 | # if np.isnan(avg): 171 | # avg = 0 172 | file.write(f", {avg:14.4f}") 173 | file.write(f", {n_success/n_total * 100}") 174 | file.write("\n") 175 | 176 | 177 | def analyse_folder(): 178 | # path = "Data/Vehicles/Cth_speeds/" 179 | # path = "Data/Vehicles/Cth_maps/" 180 | # path = "Data/Vehicles/TAL_speeds/" 181 | # path = "Data/Vehicles/TAL_maps8/" 182 | path = "Data/Vehicles/PP_maps8/" 183 | # path = "Data/Vehicles/TAL_maps/" 184 | # path = "Data/Vehicles/Cth_speedMaps/" 185 | # path = "Data/Vehicles/CthVsProgress/" 186 | 187 | 188 | TestData = AnalyseTestLapData() 189 | TestData.explore_folder(path) 190 | 191 | 192 | 193 | if __name__ == '__main__': 194 | analyse_folder() 195 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/CalculateStatisticsMaps.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | import glob 4 | import os 5 | 6 | from PIL import Image 7 | import glob 8 | import trajectory_planning_helpers as tph 9 | from matplotlib.ticker import PercentFormatter 10 | from matplotlib.collections import LineCollection 11 | 12 | from TrajectoryAidedLearning.DataTools.MapData import MapData 13 | from TrajectoryAidedLearning.RewardSignals.StdTrack import StdTrack 14 | from TrajectoryAidedLearning.RewardSignals.RacingTrack import RacingTrack 15 | from TrajectoryAidedLearning.Utils.utils import * 16 | from matplotlib.ticker import MultipleLocator 17 | 18 | # SAVE_PDF = False 19 | SAVE_PDF = True 20 | 21 | 22 | def ensure_path_exists(folder): 23 | if not os.path.exists(folder): 24 | os.makedirs(folder) 25 | 26 | 27 | class AnalyseTestLapData: 28 | def __init__(self): 29 | self.path = None 30 | self.vehicle_name = None 31 | self.map_name = None 32 | self.states = None 33 | self.actions = None 34 | self.map_data = None 35 | self.std_track = None 36 | self.summary_path = None 37 | self.lap_n = 0 38 | 39 | def explore_folder(self, path): 40 | # vehicle_folders = glob.glob(f"{path}PP*/") 41 | vehicle_folders = glob.glob(f"{path}*/") 42 | # vehicle_folders = glob.glob(f"{path}Agent*/") 43 | print(vehicle_folders) 44 | print(f"{len(vehicle_folders)} folders found") 45 | 46 | for j, folder in enumerate(vehicle_folders): 47 | print(f"Vehicle folder being opened: {folder}") 48 | 49 | # if os.path.exists(folder + "Statistics.txt"): 50 | # continue 51 | 52 | self.process_folder(folder) 53 | 54 | def process_folder(self, folder): 55 | self.path = folder 56 | 57 | 58 | self.vehicle_name = self.path.split("/")[-2] 59 | self.map_name = self.vehicle_name.split("_")[4] 60 | if self.map_name == "f1": 61 | self.map_name += "_" + self.vehicle_name.split("_")[5] 62 | if self.map_name != "f1_gbr": return 63 | # if self.map_name != "f1_esp": return 64 | 65 | map_names = ["f1_aut", "f1_esp", "f1_gbr", "f1_mco"] 66 | for map_name in map_names: 67 | self.map_name = map_name 68 | with open(self.path + f"Statistics{self.map_name[-3:].upper()}.txt", "w") as file: 69 | file.write(f"Name: {self.path}\n") 70 | file.write("Lap" + "Steering".rjust(16) + "Total Distance".rjust(16) + "Mean Curvature".rjust(16) + "Total Curvature".rjust(16) + "Mean Deviation".rjust(16) + "Total Deviation".rjust(16) + "Progress".rjust(16) + "Time".rjust(16) + "Avg Velocity".rjust(16) + "Mean R Deviation".rjust(16) + "Total R Deviation".rjust(16) + "\n") 71 | 72 | self.map_data = MapData(self.map_name) 73 | self.std_track = StdTrack(self.map_name) 74 | self.racing_track = RacingTrack(self.map_name) 75 | 76 | for self.lap_n in range(100): 77 | if not self.load_lap_data(): break # no more laps 78 | self.calculate_lap_statistics() 79 | 80 | self.generate_summary_stats() 81 | 82 | def load_lap_data(self): 83 | try: 84 | data = np.load(self.path + f"Testing{self.map_name[-3:].upper()}/" + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 85 | # data = np.load(self.path + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 86 | except Exception as e: 87 | print(e) 88 | print(f"No data for: " + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 89 | return 0 90 | self.states = data[:, :7] 91 | self.actions = data[:, 7:] 92 | 93 | return 1 # to say success 94 | 95 | def calculate_lap_statistics(self): 96 | if not self.load_lap_data(): return 97 | 98 | steering = np.abs(self.actions[:, 0]) 99 | rms_steering = np.mean(np.abs(steering)) 100 | 101 | pts = self.states[:, 0:2] 102 | ss = np.linalg.norm(np.diff(pts, axis=0), axis=1) 103 | total_distance = np.sum(ss) 104 | 105 | ths, ks = tph.calc_head_curv_num.calc_head_curv_num(pts, ss, False) 106 | mean_curvature = np.mean(np.abs(ks)) 107 | total_curvature = np.sum(np.abs(ks)) 108 | 109 | hs = [] 110 | for point in pts: 111 | idx, dists = self.std_track.get_trackline_segment(point) 112 | x, h = self.std_track.interp_pts(idx, dists) 113 | hs.append(h) 114 | 115 | hs = np.array(hs) 116 | mean_deviation = np.mean(hs) 117 | total_deviation = np.sum(hs) 118 | hs = [] 119 | for point in pts: 120 | idx, dists = self.racing_track.get_trackline_segment(point) 121 | x, h = self.racing_track.interp_pts(idx, dists) 122 | hs.append(h) 123 | 124 | hs = np.array(hs) 125 | mean_race_deviation = np.mean(hs) 126 | total_race_deviation = np.sum(hs) 127 | 128 | time = len(pts) /10 129 | vs = self.states[:, 3] 130 | avg_velocity = np.mean(vs) 131 | 132 | progress = self.std_track.calculate_progress(pts[-1])/self.std_track.total_s 133 | if progress < 0.01 or progress > 0.99: 134 | progress = 1 # it is finished 135 | 136 | with open(self.path + f"Statistics{self.map_name[-3:].upper()}.txt", "a") as file: 137 | file.write(f"{self.lap_n}, {rms_steering:14.4f}, {total_distance:14.4f}, {mean_curvature:14.4f}, {total_curvature:14.4f}, {mean_deviation:14.4f}, {total_deviation:14.4f}, {progress:14.4f}, {time:14.2f}, {avg_velocity:14.4f}, {mean_race_deviation:14.2f}, {total_race_deviation:14.2f}\n") 138 | 139 | def generate_summary_stats(self): 140 | progress_ind = 7 141 | n_values = 12 142 | data = [] 143 | for i in range(n_values): 144 | data.append([]) 145 | 146 | n_success, n_total = 0, 0 147 | progresses = [] 148 | with open(self.path + f"Statistics{self.map_name[-3:].upper()}.txt", 'r') as file: 149 | lines = file.readlines() 150 | if len(lines) < 3: return 151 | 152 | for lap_n in range(len(lines)-2): 153 | line = lines[lap_n+2] # first lap is heading 154 | line = line.split(',') 155 | progress = float(line[progress_ind]) 156 | n_total += 1 157 | progresses.append(progress) 158 | if progress < 0.01 or progress > 0.99: 159 | n_success += 1 160 | for i in range(n_values): 161 | data[i].append(float(line[i])) 162 | else: 163 | continue 164 | 165 | progresses = np.array(progresses) 166 | data = np.array(data) 167 | with open(self.path + f"SummaryStatistics{self.map_name[-3:].upper()}.txt", "w") as file: 168 | file.write(lines[0]) 169 | file.write(lines[1]) 170 | file.write("0") 171 | for i in range(1, n_values): 172 | if i == progress_ind: 173 | file.write(f", {np.mean(progresses*100):14.4f}") 174 | else: 175 | avg = np.mean(data[i]) 176 | # if np.isnan(avg): 177 | # avg = 0 178 | file.write(f", {avg:14.4f}") 179 | file.write(f", {n_success/n_total * 100}") 180 | file.write("\n") 181 | 182 | 183 | def analyse_folder(): 184 | path = "Data/Vehicles/Cth_maps3/" 185 | # path = "Data/Vehicles/TAL_maps/" 186 | 187 | 188 | TestData = AnalyseTestLapData() 189 | TestData.explore_folder(path) 190 | 191 | 192 | 193 | if __name__ == '__main__': 194 | analyse_folder() 195 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/GenerateBarPlots.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | import glob 4 | from matplotlib.ticker import MultipleLocator 5 | 6 | 7 | 8 | 9 | def plot_reward_barplot_series(folder): 10 | keys = ["time", "success", "progress"] 11 | ylabels = "Time (s), Success (%), Progress (%)".split(", ") 12 | 13 | for i in range(len(keys)): 14 | make_reward_barplot(folder, keys[i], ylabels[i]) 15 | 16 | 17 | def make_reward_barplot(folder, key, ylabel): 18 | plt.figure(figsize=(2.2, 2.4)) 19 | xs = np.arange(2) 20 | 21 | barWidth = 0.4 22 | w = 0.05 23 | br1 = xs - barWidth/2 24 | br2 = [x + barWidth for x in br1] 25 | 26 | mins, maxes, means = load_time_data(folder, "Progress") 27 | 28 | plt.bar(br1, means[key], color=pp_light[2], width=barWidth, label="Prog.") 29 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[2], w) 30 | 31 | mins, maxes, means = load_time_data(folder, "Cth") 32 | plt.bar(br2, means[key], color=pp_light[0], width=barWidth, label="Cth") 33 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[0], w) 34 | 35 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 36 | plt.ylabel(ylabel) 37 | plt.xticks([0, 1], ["ESP", "MCO"]) 38 | 39 | plt.legend(ncol=2, loc="center", bbox_to_anchor=(0.5, -0.25)) 40 | 41 | name = folder + f"RewardBarplot_{key}_{folder.split('/')[-2]}" 42 | 43 | std_img_saving(name) 44 | 45 | 46 | def make_reward_barplot_combined(folder): 47 | # plt.figure(figsize=(3.3, 1.5)) 48 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2.0)) 49 | # plt.figure(figsize=(2.2, 2.4)) 50 | xs = np.arange(2) 51 | 52 | barWidth = 0.4 53 | w = 0.05 54 | br1 = xs - barWidth/2 55 | br2 = [x + barWidth for x in br1] 56 | 57 | keys = ["time", "progress"] 58 | ylabels = "Time (s), Progress (%)".split(", ") 59 | 60 | for z in range(2): 61 | key = keys[z] 62 | 63 | mins, maxes, means = load_time_data(folder, "Progress") 64 | 65 | axs[z].bar(br1, means[key], color=pp_light[2], width=barWidth, label="Progress") 66 | plt.sca(axs[z]) 67 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[2], w) 68 | 69 | mins, maxes, means = load_time_data(folder, "Cth") 70 | axs[z].bar(br2, means[key], color=pp_light[0], width=barWidth, label="Cross-track & Heading") 71 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[0], w) 72 | 73 | axs[z].xaxis.set_major_locator(MultipleLocator(1)) 74 | axs[z].set_ylabel(ylabels[z]) 75 | axs[z].set_xticks([0, 1], ["ESP", "MCO"]) 76 | axs[z].grid(True) 77 | 78 | handles, labels = axs[0].get_legend_handles_labels() 79 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, -0.01)) 80 | # plt.legend(ncol=2, loc="center", bbox_to_anchor=(0.5, -0.25)) 81 | # fig.legend(ncol=2, loc="center", bbox_to_anchor=(0.5, -0.05)) 82 | # fig.legend(["Progress", "Cth"], ncol=2, loc="center", bbox_to_anchor=(0.5, -0.05)) 83 | 84 | name = folder + f"RewardBarplot_combined_{folder.split('/')[-2]}" 85 | 86 | std_img_saving(name) 87 | 88 | 89 | 90 | 91 | 92 | def plot_six_barplot_series(): 93 | keys = ["time", "success", "progress"] 94 | ylabels = "Time (s), Success (%), Progress (%)".split(", ") 95 | 96 | for i in range(len(keys)): 97 | make_barplot_cth_vs_tal_6(keys[i], ylabels[i]) 98 | 99 | 100 | 101 | def make_barplot_cth_vs_tal_6(key, ylabel): 102 | cth_folder = "Data/Vehicles/Cth_maps/" 103 | tal_folder = "Data/Vehicles/TAL_maps/" 104 | 105 | # plt.figure(figsize=(3.9, 1.9)) 106 | plt.figure(figsize=(2.5, 1.9)) 107 | # plt.figure(figsize=(3.3, 1.9)) 108 | xs = np.arange(4) 109 | 110 | barWidth = 0.4 111 | w = 0.05 112 | br1 = xs - barWidth/2 113 | br2 = [x + barWidth for x in br1] 114 | 115 | # key = "progress" 116 | # ylabel = "Progress (%)" 117 | 118 | mins, maxes, means = load_time_data(cth_folder, "") 119 | 120 | plt.bar(br1, means[key], color=pp_light[4], width=barWidth, label="Baseline") 121 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[4], w) 122 | 123 | mins, maxes, means = load_time_data(tal_folder, "") 124 | plt.bar(br2, means[key], color=pp_light[5], width=barWidth, label="TAL") 125 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[5], w) 126 | 127 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 128 | # plt.xlabel("Maximum speed (m/s)") 129 | plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 130 | plt.ylabel(ylabel) 131 | 132 | 133 | plt.legend(framealpha=0.95, ncol=2) 134 | 135 | name = "Data/Images/" + f"CthVsTal6_{key}" 136 | 137 | std_img_saving(name) 138 | 139 | def make_barplot_cth_vs_tal_6_success(): 140 | cth_folder = "Data/Vehicles/Cth_maps/" 141 | tal_folder = "Data/Vehicles/TAL_maps/" 142 | 143 | key = "success" 144 | ylabel = "Success (%)" 145 | 146 | plt.figure(figsize=(3.9, 1.9)) 147 | xs = np.arange(4) 148 | 149 | barWidth = 0.4 150 | w = 0.05 151 | br1 = xs - barWidth/2 152 | br2 = [x + barWidth for x in br1] 153 | 154 | mins, maxes, means = load_time_data(cth_folder, "") 155 | 156 | plt.bar(br1, means[key], color=pp_light[4], width=barWidth, label="Baseline") 157 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[4], w) 158 | 159 | mins, maxes, means = load_time_data(tal_folder, "") 160 | plt.bar(br2, means[key], color=pp_light[5], width=barWidth, label="TAL") 161 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[5], w) 162 | 163 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 164 | # plt.xlabel("Maximum speed (m/s)") 165 | plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 166 | plt.ylabel(ylabel) 167 | 168 | 169 | # plt.legend(framealpha=0.95, ncol=1, loc="center", bbox_to_anchor=(1.2, 0.5)) 170 | plt.legend(ncol=2, loc="center", bbox_to_anchor=(0.5, 1.1)) 171 | 172 | name = "Data/Images/" + f"CthVsTal6_{key}_export" 173 | 174 | std_img_saving(name) 175 | 176 | 177 | make_barplot_cth_vs_tal_6_combined() 178 | # plot_six_barplot_series() 179 | # make_barplot_cth_vs_tal_6_success() 180 | # plot_speed_barplot_series("Data/Vehicles/Cth_speedMaps/") 181 | # plot_barplot_series("Data/Vehicles/CthVsProgress/") 182 | # make_reward_barplot_combined("Data/Vehicles/CthVsProgress/") -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/AnalysePerformance/GenerateVelocityProfiles.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | # plt.rc('font', family='serif') 3 | # plt.rc('pdf',fonttype = 42) 4 | # plt.rc('text', usetex=True) 5 | plt.rcParams['pdf.use14corefonts'] = True 6 | 7 | import numpy as np 8 | import glob 9 | import os 10 | 11 | import glob 12 | from matplotlib.ticker import PercentFormatter 13 | from matplotlib.collections import LineCollection 14 | 15 | from TrajectoryAidedLearning.DataTools.MapData import MapData 16 | from TrajectoryAidedLearning.Utils.StdTrack import StdTrack 17 | from TrajectoryAidedLearning.Utils.RacingTrack import RacingTrack 18 | from TrajectoryAidedLearning.Utils.utils import * 19 | from matplotlib.ticker import MultipleLocator 20 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 21 | 22 | # SAVE_PDF = False 23 | SAVE_PDF = True 24 | 25 | 26 | def ensure_path_exists(folder): 27 | if not os.path.exists(folder): 28 | os.makedirs(folder) 29 | 30 | 31 | class AnalyseTestLapData: 32 | def __init__(self): 33 | self.path = None 34 | self.vehicle_name = None 35 | self.map_name = None 36 | self.states = None 37 | self.actions = None 38 | self.map_data = None 39 | self.std_track = None 40 | self.summary_path = None 41 | self.lap_n = 0 42 | 43 | def explore_folder(self, path): 44 | vehicle_folders = glob.glob(f"{path}*/") 45 | print(vehicle_folders) 46 | print(f"{len(vehicle_folders)} folders found") 47 | 48 | set = 1 49 | for j, folder in enumerate(vehicle_folders): 50 | print(f"Vehicle folder being opened: {folder}") 51 | 52 | 53 | 54 | self.process_folder(folder) 55 | 56 | def process_folder(self, folder): 57 | self.path = folder 58 | 59 | self.vehicle_name = self.path.split("/")[-2] 60 | 61 | # if int(self.vehicle_name.split("_")[-1]) != 1: return 62 | 63 | self.map_name = self.vehicle_name.split("_")[4] 64 | if self.map_name == "f1": 65 | self.map_name += "_" + self.vehicle_name.split("_")[5] 66 | self.map_data = MapData(self.map_name) 67 | self.std_track = StdTrack(self.map_name) 68 | self.racing_track = RacingTrack(self.map_name) 69 | 70 | if not os.path.exists(self.path + "TestingVelocities/"): 71 | os.mkdir(self.path + "TestingVelocities/") 72 | for self.lap_n in range(5): 73 | if not self.load_lap_data(): break # no more laps 74 | self.plot_velocity_heat_map() 75 | 76 | 77 | def load_lap_data(self): 78 | try: 79 | data = np.load(self.path + f"Testing/Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 80 | except Exception as e: 81 | print(e) 82 | print(f"No data for: " + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 83 | return 0 84 | self.states = data[:, :7] 85 | self.actions = data[:, 7:] 86 | 87 | return 1 # to say success 88 | 89 | 90 | def plot_velocity_heat_map(self): 91 | save_path = self.path + "TestingVelocities/" 92 | 93 | plt.figure(1) 94 | plt.clf() 95 | points = self.states[:, 0:2] 96 | vs = self.states[:, 3] 97 | 98 | self.map_data.plot_map_img() 99 | 100 | xs, ys = self.map_data.pts2rc(points) 101 | points = np.concatenate([xs[:, None], ys[:, None]], axis=1) 102 | points = points.reshape(-1, 1, 2) 103 | segments = np.concatenate([points[:-1], points[1:]], axis=1) 104 | 105 | norm = plt.Normalize(0, 8) 106 | lc = LineCollection(segments, cmap='jet', norm=norm) 107 | lc.set_array(vs) 108 | lc.set_linewidth(5) 109 | line = plt.gca().add_collection(lc) 110 | cbar = plt.colorbar(line,fraction=0.046, pad=0.04, shrink=0.99) 111 | cbar.ax.tick_params(labelsize=25) 112 | plt.gca().set_aspect('equal', adjustable='box') 113 | 114 | 115 | txt = self.vehicle_name.split("_")[3] 116 | if txt == "PP": txt = "Classic" 117 | if txt == "Cth": txt = "Baseline" 118 | # if len(txt)==5: txt = "SSS" 119 | # elif len(txt)==3: txt = "PP" 120 | # plt.text(300, 400, txt, fontsize=25, ha='left', backgroundcolor='white', color="#1B4F72") 121 | # plt.text(1050, 130, txt, fontsize=28, ha='left', backgroundcolor='white', color="#1B4F72") 122 | 123 | 124 | plt.xticks([]) 125 | plt.yticks([]) 126 | plt.tight_layout() 127 | ax = plt.gca() 128 | ax.spines['top'].set_visible(False) 129 | ax.spines['right'].set_visible(False) 130 | ax.spines['bottom'].set_visible(False) 131 | ax.spines['left'].set_visible(False) 132 | 133 | l_txt = plt.text(350, 80, txt, fontsize=28, ha='left', backgroundcolor='white', color="#1B4F72") 134 | name = save_path + f"{self.vehicle_name}_velocity_map_{self.lap_n}_left" 135 | esp_left_limits() 136 | std_img_saving(name) 137 | # del l_txt 138 | l_txt.set_visible(False) 139 | 140 | plt.text(1050, 130, txt, fontsize=28, ha='left', backgroundcolor='white', color="#1B4F72") 141 | name = save_path + f"{self.vehicle_name}_velocity_map_{self.lap_n}_right" 142 | esp_right_limits() 143 | std_img_saving(name) 144 | 145 | def esp_left_limits(): 146 | plt.xlim(20, 620) 147 | plt.ylim(50, 520) 148 | 149 | def esp_right_limits(): 150 | plt.xlim(900, 1500) 151 | plt.ylim(50, 520) 152 | 153 | def analyse_folder(): 154 | 155 | # path = "Data/Vehicles/TAL_speeds/" 156 | # path = "Data/Vehicles/TAL_speeds_old/" 157 | # path = "Data/Vehicles/Cth_speeds/" 158 | # path = "Data/Vehicles/PP_speeds/" 159 | path = "Data/Vehicles/PP_maps6/" 160 | 161 | TestData = AnalyseTestLapData() 162 | TestData.explore_folder(path) 163 | 164 | 165 | if __name__ == '__main__': 166 | analyse_folder() 167 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/MapData.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | import csv, yaml 4 | from PIL import Image 5 | from matplotlib.collections import LineCollection 6 | 7 | class MapData: 8 | def __init__(self, map_name): 9 | self.path = "maps/" 10 | # self.path = "map_data/" 11 | self.map_name = map_name 12 | 13 | self.xs, ys = None, None 14 | self.t_ss, self.t_xs, self.t_ys, self.t_ths, self.t_ks, self.t_vs, self.t_accs = None, None, None, None, None, None, None 15 | 16 | self.N = 0 17 | self.map_resolution = None 18 | self.map_origin = None 19 | self.map_img = None 20 | self.map_height = None 21 | self.map_width = None 22 | 23 | self.load_map_img() 24 | self.load_centerline() 25 | try: 26 | self.load_raceline() 27 | except: pass 28 | 29 | def load_map_img(self): 30 | with open(self.path + self.map_name + ".yaml", 'r') as file: 31 | map_yaml_data = yaml.safe_load(file) 32 | self.map_resolution = map_yaml_data["resolution"] 33 | self.map_origin = map_yaml_data["origin"] 34 | map_img_name = map_yaml_data["image"] 35 | 36 | self.map_img = np.array(Image.open(self.path + map_img_name).transpose(Image.FLIP_TOP_BOTTOM)) 37 | self.map_img = self.map_img.astype(np.float64) 38 | 39 | self.map_img[self.map_img <= 128.] = 0. 40 | self.map_img[self.map_img > 128.] = 1. 41 | 42 | self.map_height = self.map_img.shape[0] 43 | self.map_width = self.map_img.shape[1] 44 | 45 | def load_centerline(self): 46 | xs, ys = [], [] 47 | # with open(self.path + self.map_name + "_std.csv", 'r') as file: 48 | with open(self.path + self.map_name + "_centerline.csv", 'r') as file: 49 | csvFile = csv.reader(file) 50 | 51 | for i, lines in enumerate(csvFile): 52 | if i ==0: 53 | continue 54 | xs.append(float(lines[0])) 55 | ys.append(float(lines[1])) 56 | 57 | self.xs = np.array(xs) 58 | self.ys = np.array(ys) 59 | 60 | self.N = len(xs) 61 | 62 | def load_raceline(self): 63 | ss, xs, ys, thetas, ks, vs, accs = [], [], [], [], [], [], [] 64 | 65 | waypoints = np.loadtxt(self.path + self.map_name + '_raceline.csv', delimiter=',', skiprows=0) 66 | 67 | for i in range(len(waypoints)): 68 | if i ==0 or i ==1 or i ==2: 69 | continue 70 | lines = waypoints[i] 71 | ss.append(float(lines[0])) 72 | xs.append(float(lines[1])) 73 | ys.append(float(lines[2])) 74 | thetas.append(float(lines[3])) 75 | ks.append(float(lines[4])) 76 | vs.append(float(lines[5])) 77 | # accs.append(float(lines[6])) 78 | 79 | self.t_ss = np.array(ss) 80 | self.t_xs = np.array(xs) 81 | self.t_ys = np.array(ys) 82 | self.t_ths = np.array(thetas) 83 | self.t_ks = np.array(ks) 84 | self.t_vs = np.array(vs) 85 | # self.t_accs = np.array(accs) 86 | 87 | def xy2rc(self, xs, ys): 88 | xs = (xs - self.map_origin[0]) / self.map_resolution 89 | ys = (ys - self.map_origin[1]) /self.map_resolution 90 | return xs, ys 91 | 92 | def pts2rc(self, pts): 93 | return self.xy2rc(pts[:,0], pts[:,1]) 94 | 95 | def plot_centre_line(self): 96 | xs, ys = self.xy2rc(self.xs, self.ys) 97 | plt.plot(xs, ys, '--', color='black', linewidth=1) 98 | 99 | def plot_race_line(self): 100 | xs, ys = self.xy2rc(self.t_xs, self.t_ys) 101 | 102 | points = np.array([xs, ys]).T.reshape(-1, 1, 2) 103 | segments = np.concatenate([points[:-1], points[1:]], axis=1) 104 | 105 | norm = plt.Normalize(self.t_vs.min(), self.t_vs.max()) 106 | lc = LineCollection(segments, cmap='jet', norm=norm) 107 | lc.set_array(self.t_vs) 108 | lc.set_linewidth(2) 109 | line = plt.gca().add_collection(lc) 110 | plt.colorbar(line) 111 | 112 | def plot_map_img(self): 113 | self.map_img[self.map_img == 1] = 180 114 | self.map_img[self.map_img == 0 ] = 230 115 | self.map_img[0, 1] = 255 116 | self.map_img[0, 0] = 0 117 | plt.imshow(self.map_img, origin='lower', cmap='gray') 118 | 119 | def plot_map_data(self): 120 | self.plot_map_img() 121 | 122 | self.plot_centre_line() 123 | 124 | self.plot_race_line() 125 | 126 | plt.show() 127 | 128 | 129 | 130 | 131 | def main(): 132 | map_name = "f1_gbr" 133 | 134 | map_data = MapData(map_name) 135 | map_data.plot_map_data() 136 | 137 | if __name__ == '__main__': 138 | 139 | main() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/CthVsProgress_Barplot.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 2 | from matplotlib.ticker import MultipleLocator 3 | 4 | 5 | def CthVsProgress_Barplot(): 6 | folder = "Data/Vehicles/CthVsProgress/" 7 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2.0)) 8 | xs = np.arange(2) 9 | 10 | barWidth = 0.4 11 | w = 0.05 12 | br1 = xs - barWidth/2 13 | br2 = [x + barWidth for x in br1] 14 | 15 | keys = ["time", "progress"] 16 | ylabels = "Time (s), Progress (%)".split(", ") 17 | 18 | for z in range(2): 19 | key = keys[z] 20 | 21 | mins, maxes, means = load_time_data(folder, "Progress") 22 | 23 | axs[z].bar(br1, means[key], color=pp_light[2], width=barWidth, label="Progress") 24 | plt.sca(axs[z]) 25 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[2], w) 26 | 27 | mins, maxes, means = load_time_data(folder, "Cth") 28 | axs[z].bar(br2, means[key], color=pp_light[0], width=barWidth, label="Cross-track & Heading") 29 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[0], w) 30 | 31 | axs[z].xaxis.set_major_locator(MultipleLocator(1)) 32 | axs[z].set_ylabel(ylabels[z]) 33 | axs[z].set_xticks([0, 1], ["ESP", "MCO"]) 34 | axs[z].grid(True) 35 | 36 | handles, labels = axs[0].get_legend_handles_labels() 37 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, -0.01)) 38 | 39 | name = folder + f"{folder.split('/')[-2]}_Barplot" 40 | 41 | std_img_saving(name) 42 | 43 | 44 | 45 | CthVsProgress_Barplot() 46 | 47 | 48 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/Cth_speeds_Barplot.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 2 | import numpy as np 3 | from matplotlib import pyplot as plt 4 | from matplotlib.ticker import MultipleLocator 5 | 6 | 7 | def make_speed_barplot(folder, key, ylabel): 8 | plt.figure(figsize=(2.5, 1.9)) 9 | # plt.figure(figsize=(3.3, 1.9)) 10 | xs = np.arange(4, 9) 11 | 12 | barWidth = 0.4 13 | w = 0.05 14 | br1 = xs - barWidth/2 15 | br2 = [x + barWidth for x in br1] 16 | 17 | mins, maxes, means = load_time_data(folder, "gbr") 18 | 19 | plt.bar(br1, means[key], color=pp_light[1], width=barWidth, label="GBR") 20 | plot_error_bars(br1, mins[key], maxes[key], pp_darkest[1], w) 21 | 22 | mins, maxes, means = load_time_data(folder, "mco") 23 | plt.bar(br2, means[key], color=pp_light[5], width=barWidth, label="MCO") 24 | plot_error_bars(br2, mins[key], maxes[key], pp_darkest[5], w) 25 | 26 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 27 | plt.xlabel("Maximum speed (m/s)") 28 | plt.ylabel(ylabel) 29 | 30 | plt.legend() 31 | 32 | name = folder + f"SpeedBarPlot_{key}_{folder.split('/')[-2]}" 33 | 34 | std_img_saving(name) 35 | 36 | def plot_speed_barplot_series(folder): 37 | keys = ["time", "success", "progress"] 38 | ylabels = "Time (s), Success (%), Progress (%)".split(", ") 39 | 40 | for i in range(len(keys)): 41 | make_speed_barplot(folder, keys[i], ylabels[i]) 42 | 43 | 44 | def Cth_speeds_Barplot(): 45 | folder = "Data/Vehicles/Cth_speedMaps/" 46 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2.0)) 47 | xs = np.arange(4) 48 | 49 | barWidth = 0.4 50 | w = 0.05 51 | br1 = xs - barWidth/2 52 | br2 = [x + barWidth for x in br1] 53 | 54 | keys = ["time", "success"] 55 | ylabels = "Time (s), Success (%)".split(", ") 56 | 57 | for z in range(2): 58 | key = keys[z] 59 | 60 | mins, maxes, means = load_time_data(folder, "gbr") 61 | 62 | axs[z].bar(br1, means[key][0:4], color=pp_light[1], width=barWidth, label="GBR") 63 | plt.sca(axs[z]) 64 | plot_error_bars(br1, mins[key][0:4], maxes[key], pp_darkest[1], w) 65 | 66 | mins, maxes, means = load_time_data(folder, "mco") 67 | axs[z].bar(br2, means[key][0:4], color=pp_light[5], width=barWidth, label="MCO") 68 | plot_error_bars(br2, mins[key][0:4], maxes[key], pp_darkest[5], w) 69 | 70 | axs[z].xaxis.set_major_locator(MultipleLocator(1)) 71 | axs[z].set_ylabel(ylabels[z]) 72 | axs[z].set_xticks([0, 1, 2, 3], [4, 5, 6, 7]) 73 | axs[z].grid(True) 74 | 75 | handles, labels = axs[0].get_legend_handles_labels() 76 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, -0.01)) 77 | axs[0].set_xlabel("Maximum speed (m/s)") 78 | axs[1].set_xlabel("Maximum speed (m/s)") 79 | 80 | name = folder + f"{folder.split('/')[-2]}_Barplot" 81 | 82 | std_img_saving(name) 83 | 84 | 85 | Cth_speeds_Barplot() 86 | #TODO: fix this 87 | # plot_speed_barplot_series("Data/Vehicles/Cth_speedMaps/") -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/LiteratureComparison.py: -------------------------------------------------------------------------------- 1 | 2 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from matplotlib.ticker import MultipleLocator 6 | 7 | 8 | def LiteratureComparison_Barplot(): 9 | brunnbauer = [32, 73, 0, 0] 10 | bosello = [23, 56, 48, 42] 11 | # tal = [22, 47.5, 39, 34.6] # update these times .... 12 | tal = [20.4, 43.4, 35, 32.8] # update these times .... 13 | pp = [21.5, 47.1, 38.6, 35.2] 14 | # pp = [21, 47, 38.5, 35] 15 | 16 | maps = ["AUT", "ESP", "GBR", "MCO"] 17 | 18 | plt.figure(1, figsize=(4.95, 2.2)) 19 | barWidth = 0.2 20 | br1 = np.arange(len(tal)) - 0.3 21 | br2 = [x + barWidth for x in br1] 22 | br3 = [x + barWidth for x in br2] 23 | br4 = [x + barWidth for x in br3] 24 | 25 | 26 | 27 | plt.bar(br1, brunnbauer, color=light_purple, label="Brunnbauer", width=barWidth) 28 | plt.bar(br2, bosello, color=light_yellow, label="Bosello", width=barWidth) 29 | plt.bar(br3, tal, color=light_red, label="TAL", width=barWidth) 30 | plt.bar(br4, pp, color=light_green, label="Classic", width=barWidth) 31 | 32 | # plt.xticks([r + barWidth*2 for r in range(len(ppps))], maps) 33 | plt.xticks([0, 1, 2, 3], maps) 34 | plt.legend(loc='center', bbox_to_anchor=(0.47, -0.4), ncol=4) 35 | # plt.legend(loc='center', bbox_to_anchor=(1.15, 0.5)) 36 | plt.ylabel("Lap Time (s)") 37 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(15)) 38 | 39 | plt.grid(True) 40 | 41 | name = "Data/Images/LiteratureComparison_Barplot" 42 | std_img_saving(name) 43 | 44 | 45 | LiteratureComparison_Barplot() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/TAL_Cth_maps_Barplot.py: -------------------------------------------------------------------------------- 1 | 2 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from matplotlib.ticker import MultipleLocator 6 | 7 | 8 | 9 | 10 | def TAL_Cth_maps_Barplot(): 11 | cth_folder = "Data/Vehicles/Cth_maps/" 12 | tal_folder = "Data/Vehicles/TAL_maps/" 13 | 14 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 1.8)) 15 | xs = np.arange(4) 16 | 17 | barWidth = 0.4 18 | w = 0.05 19 | br1 = xs - barWidth/2 20 | br2 = [x + barWidth for x in br1] 21 | 22 | keys = ["time", "success"] 23 | ylabels = "Time (s), Completion (%)".split(", ") 24 | 25 | for z in range(2): 26 | key = keys[z] 27 | plt.sca(axs[z]) 28 | mins, maxes, means = load_time_data(cth_folder, "") 29 | 30 | plt.bar(br1, means[key], color=light_blue, width=barWidth, label="Baseline") 31 | plot_error_bars(br1, mins[key], maxes[key], dark_blue, w) 32 | 33 | mins, maxes, means = load_time_data(tal_folder, "") 34 | plt.bar(br2, means[key], color=light_red, width=barWidth, label="TAL") 35 | plot_error_bars(br2, mins[key], maxes[key], dark_red, w) 36 | 37 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 38 | plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 39 | plt.ylabel(ylabels[z]) 40 | plt.grid(True) 41 | 42 | axs[0].yaxis.set_major_locator(MultipleLocator(15)) 43 | axs[1].yaxis.set_major_locator(MultipleLocator(25)) 44 | handles, labels = axs[0].get_legend_handles_labels() 45 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, 0.01)) 46 | 47 | name = "Data/Images/" + f"TAL_Cth_maps_Barplot" 48 | 49 | std_img_saving(name) 50 | 51 | 52 | TAL_Cth_maps_Barplot() 53 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/TAL_Cth_maps_BarplotMaps.py: -------------------------------------------------------------------------------- 1 | 2 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from matplotlib.ticker import MultipleLocator 6 | 7 | 8 | 9 | 10 | def TAL_Cth_maps_Barplot(): 11 | cth_folder = "Data/Vehicles/Cth_maps2/" 12 | tal_folder = "Data/Vehicles/TAL_maps2/" 13 | 14 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 1.8)) 15 | xs = np.arange(4) 16 | 17 | barWidth = 0.4 18 | w = 0.05 19 | br1 = xs - barWidth/2 20 | br2 = [x + barWidth for x in br1] 21 | 22 | keys = ["time", "success"] 23 | ylabels = "Time (s), Completion (%)".split(", ") 24 | 25 | loading_key = "gbr_6_1_test" 26 | for z in range(2): 27 | key = keys[z] 28 | plt.sca(axs[z]) 29 | mins, maxes, means = load_time_data(cth_folder, loading_key) 30 | 31 | plt.bar(br1, means[key], color=light_blue, width=barWidth, label="Baseline") 32 | plot_error_bars(br1, mins[key], maxes[key], dark_blue, w) 33 | 34 | mins, maxes, means = load_time_data(tal_folder, loading_key) 35 | plt.bar(br2, means[key], color=light_red, width=barWidth, label="TAL") 36 | plot_error_bars(br2, mins[key], maxes[key], dark_red, w) 37 | 38 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 39 | plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 40 | plt.ylabel(ylabels[z]) 41 | plt.grid(True) 42 | 43 | axs[0].yaxis.set_major_locator(MultipleLocator(15)) 44 | axs[1].yaxis.set_major_locator(MultipleLocator(25)) 45 | handles, labels = axs[0].get_legend_handles_labels() 46 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, 0.01)) 47 | 48 | name = "Data/Images/" + f"TAL_Cth_maps_BarplotMaps" 49 | 50 | std_img_saving(name) 51 | 52 | 53 | TAL_Cth_maps_Barplot() 54 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/TAL_Cth_speeds_Barplot.py: -------------------------------------------------------------------------------- 1 | 2 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from matplotlib.ticker import MultipleLocator 6 | 7 | 8 | 9 | 10 | def TAL_Cth_speeds_Barplot(): 11 | cth_folder = "Data/Vehicles/Cth_speeds/" 12 | tal_folder = "Data/Vehicles/TAL_speeds_old/" 13 | 14 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2)) 15 | xs = np.arange(5) 16 | 17 | barWidth = 0.4 18 | w = 0.05 19 | br1 = xs - barWidth/2 20 | br2 = [x + barWidth for x in br1] 21 | 22 | keys = ["time", "success"] 23 | ylabels = "Time (s), Completion (%)".split(", ") 24 | 25 | for z in range(2): 26 | key = keys[z] 27 | plt.sca(axs[z]) 28 | mins, maxes, means = load_time_data(cth_folder, "") 29 | 30 | plt.bar(br1, means[key], color=light_blue, width=barWidth, label="Baseline") 31 | plot_error_bars(br1, mins[key], maxes[key], dark_blue, w) 32 | 33 | mins, maxes, means = load_time_data(tal_folder, "") 34 | plt.bar(br2, means[key], color=light_red, width=barWidth, label="TAL") 35 | plot_error_bars(br2, mins[key], maxes[key], dark_red, w) 36 | 37 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 38 | # plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 39 | plt.xticks([0, 1, 2, 3, 4], [4, 5, 6, 7, 8]) 40 | plt.xlabel("Maximum speed (m/s)") 41 | 42 | plt.ylabel(ylabels[z]) 43 | plt.grid(True) 44 | 45 | axs[0].yaxis.set_major_locator(MultipleLocator(15)) 46 | axs[1].yaxis.set_major_locator(MultipleLocator(25)) 47 | handles, labels = axs[0].get_legend_handles_labels() 48 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, 0.01)) 49 | 50 | name = "Data/Images/" + f"TAL_Cth_speeds_Barplot" 51 | 52 | std_img_saving(name) 53 | 54 | 55 | TAL_Cth_speeds_Barplot() 56 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/TAL_pp_maps_Barplot.py: -------------------------------------------------------------------------------- 1 | 2 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from matplotlib.ticker import MultipleLocator 6 | 7 | 8 | 9 | 10 | def TAL_Cth_maps_Barplot(): 11 | cth_folder = "Data/Vehicles/PP_maps8/" 12 | tal_folder = "Data/Vehicles/TAL_maps8/" 13 | 14 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 1.8)) 15 | xs = np.arange(4) 16 | 17 | barWidth = 0.4 18 | w = 0.05 19 | br1 = xs - barWidth/2 20 | br2 = [x + barWidth for x in br1] 21 | 22 | keys = ["time", "success"] 23 | ylabels = "Time (s), Success (%)".split(", ") 24 | 25 | for z in range(2): 26 | key = keys[z] 27 | plt.sca(axs[z]) 28 | mins, maxes, means = load_time_data(cth_folder, "") 29 | 30 | plt.bar(br1, means[key], color=light_blue, width=barWidth, label="PP") 31 | print(f"PP {key} times: {means[key]}") 32 | plot_error_bars(br1, mins[key], maxes[key], dark_blue, w) 33 | 34 | mins, maxes, means = load_time_data(tal_folder, "") 35 | plt.bar(br2, means[key], color=light_red, width=barWidth, label="TAL") 36 | print(f"TAL {key} times: {means[key]}") 37 | plot_error_bars(br2, mins[key], maxes[key], dark_red, w) 38 | 39 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 40 | plt.xticks([0, 1, 2, 3], ["AUT", "ESP", "GBR", "MCO"]) 41 | plt.ylabel(ylabels[z]) 42 | plt.grid(True) 43 | 44 | axs[0].yaxis.set_major_locator(MultipleLocator(15)) 45 | axs[1].yaxis.set_major_locator(MultipleLocator(25)) 46 | handles, labels = axs[0].get_legend_handles_labels() 47 | fig.legend(handles, labels, ncol=2, loc="center", bbox_to_anchor=(0.55, 0.01)) 48 | 49 | name = "Data/Images/" + f"TAL_PP_maps8_Barplot" 50 | 51 | std_img_saving(name) 52 | 53 | 54 | TAL_Cth_maps_Barplot() 55 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/PlotPerformance/TAL_speeds_AvgProgress.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 2 | import numpy as np 3 | from matplotlib import pyplot as plt 4 | from matplotlib.ticker import MultipleLocator 5 | 6 | 7 | 8 | 9 | 10 | def TAL_speeds_AvgProgress(): 11 | cth_folder = "Data/Vehicles/Cth_speeds/" 12 | tal_folder = "Data/Vehicles/TAL_speeds_old/" 13 | 14 | plt.figure(figsize=(4.5, 2.6)) 15 | xs = np.arange(4, 9) 16 | 17 | barWidth = 0.4 18 | w = 0.05 19 | br1 = xs - barWidth/2 20 | br2 = [x + barWidth for x in br1] 21 | 22 | key = "progress" 23 | ylabel = "Average Track \nProgress (%)" 24 | 25 | mins, maxes, means = load_time_data(cth_folder, "") 26 | 27 | plt.bar(br1, means[key], color=light_blue, width=barWidth, label="Baseline") 28 | plot_error_bars(br1, mins[key], maxes[key], dark_blue, w) 29 | 30 | mins, maxes, means = load_time_data(tal_folder, "") 31 | plt.bar(br2, means[key], color=light_red, width=barWidth, label="Trajectory-aided Learning (TAL)") 32 | plot_error_bars(br2, mins[key], maxes[key], dark_red, w) 33 | 34 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(1)) 35 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 36 | plt.xlabel("Maximum speed (m/s)") 37 | plt.ylabel(ylabel) 38 | 39 | plt.legend(loc="center", ncol=2, bbox_to_anchor=(0.5, -0.52)) 40 | 41 | name = "Data/Images/" + f"TAL_speeds_AvgProgress" 42 | 43 | std_img_saving(name) 44 | 45 | 46 | TAL_speeds_AvgProgress() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/ProfileAnalysis/Cth_speeds_Profiles.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.ticker import MultipleLocator 4 | from matplotlib.collections import LineCollection 5 | from TrajectoryAidedLearning.DataTools.MapData import MapData 6 | from TrajectoryAidedLearning.RewardSignals.StdTrack import StdTrack 7 | 8 | from TrajectoryAidedLearning.Utils.utils import * 9 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 10 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 11 | 12 | 13 | class TestLapData: 14 | def __init__(self, path, lap_n=0): 15 | self.path = path 16 | self.vehicle_name = self.path.split("/")[-2] 17 | self.map_name = self.vehicle_name.split("_")[4] 18 | if self.map_name == "f1": 19 | self.map_name += "_" + self.vehicle_name.split("_")[5] 20 | self.map_data = MapData(self.map_name) 21 | self.race_track = StdTrack(self.map_name) 22 | 23 | self.states = None 24 | self.actions = None 25 | self.lap_n = lap_n 26 | 27 | self.load_lap_data() 28 | 29 | def load_lap_data(self): 30 | try: 31 | data = np.load(self.path + f"Testing/Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 32 | except Exception as e: 33 | print(e) 34 | print(f"No data for: " + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 35 | return 0 36 | self.states = data[:, :7] 37 | self.actions = data[:, 7:] 38 | 39 | return 1 # to say success 40 | 41 | def generate_state_progress_list(self): 42 | pts = self.states[:, 0:2] 43 | progresses = [0] 44 | for pt in pts: 45 | p = self.race_track.calculate_progress_percent(pt) 46 | # if p < progresses[-1]: continue 47 | progresses.append(p) 48 | 49 | return np.array(progresses[:-1]) 50 | 51 | 52 | 53 | def make_slip_compare_graph(): 54 | map_name = "f1_esp" 55 | 56 | pp_path = f"Data/Vehicles/PerformanceSpeed/PP_Std5_{map_name}_1_0/" 57 | agent_path = f"Data/Vehicles/PerformanceSpeed/Agent_Cth_{map_name}_3_0/" 58 | 59 | 60 | pp_data = TestLapData(pp_path) 61 | agent_data = TestLapData(agent_path, 2) 62 | 63 | fig, (ax1) = plt.subplots(1, 1, figsize=(6, 1.7), sharex=True) 64 | ax1.plot(agent_data.states[:, 6], color=pp[1], label="Agent") 65 | ax1.plot(pp_data.states[:, 6], color=pp[0], label="PP") 66 | 67 | 68 | ax1.set_ylabel("Slip angle") 69 | ax1.set_xlabel("Time steps") 70 | ax1.legend(ncol=2) 71 | 72 | plt.grid(True) 73 | plt.tight_layout() 74 | 75 | plt.savefig(f"Data/HighSpeedEval/SlipCompare_{map_name}.pdf", bbox_inches='tight') 76 | 77 | plt.show() 78 | 79 | 80 | 81 | 82 | def compare_5_7_cth_speed(): 83 | map_name = "f1_esp" 84 | path = "Data/Vehicles/Cth_speeds/" 85 | a1 = path + f"fast_Std_Std_Cth_{map_name}_5_1_1/" 86 | a2 = path + f"fast_Std_Std_Cth_{map_name}_7_1_1/" 87 | 88 | data1 = TestLapData(a1, 2) 89 | data2 = TestLapData(a2, 2) 90 | xs1 = data1.generate_state_progress_list()*100 91 | xs2 = data2.generate_state_progress_list()*100 92 | 93 | fig, (ax1) = plt.subplots(1, 1, figsize=(4.2, 1.7), sharex=True) 94 | ax1.plot(xs1[:-1], data1.states[:-1, 3], color=pp[1], label="5 m/s", linewidth=2) 95 | ax1.plot(xs2[:-1], data2.states[:-1, 3], color=pp[0], label="7 m/s", linewidth=2) 96 | 97 | ax1.set_ylabel("Speed (m/s)") 98 | ax1.set_xlabel("Track progress (%)") 99 | ax1.legend(ncol=2) 100 | ax1.yaxis.set_major_locator(MultipleLocator(2)) 101 | # ax2.set_ylabel("Slip Angle") 102 | 103 | plt.grid(True) 104 | plt.xlim(-2, 40) 105 | plt.tight_layout() 106 | 107 | name = path + "compare_5_7_cth_speed" 108 | std_img_saving(name) 109 | 110 | 111 | def compare_5_7_cth_slip(): 112 | map_name = "f1_esp" 113 | path = "Data/Vehicles/Cth_speeds/" 114 | a1 = path + f"fast_Std_Std_Cth_{map_name}_5_1_1/" 115 | a2 = path + f"fast_Std_Std_Cth_{map_name}_7_1_1/" 116 | 117 | data1 = TestLapData(a1, 2) 118 | data2 = TestLapData(a2, 2) 119 | xs1 = data1.generate_state_progress_list()*100 120 | xs2 = data2.generate_state_progress_list()*100 121 | 122 | fig, (ax1) = plt.subplots(1, 1, figsize=(4.2, 1.7), sharex=True) 123 | s1 = np.rad2deg(data1.states[:-1, 6]) 124 | ax1.plot(xs1[:-1], s1, color=pp[1], label="5", linewidth=2, alpha=0.88) 125 | s2 = np.rad2deg(data2.states[:, 6]) 126 | ax1.plot(xs2, s2, color=pp[0], label="7", linewidth=2, alpha=0.9) 127 | 128 | ax1.set_ylabel("Slip angle (deg)") 129 | ax1.set_xlabel("Track progress (%)") 130 | ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.77, 0.15)) 131 | # ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.7, 1.02)) 132 | # ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.35, 1.02)) 133 | ax1.yaxis.set_major_locator(MultipleLocator(25)) 134 | 135 | plt.grid(True) 136 | plt.xlim(-2, 40) 137 | plt.tight_layout() 138 | 139 | name = path + "compare_5_7_cth_slip" 140 | std_img_saving(name) 141 | 142 | 143 | def Cth_speed_slip_profile(): 144 | map_name = "f1_esp" 145 | path = "Data/Vehicles/Cth_speeds/" 146 | a1 = path + f"fast_Std_Std_Cth_{map_name}_5_1_1/" 147 | a2 = path + f"fast_Std_Std_Cth_{map_name}_7_1_1/" 148 | 149 | data1 = TestLapData(a1, 2) 150 | data2 = TestLapData(a2, 2) 151 | xs1 = data1.generate_state_progress_list()*100 152 | xs2 = data2.generate_state_progress_list()*100 153 | 154 | fig, (ax0, ax1) = plt.subplots(2, 1, figsize=(4.2, 2.5), sharex=True) 155 | 156 | ax0.plot(xs1[:-1], data1.states[:-1, 3], color=pp[1], label="5 m/s", linewidth=2) 157 | ax0.plot(xs2[:-1], data2.states[:-1, 3], color=pp[0], label="7 m/s", linewidth=2) 158 | ax0.yaxis.set_major_locator(MultipleLocator(2)) 159 | 160 | ax0.set_ylabel("Speed (m/s)") 161 | ax0.grid(True) 162 | 163 | s1 = np.rad2deg(data1.states[:-1, 6]) 164 | ax1.plot(xs1[:-1], s1, color=pp[1], linewidth=2, alpha=0.88) 165 | s2 = np.rad2deg(data2.states[:, 6]) 166 | ax1.plot(xs2, s2, color=pp[0], linewidth=2, alpha=0.9) 167 | 168 | ax1.set_ylabel("Slip angle (deg)") 169 | ax1.set_xlabel("Track progress (%)") 170 | # ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.77, 0.15)) 171 | # ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.7, 1.02)) 172 | # ax1.legend(ncol=2, loc='center', bbox_to_anchor=(0.35, 1.02)) 173 | ax1.yaxis.set_major_locator(MultipleLocator(25)) 174 | 175 | fig.legend(ncol=2, loc='center', bbox_to_anchor=(0.5, 0.)) 176 | 177 | plt.grid(True) 178 | plt.xlim(-2, 40) 179 | plt.tight_layout() 180 | 181 | name = path + "Cth_speed_slip_profile" 182 | std_img_saving(name) 183 | 184 | 185 | 186 | Cth_speed_slip_profile() 187 | # compare_5_7_cth_speed() 188 | # compare_5_7_cth_slip() 189 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/ProfileAnalysis/TAL_speedProfiles.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.ticker import MultipleLocator 4 | from matplotlib.collections import LineCollection 5 | from TrajectoryAidedLearning.DataTools.MapData import MapData 6 | from TrajectoryAidedLearning.RewardSignals.StdTrack import StdTrack 7 | 8 | from TrajectoryAidedLearning.Utils.utils import * 9 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 10 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 11 | 12 | 13 | class TestLapData: 14 | def __init__(self, path, lap_n=0): 15 | self.path = path 16 | self.vehicle_name = self.path.split("/")[-2] 17 | self.map_name = self.vehicle_name.split("_")[4] 18 | if self.map_name == "f1": 19 | self.map_name += "_" + self.vehicle_name.split("_")[5] 20 | self.map_data = MapData(self.map_name) 21 | self.race_track = StdTrack(self.map_name) 22 | 23 | self.states = None 24 | self.actions = None 25 | self.lap_n = lap_n 26 | 27 | self.load_lap_data() 28 | 29 | def load_lap_data(self): 30 | try: 31 | data = np.load(self.path + f"Testing/Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 32 | except Exception as e: 33 | print(e) 34 | print(f"No data for: " + f"Lap_{self.lap_n}_history_{self.vehicle_name}_{self.map_name}.npy") 35 | return 0 36 | self.states = data[:, :7] 37 | self.actions = data[:, 7:] 38 | 39 | return 1 # to say success 40 | 41 | def generate_state_progress_list(self): 42 | pts = self.states[:, 0:2] 43 | progresses = [0] 44 | for pt in pts: 45 | p = self.race_track.calculate_progress_percent(pt) 46 | # if p < progresses[-1]: continue 47 | progresses.append(p) 48 | 49 | return np.array(progresses[:-1]) 50 | 51 | 52 | 53 | def make_slip_compare_graph(): 54 | # map_name = "f1_gbr" 55 | map_name = "f1_esp" 56 | # pp_path = f"Data/Vehicles/RacingResultsWeekend/PP_Std_{map_name}_1_0/" 57 | # agent_path = f"Data/Vehicles/RacingResultsWeekend/Agent_Cth_{map_name}_2_1/" 58 | 59 | pp_path = f"Data/Vehicles/PerformanceSpeed/PP_Std5_{map_name}_1_0/" 60 | agent_path = f"Data/Vehicles/PerformanceSpeed/Agent_Cth_{map_name}_3_0/" 61 | 62 | 63 | pp_data = TestLapData(pp_path) 64 | agent_data = TestLapData(agent_path, 2) 65 | 66 | fig, (ax1) = plt.subplots(1, 1, figsize=(6, 1.7), sharex=True) 67 | ax1.plot(agent_data.states[:, 6], color=pp[1], label="Agent") 68 | ax1.plot(pp_data.states[:, 6], color=pp[0], label="PP") 69 | 70 | 71 | ax1.set_ylabel("Slip angle") 72 | ax1.set_xlabel("Time steps") 73 | ax1.legend(ncol=2) 74 | 75 | plt.grid(True) 76 | plt.tight_layout() 77 | 78 | plt.savefig(f"Data/HighSpeedEval/SlipCompare_{map_name}.pdf", bbox_inches='tight') 79 | 80 | plt.show() 81 | 82 | 83 | 84 | 85 | def compare_tal_pp_cth_speed(): 86 | map_name = "f1_esp" 87 | path = "Data/Vehicles/" 88 | a1 = path + f"Cth_speeds/fast_Std_Std_Cth_{map_name}_6_1_1/" 89 | a2 = path + f"TAL_speeds/fast_Std_Std_TAL_{map_name}_6_1_1/" 90 | a3 = path + f"PP_speeds/PP_PP_Std_PP_{map_name}_6_1_0/" 91 | 92 | # colors = ["#E67E22", "#2ECC71", "#9B59B6"] 93 | # vehicles = [a2, a3, a1] 94 | # labels = ["TAL", "PP", "Baseline"] 95 | 96 | colors = ["#2ECC71", "#E67E22", "#9B59B6"] 97 | vehicles = [a3, a2, a1] 98 | labels = ["PP", "TAL", "Baseline"] 99 | 100 | fig, (ax1) = plt.subplots(1, 1, figsize=(4.2, 1.7), sharex=True) 101 | for i in range(len(vehicles)): 102 | vehicle = TestLapData(vehicles[i], 0) 103 | xs = vehicle.generate_state_progress_list()*100 104 | ax1.plot(xs, vehicle.states[:, 3], color=colors[i], label=labels[i], linewidth=2) 105 | 106 | ax1.set_ylabel("Speed (m/s)") 107 | ax1.set_xlabel("Track progress (%)") 108 | ax1.legend(ncol=3) 109 | ax1.yaxis.set_major_locator(MultipleLocator(2)) 110 | # ax2.set_ylabel("Slip Angle") 111 | 112 | plt.grid(True) 113 | plt.xlim(-2, 40) 114 | plt.tight_layout() 115 | 116 | name = "Data/Images/compare_speed_tal_baseline_6" 117 | std_img_saving(name) 118 | 119 | 120 | def compare_tal_pp_cth_slip_6(): 121 | map_name = "f1_esp" 122 | path = "Data/Vehicles/" 123 | a1 = path + f"Cth_speeds/fast_Std_Std_Cth_{map_name}_6_1_1/" 124 | a2 = path + f"TAL_speeds/fast_Std_Std_TAL_{map_name}_6_1_1/" 125 | a3 = path + f"PP_speeds/PP_PP_Std_PP_{map_name}_6_1_0/" 126 | 127 | 128 | # colors = ["#2ECC71", "#E67E22", "#9B59B6"] 129 | # vehicles = [a3, a2, a1] 130 | # labels = ["PP", "TAL", "Baseline"] 131 | colors = ["#9B59B6", "#E67E22", "#2ECC71"] 132 | vehicles = [a1, a2, a3] 133 | labels = ["Baseline", "TAL","PP"] 134 | 135 | fig, (ax1) = plt.subplots(1, 1, figsize=(4.2, 1.7), sharex=True) 136 | for i in range(len(vehicles)): 137 | vehicle = TestLapData(vehicles[i], 0) 138 | xs = vehicle.generate_state_progress_list()*100 139 | slip = np.rad2deg(vehicle.states[:, 6]) 140 | slip = np.abs(slip) 141 | ax1.plot(xs, slip, color=colors[i], label=labels[i], linewidth=2) 142 | 143 | ax1.set_ylabel("Slip angle (deg)") 144 | # ax1.set_ylabel("Speed (m/s)") 145 | ax1.set_xlabel("Track progress (%)") 146 | handles, labels = ax1.get_legend_handles_labels() 147 | h2 = [handles[1], handles[0], handles[2]] 148 | l2 = [labels[1], labels[0], labels[2]] 149 | # ax1.legend(handles[::-1], labels[::-1], ncol=2) 150 | # ax1.legend(handles, labels, ncol=2) 151 | ax1.legend(h2, l2, ncol=2, loc="upper right") 152 | # ax1.legend(ncol=2, loc="upper right") 153 | ax1.yaxis.set_major_locator(MultipleLocator(10)) 154 | # ax2.set_ylabel("Slip Angle") 155 | 156 | plt.grid(True) 157 | plt.xlim(-2, 40) 158 | plt.tight_layout() 159 | 160 | name = "Data/Images/compare_slip_tal_baseline_6" 161 | std_img_saving(name) 162 | 163 | 164 | def TAL_baseline_PP_slip_speed_6(): 165 | map_name = "f1_esp" 166 | path = "Data/Vehicles/" 167 | a1 = path + f"Cth_speeds/fast_Std_Std_Cth_{map_name}_6_1_1/" 168 | a2 = path + f"TAL_speeds_old/fast_Std_Std_TAL_{map_name}_6_1_1/" 169 | a3 = path + f"PP_speeds/PP_PP_Std_PP_{map_name}_6_1_0/" 170 | 171 | 172 | # colors = ["#9B59B6", "#E67E22", "#2ECC71"] 173 | colors = [plot_blue, plot_red, plot_green] 174 | vehicles = [a1, a2, a3] 175 | labels = ["Baseline", "TAL","Classic"] 176 | 177 | fig, (ax0, ax1) = plt.subplots(2, 1, figsize=(4.6, 2.7), sharex=True) 178 | lap_n = 1 179 | for i in range(len(vehicles)): 180 | vehicle = TestLapData(vehicles[i], lap_n) 181 | xs = vehicle.generate_state_progress_list()*100 182 | ax0.plot(xs, vehicle.states[:, 3], color=colors[i], label=labels[i], linewidth=2) 183 | 184 | 185 | vehicle = TestLapData(vehicles[i], lap_n) 186 | xs = vehicle.generate_state_progress_list()*100 187 | slip = np.rad2deg(vehicle.states[:, 6]) 188 | slip = np.abs(slip) 189 | ax1.plot(xs, slip, color=colors[i], label=labels[i], linewidth=2) 190 | 191 | 192 | ax1.set_ylabel("Slip angle (deg)") 193 | ax0.set_ylabel("Speed (m/s)") 194 | ax1.set_xlabel("Track progress (%)") 195 | handles, labels = ax1.get_legend_handles_labels() 196 | # h2 = [handles[1], handles[0], handles[2]] 197 | # l2 = [labels[1], labels[0], labels[2]] 198 | h2, l2 = handles, labels 199 | fig.legend(h2, l2, ncol=3, loc="center", bbox_to_anchor=(0.5, 0)) 200 | ax1.yaxis.set_major_locator(MultipleLocator(10)) 201 | ax0.yaxis.set_major_locator(MultipleLocator(2)) 202 | 203 | ax0.grid(True) 204 | plt.grid(True) 205 | plt.xlim(0, 60) 206 | # plt.xlim(-2, 40) 207 | plt.tight_layout() 208 | 209 | name = "Data/Images/TAL_baseline_PP_slip_speed_6" 210 | std_img_saving(name) 211 | 212 | 213 | def TAL_PP_speed_profile_8(): 214 | map_name = "f1_esp" 215 | path = "Data/Vehicles/" 216 | # a1 = path + f"Cth_speeds/fast_Std_Std_Cth_{map_name}_6_1_1/" 217 | a2 = path + f"TAL_speeds_old/fast_Std_Std_TAL_{map_name}_8_1_0/" 218 | a3 = path + f"PP_speeds/PP_PP_Std_PP_{map_name}_8_1_0/" 219 | 220 | # colors = ["#2ECC71", "#E67E22"] 221 | colors = [plot_green, plot_red] 222 | vehicles = [a3, a2] 223 | labels = ["Classic", "Trajectory-aided learning (TAL)"] 224 | 225 | fig, (ax1) = plt.subplots(1, 1, figsize=(4.6, 1.7), sharex=True) 226 | for i in range(len(vehicles)): 227 | vehicle = TestLapData(vehicles[i], 1) 228 | xs = vehicle.generate_state_progress_list()*100 229 | ax1.plot(xs, vehicle.states[:, 3], color=colors[i], label=labels[i], linewidth=2) 230 | 231 | ax1.set_ylabel("Speed (m/s)") 232 | ax1.set_xlabel("Track progress (%)") 233 | fig.legend(ncol=3, loc="center", bbox_to_anchor=(0.5, 0)) 234 | ax1.yaxis.set_major_locator(MultipleLocator(2)) 235 | # ax2.set_ylabel("Slip Angle") 236 | 237 | plt.grid(True) 238 | plt.xlim(30, 89) 239 | # plt.xlim(-2, 40) 240 | plt.tight_layout() 241 | 242 | name = "Data/Images/TAL_PP_speed_profile_8" 243 | std_img_saving(name) 244 | 245 | 246 | 247 | # TAL_baseline_PP_slip_speed_6() 248 | TAL_PP_speed_profile_8() 249 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/CthVsProgress_TrainingGraph.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import tikzplotlib 4 | import csv 5 | import glob, os 6 | from matplotlib.ticker import MultipleLocator, PercentFormatter 7 | 8 | from TrajectoryAidedLearning.Utils.utils import * 9 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 10 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 11 | 12 | 13 | 14 | def CthVsProgress_TrainingGraphs(): 15 | p = "Data/Vehicles/CthVsProgress/" 16 | 17 | set_n =1 18 | repeats = 5 19 | speed = 5 20 | 21 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2), sharey=True) 22 | # fig, axs = plt.subplots(1, 2, figsize=(4.8, 1.6), sharey=True) 23 | 24 | xs = np.linspace(0, 100, 300) 25 | 26 | colors = ["#28B463", "#CB4335"] 27 | 28 | for z, map_name in enumerate(["f1_esp", "f1_mco"]): 29 | progress_steps = [] 30 | progress_progresses = [] 31 | cth_steps = [] 32 | cth_progresses = [] 33 | for i in range(repeats): 34 | path_progress = p + f"fast_Std_Std_Progress_{map_name}_{speed}_{set_n}_{i}/" 35 | path_cth = p + f"fast_Std_Std_Cth_{map_name}_{speed}_{set_n}_{i}/" 36 | 37 | rewards_progress, lengths_progress, progresses_progress, _ = load_csv_data(path_progress) 38 | rewards_cth, lengths_cth, progresses_cth, _ = load_csv_data(path_cth) 39 | 40 | steps_progress = np.cumsum(lengths_progress) / 1000 41 | avg_progress_progress = true_moving_average(progresses_progress, 20)* 100 42 | steps_cth = np.cumsum(lengths_cth) / 1000 43 | avg_progress_cth = true_moving_average(progresses_cth, 20) * 100 44 | 45 | progress_steps.append(steps_progress) 46 | progress_progresses.append(avg_progress_progress) 47 | cth_steps.append(steps_cth) 48 | cth_progresses.append(avg_progress_cth) 49 | 50 | 51 | min_progress, max_progress, mean_progress = convert_to_min_max_avg(progress_steps, progress_progresses, xs) 52 | min_cth, max_cth, mean_cth = convert_to_min_max_avg(cth_steps, cth_progresses, xs) 53 | 54 | alpha = 0.3 55 | if map_name == "f1_esp": 56 | axs[z].set_title("ESP") 57 | axs[z].plot(xs, mean_progress, '-', color=colors[0], linewidth=2, label='Progress') 58 | axs[z].fill_between(xs, min_progress, max_progress, color=colors[0], alpha=alpha) 59 | axs[z].plot(xs, mean_cth, '-', color=colors[1], linewidth=2, label='Cth') 60 | axs[z].fill_between(xs, min_cth, max_cth, color=colors[1], alpha=alpha) 61 | axs[z].yaxis.set_major_locator(MultipleLocator(25)) 62 | axs[z].grid(True) 63 | axs[z].set_xlabel("Training Steps (x1000)") 64 | else: 65 | axs[z].set_title("MCO") 66 | axs[z].plot(xs, mean_progress, '-', color=colors[0], linewidth=2) 67 | axs[z].fill_between(xs, min_progress, max_progress, color=colors[0], alpha=alpha) 68 | axs[z].plot(xs, mean_cth, '-', color=colors[1], linewidth=2) 69 | axs[z].fill_between(xs, min_cth, max_cth, color=colors[1], alpha=alpha) 70 | axs[z].yaxis.set_major_locator(MultipleLocator(25)) 71 | axs[z].grid(True) 72 | axs[z].set_xlabel("Training Steps (x1000)") 73 | 74 | axs[z].xaxis.set_major_locator(MultipleLocator(25)) 75 | 76 | plt.ylim(0, 100) 77 | axs[0].set_ylabel("Track Progress %") 78 | fig.legend(loc='center', ncol=2, bbox_to_anchor=(0.5, -0.00), framealpha=0.98) 79 | 80 | name = p + f"CthVsProgress_Training_{speed}" 81 | std_img_saving(name) 82 | 83 | 84 | 85 | CthVsProgress_TrainingGraphs() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/Cth_TAL_maps_RewardTrainingGraph.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | 10 | 11 | 12 | 13 | def Cth_TAL_speeds_TrainingGraph_small(): 14 | p = "Data/Vehicles/Cth_maps/" 15 | 16 | steps_list = [] 17 | progresses_list = [] 18 | 19 | n_repeats = 5 20 | v = 6 21 | map_names = ['aut', "esp", "gbr", "mco"] 22 | for i, m in enumerate(map_names): 23 | steps_list.append([]) 24 | progresses_list.append([]) 25 | for j in range(n_repeats): 26 | path = p + f"fast_Std_Std_Cth_f1_{m}_{v}_1_{j}/" 27 | rewards, lengths, progresses, _ = load_csv_data(path) 28 | steps = np.cumsum(lengths[:-1]) / 1000 29 | avg_progress = true_moving_average(rewards[:-1], 20) 30 | steps_list[i].append(steps) 31 | progresses_list[i].append(avg_progress) 32 | 33 | fig, axs = plt.subplots(1, 2, figsize=(5.4, 2.2), sharey=False) 34 | plt.sca(axs[0]) 35 | 36 | labels = [i.upper() for i in map_names] 37 | 38 | xs = np.linspace(0, 100, 300) 39 | for i in range(len(steps_list)): 40 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 41 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 42 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 43 | plt.xlabel("Training Steps (x1000)") 44 | plt.grid(True) 45 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 46 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(40)) 47 | plt.ylabel("Ep. Reward") 48 | plt.title("Baseline") 49 | plt.ylim(0, 180) 50 | 51 | p = "Data/Vehicles/TAL_maps/" 52 | 53 | steps_list = [] 54 | progresses_list = [] 55 | 56 | n_repeats = 5 57 | for i, m in enumerate(map_names): 58 | steps_list.append([]) 59 | progresses_list.append([]) 60 | for j in range(n_repeats): 61 | path = p + f"fast_Std_Std_TAL_f1_{m}_{v}_1_{j}/" 62 | rewards, lengths, progresses, _ = load_csv_data(path) 63 | steps = np.cumsum(lengths[:-1]) / 1000 64 | avg_progress = true_moving_average(rewards[:-1], 20) 65 | steps_list[i].append(steps) 66 | progresses_list[i].append(avg_progress) 67 | 68 | plt.sca(axs[1]) 69 | xs = np.linspace(0, 100, 300) 70 | for i in range(len(steps_list)): 71 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 72 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2) 73 | # plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 74 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 75 | plt.xlabel("Training Steps (x1000)") 76 | # plt.ylabel("Ep. Reward") 77 | plt.title("TAL") 78 | 79 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(20)) 80 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 81 | 82 | plt.ylim(0, 70) 83 | # plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 84 | 85 | fig.legend(loc='center', bbox_to_anchor=(0.5, -0.), ncol=5) 86 | plt.tight_layout() 87 | plt.grid(True) 88 | 89 | name = p + f"Cth_TAL_maps_RewardTrainingGraph" 90 | std_img_saving(name) 91 | 92 | Cth_TAL_speeds_TrainingGraph_small() 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/Cth_TAL_speeds_TrainingAnimation.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | import matplotlib.animation as animation 10 | 11 | 12 | 13 | def Cth_TAL_speeds_TrainingGraph_small(): 14 | p = "Data/Vehicles/Cth_speeds/" 15 | 16 | cth_steps_list = [] 17 | cth_progresses_list = [] 18 | 19 | n_repeats = 4 20 | speed_range = (4, 6, 8) 21 | for i, v in enumerate(speed_range): 22 | cth_steps_list.append([]) 23 | cth_progresses_list.append([]) 24 | for j in range(n_repeats): 25 | path = p + f"fast_Std_Std_Cth_f1_esp_{v}_1_{j}/" 26 | rewards, lengths, progresses, _ = load_csv_data(path) 27 | steps = np.cumsum(lengths[:-1]) / 1000 28 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 29 | cth_steps_list[i].append(steps) 30 | cth_progresses_list[i].append(avg_progress) 31 | 32 | # plt.figure(2, figsize=(4.5, 2.3)) 33 | 34 | p = "Data/Vehicles/TAL_speeds/" 35 | 36 | tal_steps_list = [] 37 | tal_progresses_list = [] 38 | 39 | n_repeats = 4 40 | for i, v in enumerate(speed_range): 41 | tal_steps_list.append([]) 42 | tal_progresses_list.append([]) 43 | for j in range(n_repeats): 44 | path = p + f"fast_Std_Std_TAL_f1_esp_{v}_1_{j}/" 45 | rewards, lengths, progresses, _ = load_csv_data(path) 46 | steps = np.cumsum(lengths[:-1]) / 1000 47 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 48 | tal_steps_list[i].append(steps) 49 | tal_progresses_list[i].append(avg_progress) 50 | 51 | fig, axs = plt.subplots(1, 2, figsize=(8, 3), sharey=True) 52 | # fig, axs = plt.subplots(1, 2, figsize=(5.4, 2.2), sharey=True) 53 | 54 | plt.sca(axs[0]) 55 | plt.xlabel("Training Steps (x1000)") 56 | plt.grid(True) 57 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 58 | plt.ylabel("Track Progress %") 59 | plt.title("Baseline") 60 | plt.xlim(-4, 100) 61 | 62 | plt.sca(axs[1]) 63 | plt.xlabel("Training Steps (x1000)") 64 | plt.title("Trajectory-aided Learning (ours)", fontdict={'weight': 'bold'}) 65 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 66 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 67 | plt.ylim(0, 105) 68 | plt.grid(True) 69 | 70 | plt.xlim(-4, 100) 71 | 72 | labels = ['4 m/s', '6 m/s', '8 m/s'] 73 | # labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 74 | 75 | new_colors = [pp[0], pp[2], pp[4]] 76 | 77 | xs = np.linspace(0, 100, 300) 78 | line1s = [] 79 | line2s = [] 80 | for i in range(len(cth_steps_list)): 81 | plt.sca(axs[0]) 82 | min, max, mean = convert_to_min_max_avg_iqm5(cth_steps_list[i], cth_progresses_list[i], xs) 83 | line1 = axs[0].plot(xs[0], mean[0], '-', color=new_colors[i], linewidth=3, label=labels[i]) 84 | line1s.append(line1) 85 | # plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 86 | 87 | 88 | plt.sca(axs[1]) 89 | min, max, mean = convert_to_min_max_avg_iqm5(tal_steps_list[i], tal_progresses_list[i], xs) 90 | line2 = axs[1].plot(xs[0], mean[0], '-', color=new_colors[i], linewidth=3) 91 | # plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 92 | line2s.append(line2) 93 | 94 | fig.legend(loc='lower center', bbox_to_anchor=(0.5, 0), ncol=5) 95 | plt.tight_layout() 96 | fig.subplots_adjust(bottom=0.25) 97 | 98 | def update(frame): 99 | end_ind = frame * 5 100 | for i in range(len(cth_steps_list)): 101 | # plt.sca(axs[0]) 102 | min, max, mean = convert_to_min_max_avg_iqm5(cth_steps_list[i], cth_progresses_list[i], xs) 103 | line1s[i][0].set_xdata(xs[:end_ind]) 104 | line1s[i][0].set_ydata(mean[:end_ind]) 105 | # plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 106 | 107 | 108 | # plt.sca(axs[1]) 109 | min, max, mean = convert_to_min_max_avg_iqm5(tal_steps_list[i], tal_progresses_list[i], xs) 110 | line2s[i][0].set_xdata(xs[:end_ind]) 111 | line2s[i][0].set_ydata(mean[:end_ind]) 112 | # plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 113 | 114 | ani = animation.FuncAnimation(fig=fig, func=update, frames=60, interval=50) 115 | # plt.show() 116 | 117 | ani.save("Data/animation.gif", writer='pillow') 118 | # ani.save("Data/animation.webp", writer='pillow') 119 | 120 | Cth_TAL_speeds_TrainingGraph_small() 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/Cth_TAL_speeds_TrainingGraph.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | 10 | 11 | 12 | 13 | def Cth_TAL_speeds_TrainingGraph_small(): 14 | p = "Data/Vehicles/Cth_speeds/" 15 | 16 | steps_list = [] 17 | progresses_list = [] 18 | 19 | n_repeats = 4 20 | for i, v in enumerate(range(4, 9)): 21 | steps_list.append([]) 22 | progresses_list.append([]) 23 | for j in range(n_repeats): 24 | path = p + f"fast_Std_Std_Cth_f1_esp_{v}_1_{j}/" 25 | rewards, lengths, progresses, _ = load_csv_data(path) 26 | steps = np.cumsum(lengths[:-1]) / 1000 27 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 28 | steps_list[i].append(steps) 29 | progresses_list[i].append(avg_progress) 30 | 31 | # plt.figure(2, figsize=(4.5, 2.3)) 32 | fig, axs = plt.subplots(1, 2, figsize=(5.4, 2.2), sharey=True) 33 | plt.sca(axs[0]) 34 | 35 | labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 36 | 37 | xs = np.linspace(0, 100, 300) 38 | for i in range(len(steps_list)): 39 | # min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 40 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 41 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 42 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 43 | plt.xlabel("Training Steps (x1000)") 44 | plt.grid(True) 45 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 46 | plt.ylabel("Track Progress %") 47 | plt.title("Baseline") 48 | 49 | p = "Data/Vehicles/TAL_speeds/" 50 | 51 | steps_list = [] 52 | progresses_list = [] 53 | 54 | n_repeats = 4 55 | for i, v in enumerate(range(4, 9)): 56 | steps_list.append([]) 57 | progresses_list.append([]) 58 | for j in range(n_repeats): 59 | path = p + f"fast_Std_Std_TAL_f1_esp_{v}_1_{j}/" 60 | rewards, lengths, progresses, _ = load_csv_data(path) 61 | steps = np.cumsum(lengths[:-1]) / 1000 62 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 63 | steps_list[i].append(steps) 64 | progresses_list[i].append(avg_progress) 65 | 66 | plt.sca(axs[1]) 67 | xs = np.linspace(0, 100, 300) 68 | for i in range(len(steps_list)): 69 | # min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 70 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 71 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2) 72 | # plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 73 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 74 | plt.xlabel("Training Steps (x1000)") 75 | plt.title("TAL") 76 | 77 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 78 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 79 | 80 | plt.ylim(0, 100) 81 | # plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 82 | 83 | fig.legend(loc='center', bbox_to_anchor=(0.5, -0.), ncol=5) 84 | plt.tight_layout() 85 | plt.grid(True) 86 | 87 | name = p + f"Cth_TAL_speeds_TrainingGraph" 88 | std_img_saving(name) 89 | 90 | Cth_TAL_speeds_TrainingGraph_small() 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/Cth_speeds_TrainingGraph.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | 10 | 11 | def Cth_speeds_TrainingGraph(): 12 | p = "Data/Vehicles/Cth_speeds/" 13 | 14 | steps_list = [] 15 | progresses_list = [] 16 | 17 | n_repeats = 5 18 | for i, v in enumerate(range(4, 9)): 19 | steps_list.append([]) 20 | progresses_list.append([]) 21 | for j in range(n_repeats): 22 | path = p + f"fast_Std_Std_Cth_f1_esp_{v}_1_{j}/" 23 | rewards, lengths, progresses, _ = load_csv_data(path) 24 | steps = np.cumsum(lengths[:-1]) / 1000 25 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 26 | steps_list[i].append(steps) 27 | progresses_list[i].append(avg_progress) 28 | 29 | plt.figure(2, figsize=(4.5, 2.3)) 30 | 31 | # labels = ["4", "5", "6", "7", "8"] 32 | labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 33 | 34 | xs = np.linspace(0, 100, 300) 35 | for i in range(len(steps_list)): 36 | min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 37 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 38 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 39 | 40 | 41 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 42 | 43 | plt.xlabel("Training Steps (x1000)") 44 | plt.ylabel("Track Progress %") 45 | plt.ylim(0, 100) 46 | # plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 47 | plt.legend(loc='center', bbox_to_anchor=(0.5, -0.52), ncol=3) 48 | plt.tight_layout() 49 | plt.grid() 50 | 51 | name = p + f"Cth_speeds_TrainingGraph" 52 | std_img_saving(name) 53 | 54 | def Cth_speeds_TrainingGraph_small(): 55 | p = "Data/Vehicles/Cth_speeds/" 56 | 57 | steps_list = [] 58 | progresses_list = [] 59 | 60 | n_repeats = 5 61 | for i, v in enumerate(range(4, 9)): 62 | steps_list.append([]) 63 | progresses_list.append([]) 64 | for j in range(n_repeats): 65 | path = p + f"fast_Std_Std_Cth_f1_esp_{v}_1_{j}/" 66 | rewards, lengths, progresses, _ = load_csv_data(path) 67 | steps = np.cumsum(lengths[:-1]) / 1000 68 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 69 | steps_list[i].append(steps) 70 | progresses_list[i].append(avg_progress) 71 | 72 | plt.figure(2, figsize=(4.5, 2.3)) 73 | 74 | # labels = ["4", "5", "6", "7", "8"] 75 | labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 76 | 77 | xs = np.linspace(0, 100, 300) 78 | for i in range(len(steps_list)): 79 | min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 80 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 81 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 82 | 83 | 84 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 85 | 86 | plt.xlabel("Training Steps (x1000)") 87 | plt.ylabel("Track Progress %") 88 | plt.ylim(0, 100) 89 | # plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 90 | plt.legend(loc='center', bbox_to_anchor=(0.5, -0.52), ncol=3) 91 | plt.tight_layout() 92 | plt.grid() 93 | 94 | name = p + f"Cth_speeds_TrainingGraph" 95 | std_img_saving(name) 96 | 97 | # Cth_speeds_TrainingGraph() 98 | Cth_speeds_TrainingGraph_small() 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/TAL_Cth_maps_Training.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import tikzplotlib 4 | import csv 5 | import glob, os 6 | from matplotlib.ticker import MultipleLocator, PercentFormatter 7 | 8 | from TrajectoryAidedLearning.Utils.utils import * 9 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 10 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 11 | 12 | 13 | 14 | def CthVsTal_training(): 15 | p = "Data/Vehicles/CthAndTal6/" 16 | 17 | set_n =1 18 | repeats = 5 19 | speed = 6 20 | 21 | plt.figure(1, figsize=(4.8, 2.0)) 22 | plt.clf() 23 | 24 | xs = np.linspace(0, 100, 300) 25 | 26 | colors = ["#D68910", "#884EA0"] 27 | map_name = "f1_esp" 28 | 29 | tal_steps = [] 30 | tal_progresses = [] 31 | cth_steps = [] 32 | cth_progresses = [] 33 | for i in range(repeats): 34 | path_tal = p + f"fast_Std_Std_TAL_{map_name}_{speed}_{set_n}_{i}/" 35 | path_cth = p + f"fast_Std_Std_Cth_{map_name}_{speed}_{set_n}_{i}/" 36 | 37 | rewards_tal, lengths_tal, progresses_tal, _ = load_csv_data(path_tal) 38 | rewards_cth, lengths_cth, progresses_cth, _ = load_csv_data(path_cth) 39 | 40 | steps_tal = np.cumsum(lengths_tal) / 1000 41 | avg_progress_tal = true_moving_average(progresses_tal, 20)* 100 42 | steps_cth = np.cumsum(lengths_cth) / 1000 43 | avg_progress_cth = true_moving_average(progresses_cth, 20) * 100 44 | 45 | tal_steps.append(steps_tal) 46 | tal_progresses.append(avg_progress_tal) 47 | cth_steps.append(steps_cth) 48 | cth_progresses.append(avg_progress_cth) 49 | 50 | 51 | min_tal, max_tal, mean_tal = convert_to_min_max_avg(tal_steps, tal_progresses, xs) 52 | min_cth, max_cth, mean_cth = convert_to_min_max_avg(cth_steps, cth_progresses, xs) 53 | 54 | plt.plot(xs, mean_cth, '-', color=colors[1], linewidth=2.5, label='Baseline') 55 | plt.gca().fill_between(xs, min_cth, max_cth, color=colors[1], alpha=0.4) 56 | plt.plot(xs, mean_tal, '-', color=colors[0], linewidth=2.5, label='TAL') 57 | plt.gca().fill_between(xs, min_tal, max_tal, color=colors[0], alpha=0.4) 58 | 59 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 60 | 61 | plt.xlabel("Training Steps (x1000)") 62 | plt.ylabel("Track Progress (%)") 63 | plt.ylim(0, 100) 64 | 65 | plt.legend(loc='lower right', ncol=2) 66 | plt.tight_layout() 67 | plt.grid(True) 68 | 69 | name = p + f"CthVsTal_training{speed}" 70 | std_img_saving(name) 71 | 72 | 73 | def TAL_Cth_maps_TrainingGraph(): 74 | p_cth = "Data/Vehicles/Cth_maps/" 75 | p_tal = "Data/Vehicles/TAL_maps/" 76 | 77 | set_n =1 78 | repeats = 5 79 | speed = 6 80 | 81 | fig, axs = plt.subplots(1, 2, figsize=(4.5, 2), sharey=True) 82 | 83 | xs = np.linspace(0, 100, 300) 84 | 85 | colors = ["#D68910", "#884EA0"] 86 | map_names = ["f1_esp", "f1_gbr"] 87 | for z, map_name in enumerate(map_names): 88 | 89 | tal_steps = [] 90 | tal_progresses = [] 91 | cth_steps = [] 92 | cth_progresses = [] 93 | for i in range(repeats): 94 | path_tal = p_tal + f"fast_Std_Std_TAL_{map_name}_{speed}_{set_n}_{i}/" 95 | path_cth = p_cth + f"fast_Std_Std_Cth_{map_name}_{speed}_{set_n}_{i}/" 96 | 97 | rewards_tal, lengths_tal, progresses_tal, _ = load_csv_data(path_tal) 98 | rewards_cth, lengths_cth, progresses_cth, _ = load_csv_data(path_cth) 99 | 100 | steps_tal = np.cumsum(lengths_tal) / 1000 101 | avg_progress_tal = true_moving_average(progresses_tal, 20)* 100 102 | steps_cth = np.cumsum(lengths_cth) / 1000 103 | avg_progress_cth = true_moving_average(progresses_cth, 20) * 100 104 | 105 | tal_steps.append(steps_tal) 106 | tal_progresses.append(avg_progress_tal) 107 | cth_steps.append(steps_cth) 108 | cth_progresses.append(avg_progress_cth) 109 | 110 | 111 | min_tal, max_tal, mean_tal = convert_to_min_max_avg(tal_steps, tal_progresses, xs) 112 | min_cth, max_cth, mean_cth = convert_to_min_max_avg(cth_steps, cth_progresses, xs) 113 | 114 | plt.sca(axs[z]) 115 | plt.plot(xs, mean_cth, '-', color=colors[1], linewidth=2.5, label='Baseline') 116 | plt.gca().fill_between(xs, min_cth, max_cth, color=colors[1], alpha=0.4) 117 | plt.plot(xs, mean_tal, '-', color=colors[0], linewidth=2.5, label='TAL') 118 | plt.gca().fill_between(xs, min_tal, max_tal, color=colors[0], alpha=0.4) 119 | 120 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 121 | plt.gca().get_xaxis().set_major_locator(MultipleLocator(25)) 122 | 123 | plt.xlabel("Training Steps (x1000)") 124 | plt.grid(True) 125 | 126 | axs[0].set_ylabel("Track Progress (%)") 127 | axs[0].set_ylim(0, 100) 128 | axs[0].set_title("ESP") 129 | axs[1].set_title("GBR") 130 | 131 | 132 | handles, labels = axs[0].get_legend_handles_labels() 133 | fig.legend(handles, labels, loc='center', ncol=2, bbox_to_anchor=(0.5, 0.), framealpha=0.95) 134 | # fig.legend(loc='center', ncol=2) 135 | 136 | name = "Data/Images/" + f"TAL_Cth_maps_TrainingGraph" 137 | std_img_saving(name) 138 | 139 | 140 | 141 | TAL_Cth_maps_TrainingGraph() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/TAL_maps_TrainingGraphs.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | 10 | 11 | 12 | 13 | 14 | def TAL_Maps_TrainingProgress(): 15 | p = "Data/Vehicles/TAL_maps8/" 16 | 17 | steps_list = [] 18 | progresses_list = [] 19 | map_names = ["f1_esp", "f1_mco", "f1_gbr", "f1_aut"] 20 | 21 | n_repeats = 5 22 | for i, map_name in enumerate(map_names): 23 | steps_list.append([]) 24 | progresses_list.append([]) 25 | for j in range(n_repeats): 26 | path = p + f"fast_Std_Std_TAL_{map_name}_8_1_{j}/" 27 | rewards, lengths, progresses, _ = load_csv_data(path) 28 | steps = np.cumsum(lengths[:-1]) / 1000 29 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 30 | steps_list[i].append(steps) 31 | progresses_list[i].append(avg_progress) 32 | 33 | plt.figure(2, figsize=(4.5, 2.1)) 34 | 35 | # labels = ["4", "5", "6", "7", "8"] 36 | labels = ['ESP', "MCO", "GBR", "AUT"] 37 | # labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 38 | 39 | xs = np.linspace(0, 100, 300) 40 | for i in range(len(steps_list)): 41 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 42 | # min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 43 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 44 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 45 | 46 | 47 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 48 | 49 | plt.xlabel("Training Steps (x1000)") 50 | plt.ylabel("Track Progress %") 51 | plt.ylim(0, 100) 52 | plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 53 | # plt.legend(loc='center', bbox_to_anchor=(0.5, 1.2), ncol=5) 54 | plt.tight_layout() 55 | plt.grid() 56 | 57 | name = p + f"Eval_Maps_TrainingProgress" 58 | std_img_saving(name) 59 | 60 | 61 | TAL_Maps_TrainingProgress() 62 | 63 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/TAL_speeds_TrainingGraph.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import MultipleLocator, PercentFormatter 5 | 6 | from TrajectoryAidedLearning.Utils.utils import * 7 | from TrajectoryAidedLearning.DataTools.TrainingGraphs.TrainingUtils import * 8 | from TrajectoryAidedLearning.DataTools.plotting_utils import * 9 | 10 | 11 | def TAL_Speeds_TrainingGraph(): 12 | # p = "Data/Vehicles/TAL_speedsN/" 13 | p = "Data/Vehicles/TAL_speedsRetry1/" 14 | 15 | steps_list = [] 16 | progresses_list = [] 17 | 18 | n_repeats = 5 19 | # for i, v in enumerate(range(7, 9)): 20 | for i, v in enumerate(range(4, 9)): 21 | steps_list.append([]) 22 | progresses_list.append([]) 23 | for j in range(n_repeats): 24 | path = p + f"fast_Std_Std_TAL_f1_esp_{v}_1_{j}/" 25 | rewards, lengths, progresses, _ = load_csv_data(path) 26 | steps = np.cumsum(lengths[:-1]) / 1000 27 | avg_progress = true_moving_average(progresses[:-1], 20)* 100 28 | steps_list[i].append(steps) 29 | progresses_list[i].append(avg_progress) 30 | 31 | plt.figure(2, figsize=(4.5, 2.3)) 32 | 33 | # labels = ["4", "5", "6", "7", "8"] 34 | labels = ['4 m/s', '5 m/s', '6 m/s', '7 m/s', '8 m/s'] 35 | 36 | xs = np.linspace(0, 100, 300) 37 | for i in range(len(steps_list)): 38 | # min, max, mean = convert_to_min_max_avg(steps_list[i], progresses_list[i], xs) 39 | min, max, mean = convert_to_min_max_avg_iqm5(steps_list[i], progresses_list[i], xs) 40 | plt.plot(xs, mean, '-', color=pp[i], linewidth=2, label=labels[i]) 41 | plt.gca().fill_between(xs, min, max, color=pp[i], alpha=0.2) 42 | #TODO: add filling for the IQR 43 | 44 | plt.gca().get_yaxis().set_major_locator(MultipleLocator(25)) 45 | 46 | plt.xlabel("Training Steps (x1000)") 47 | plt.ylabel("Track Progress %") 48 | plt.ylim(0, 100) 49 | # plt.legend(loc='center', bbox_to_anchor=(1.06, 0.5), ncol=1) 50 | # plt.legend(loc='center', bbox_to_anchor=(0.5, 1.2), ncol=5) 51 | plt.legend(loc='center', bbox_to_anchor=(0.5, -0.52), ncol=3) 52 | plt.tight_layout() 53 | plt.grid() 54 | 55 | name = p + f"TAL_Speed_TrainingGraph" 56 | std_img_saving(name) 57 | 58 | 59 | TAL_Speeds_TrainingGraph() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/TrainingGraphs/TrainingUtils.py: -------------------------------------------------------------------------------- 1 | import csv 2 | import numpy as np 3 | from matplotlib import pyplot as plt 4 | 5 | def load_csv_data(path): 6 | """loads data from a csv training file 7 | 8 | Args: 9 | path (file_path): path to the agent 10 | 11 | Returns: 12 | rewards: ndarray of rewards 13 | lengths: ndarray of episode lengths 14 | progresses: ndarray of track progresses 15 | laptimes: ndarray of laptimes 16 | """ 17 | rewards, lengths, progresses, laptimes = [], [], [], [] 18 | with open(f"{path}training_data_episodes.csv", "r") as f: 19 | reader = csv.reader(f) 20 | for row in reader: 21 | if float(row[2]) > 0: 22 | rewards.append(float(row[1])) 23 | lengths.append(float(row[2])) 24 | progresses.append(float(row[3])) 25 | laptimes.append(float(row[4])) 26 | 27 | rewards = np.array(rewards)[:-1] 28 | lengths = np.array(lengths)[:-1] 29 | progresses = np.array(progresses)[:-1] 30 | laptimes = np.array(laptimes)[:-1] 31 | 32 | return rewards, lengths, progresses, laptimes 33 | 34 | def convert_to_min_max_avg(step_list, progress_list, xs): 35 | """Returns the 3 lines 36 | - Minimum line 37 | - maximum line 38 | - average line 39 | """ 40 | n = len(step_list) 41 | 42 | # xs = np.arange(length_xs) 43 | ys = np.zeros((n, len(xs))) 44 | # xs = np.linspace(0, x_lim, length_xs) 45 | # xs = np.linspace(step_list[0][0], step_list[0][-1], length_xs) 46 | for i in range(n): 47 | ys[i] = np.interp(xs, step_list[i], progress_list[i]) 48 | 49 | min_line = np.min(ys, axis=0) 50 | max_line = np.max(ys, axis=0) 51 | avg_line = np.mean(ys, axis=0) 52 | 53 | return min_line, max_line, avg_line 54 | 55 | def convert_to_min_max_avg(step_list, progress_list, xs): 56 | """Returns the 3 lines 57 | - Minimum line 58 | - maximum line 59 | - average line 60 | """ 61 | n = len(step_list) 62 | 63 | ys = np.zeros((n, len(xs))) 64 | for i in range(n): 65 | ys[i] = np.interp(xs, step_list[i], progress_list[i]) 66 | 67 | # iqr = np.iqr(ys, axis=0) 68 | 69 | min_line = np.min(ys, axis=0) 70 | max_line = np.max(ys, axis=0) 71 | avg_line = np.mean(ys, axis=0) 72 | 73 | return min_line, max_line, avg_line 74 | 75 | def convert_to_min_max_avg_iqm5(step_list, progress_list, xs): 76 | """Returns the 3 lines 77 | - Minimum line 78 | - maximum line 79 | - average line 80 | """ 81 | n = len(step_list) 82 | 83 | ys = np.zeros((n, len(xs))) 84 | for i in range(n): 85 | ys[i] = np.interp(xs, step_list[i], progress_list[i]) 86 | 87 | iq_data = np.zeros((ys.shape[1], 2)) 88 | for i in range(ys.shape[1]): 89 | data = ys[:, i] 90 | min_ind = np.where(data == np.min(data)) 91 | data = np.delete(data, min_ind[0][0]) 92 | max_ind = np.where(data == np.max(data)) 93 | data = np.delete(data, max_ind[0]) 94 | iq_data[i] = data 95 | 96 | iq_data = iq_data.T # for formatting 97 | min_line = np.min(iq_data, axis=0) 98 | max_line = np.max(iq_data, axis=0) 99 | avg_line = np.mean(iq_data, axis=0) 100 | 101 | return min_line, max_line, avg_line 102 | 103 | def smooth_line(steps, progresses, length_xs=300): 104 | xs = np.linspace(steps[0], steps[-1], length_xs) 105 | smooth_line = np.interp(xs, steps, progresses) 106 | 107 | return xs, smooth_line 108 | 109 | 110 | 111 | ### ---- 112 | 113 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/DataTools/plotting_utils.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import glob 4 | 5 | 6 | 7 | def std_img_saving(name): 8 | 9 | plt.rcParams['pdf.use14corefonts'] = True 10 | 11 | plt.tight_layout() 12 | plt.grid(True) 13 | plt.savefig(name + ".pdf", bbox_inches='tight', pad_inches=0) 14 | plt.savefig(name + ".svg", bbox_inches='tight', pad_inches=0) 15 | # new_name = "Data/UploadImgs2/" + name.split("/")[-1] 16 | # plt.savefig(new_name + ".pdf", bbox_inches='tight', pad_inches=0) 17 | # plt.savefig(new_name + ".svg", bbox_inches='tight', pad_inches=0) 18 | 19 | 20 | 21 | 22 | def load_time_data(folder, map_name=""): 23 | files = glob.glob(folder + f"Results_*{map_name}*.txt") 24 | files.sort() 25 | print(files) 26 | keys = ["time", "success", "progress"] 27 | mins, maxes, means = {}, {}, {} 28 | for key in keys: 29 | mins[key] = [] 30 | maxes[key] = [] 31 | means[key] = [] 32 | 33 | for i in range(len(files)): 34 | with open(files[i], 'r') as file: 35 | lines = file.readlines() 36 | for j in range(len(keys)): 37 | mins[keys[j]].append(float(lines[3].split(",")[1+j])) 38 | maxes[keys[j]].append(float(lines[4].split(",")[1+j])) 39 | means[keys[j]].append(float(lines[1].split(",")[1+j])) 40 | 41 | return mins, maxes, means 42 | 43 | 44 | 45 | pp_light = ["#EC7063", "#5499C7", "#58D68D", "#F4D03F", "#AF7AC5", "#F5B041", "#EB984E"] 46 | pp_dark = ["#943126", "#1A5276", "#1D8348", "#9A7D0A", "#633974", "#9C640C"] 47 | pp_darkest = ["#78281F", "#154360", "#186A3B", "#7D6608", "#512E5F", "#7E5109"] 48 | 49 | light_blue = "#5DADE2" 50 | dark_blue = "#154360" 51 | light_red = "#EC7063" 52 | dark_red = "#78281F" 53 | light_green = "#58D68D" 54 | dark_green = "#186A3B" 55 | 56 | light_purple = "#AF7AC5" 57 | light_yellow = "#F7DC6F" 58 | 59 | plot_green = "#2ECC71" 60 | plot_red = "#E74C3C" 61 | plot_blue = "#3498DB" 62 | 63 | def plot_error_bars(x_base, mins, maxes, dark_color, w): 64 | for i in range(len(x_base)): 65 | xs = [x_base[i], x_base[i]] 66 | ys = [mins[i], maxes[i]] 67 | plt.plot(xs, ys, color=dark_color, linewidth=2) 68 | xs = [x_base[i]-w, x_base[i]+w] 69 | y1 = [mins[i], mins[i]] 70 | y2 = [maxes[i], maxes[i]] 71 | plt.plot(xs, y1, color=dark_color, linewidth=2) 72 | plt.plot(xs, y2, color=dark_color, linewidth=2) 73 | 74 | 75 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Planners/AgentPlanners.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from TrajectoryAidedLearning.Utils.TD3 import TD3 3 | from TrajectoryAidedLearning.Utils.HistoryStructs import TrainHistory 4 | import torch 5 | from numba import njit 6 | 7 | from TrajectoryAidedLearning.Utils.utils import init_file_struct 8 | from matplotlib import pyplot as plt 9 | 10 | 11 | class FastArchitecture: 12 | def __init__(self, run, conf): 13 | self.state_space = conf.n_beams 14 | self.range_finder_scale = conf.range_finder_scale 15 | self.n_beams = conf.n_beams 16 | self.max_speed = run.max_speed 17 | self.max_steer = conf.max_steer 18 | 19 | self.action_space = 2 20 | 21 | self.n_scans = run.n_scans 22 | self.scan_buffer = np.zeros((self.n_scans, self.n_beams)) 23 | self.state_space *= self.n_scans 24 | 25 | def transform_obs(self, obs): 26 | """ 27 | Transforms the observation received from the environment into a vector which can be used with a neural network. 28 | 29 | Args: 30 | obs: observation from env 31 | 32 | Returns: 33 | nn_obs: observation vector for neural network 34 | """ 35 | 36 | scan = np.array(obs['scan']) 37 | 38 | scaled_scan = scan/self.range_finder_scale 39 | scan = np.clip(scaled_scan, 0, 1) 40 | 41 | if self.scan_buffer.all() ==0: # first reading 42 | for i in range(self.n_scans): 43 | self.scan_buffer[i, :] = scan 44 | else: 45 | self.scan_buffer = np.roll(self.scan_buffer, 1, axis=0) 46 | self.scan_buffer[0, :] = scan 47 | 48 | nn_obs = np.reshape(self.scan_buffer, (self.n_beams * self.n_scans)) 49 | 50 | return nn_obs 51 | 52 | def transform_action(self, nn_action): 53 | steering_angle = nn_action[0] * self.max_steer 54 | speed = (nn_action[1] + 1) * (self.max_speed / 2 - 0.5) + 1 55 | speed = min(speed, self.max_speed) # cap the speed 56 | 57 | action = np.array([steering_angle, speed]) 58 | 59 | return action 60 | 61 | 62 | 63 | class AgentTrainer: 64 | def __init__(self, run, conf): 65 | self.run, self.conf = run, conf 66 | self.name = run.run_name 67 | self.path = conf.vehicle_path + run.path + run.run_name 68 | init_file_struct(self.path) 69 | 70 | self.v_min_plan = conf.v_min_plan 71 | 72 | self.state = None 73 | self.nn_state = None 74 | self.nn_act = None 75 | self.action = None 76 | 77 | self.architecture = FastArchitecture(run, conf) 78 | 79 | self.agent = TD3(self.architecture.state_space, self.architecture.action_space, 1, run.run_name) 80 | self.agent.create_agent(conf.h_size) 81 | 82 | self.t_his = TrainHistory(run, conf) 83 | 84 | self.train = self.agent.train # alias for sss 85 | self.save = self.agent.save # alias for sss 86 | 87 | def plan(self, obs, add_mem_entry=True): 88 | nn_state = self.architecture.transform_obs(obs) 89 | if add_mem_entry: 90 | self.add_memory_entry(obs, nn_state) 91 | 92 | if obs['state'][3] < self.v_min_plan: 93 | self.action = np.array([0, 7]) 94 | return self.action 95 | 96 | if np.isnan(nn_state).any(): 97 | print(f"NAN in state: {nn_state}") 98 | 99 | self.nn_state = nn_state # after to prevent call before check for v_min_plan 100 | self.nn_act = self.agent.act(self.nn_state) 101 | 102 | if np.isnan(self.nn_act).any(): 103 | print(f"NAN in act: {nn_state}") 104 | raise Exception("Unknown NAN in act") 105 | 106 | self.architecture.transform_obs(obs) # to ensure correct PP actions 107 | self.action = self.architecture.transform_action(self.nn_act) 108 | 109 | return self.action 110 | 111 | def add_memory_entry(self, s_prime, nn_s_prime): 112 | if self.nn_state is not None: 113 | self.t_his.add_step_data(s_prime['reward']) 114 | 115 | self.agent.replay_buffer.add(self.nn_state, self.nn_act, nn_s_prime, s_prime['reward'], False) 116 | 117 | def intervention_entry(self, s_prime): 118 | """ 119 | To be called when the supervisor intervenes. 120 | The lap isn't complete, but it is a terminal state 121 | """ 122 | nn_s_prime = self.architecture.transform_obs(s_prime) 123 | if self.nn_state is None: 124 | # print(f"Intervened on first step: RETURNING") 125 | return 126 | self.t_his.add_step_data(s_prime['reward']) 127 | 128 | self.agent.replay_buffer.add(self.nn_state, self.nn_act, nn_s_prime, s_prime['reward'], True) 129 | 130 | def done_entry(self, s_prime): 131 | """ 132 | To be called when ep is done. 133 | """ 134 | nn_s_prime = self.architecture.transform_obs(s_prime) 135 | 136 | self.t_his.lap_done(s_prime['reward'], s_prime['progress'], False) 137 | if self.nn_state is None: 138 | print(f"Crashed on first step: RETURNING") 139 | return 140 | 141 | self.agent.save(self.path) 142 | if np.isnan(self.nn_act).any(): 143 | print(f"NAN in act: {self.nn_act}") 144 | raise Exception("NAN in act") 145 | if np.isnan(nn_s_prime).any(): 146 | print(f"NAN in state: {nn_s_prime}") 147 | raise Exception("NAN in state") 148 | 149 | self.agent.replay_buffer.add(self.nn_state, self.nn_act, nn_s_prime, s_prime['reward'], True) 150 | self.nn_state = None 151 | 152 | def lap_complete(self): 153 | pass 154 | 155 | def save_training_data(self): 156 | self.t_his.print_update(True) 157 | self.t_his.save_csv_data() 158 | self.agent.save(self.path) 159 | 160 | class AgentTester: 161 | def __init__(self, run, conf): 162 | """ 163 | Testing vehicle using the reference modification navigation stack 164 | 165 | Args: 166 | agent_name: name of the agent for saving and reference 167 | conf: namespace with simulation parameters 168 | mod_conf: namespace with modification planner parameters 169 | """ 170 | self.run, self.conf = run, conf 171 | self.v_min_plan = conf.v_min_plan 172 | self.path = conf.vehicle_path + run.path + run.run_name 173 | 174 | self.actor = torch.load(self.path + '/' + run.run_name + "_actor.pth") 175 | 176 | self.architecture = FastArchitecture(run, conf) 177 | 178 | print(f"Agent loaded: {run.run_name}") 179 | 180 | def plan(self, obs): 181 | nn_obs = self.architecture.transform_obs(obs) 182 | 183 | if obs['state'][3] < self.v_min_plan: 184 | self.action = np.array([0, 7]) 185 | return self.action 186 | 187 | nn_obs = torch.FloatTensor(nn_obs.reshape(1, -1)) 188 | nn_action = self.actor(nn_obs).data.numpy().flatten() 189 | self.nn_act = nn_action 190 | 191 | self.action = self.architecture.transform_action(nn_action) 192 | 193 | return self.action 194 | 195 | def lap_complete(self): 196 | pass 197 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Planners/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/TrajectoryAidedLearning/Planners/__init__.py -------------------------------------------------------------------------------- /TrajectoryAidedLearning/TestSimulation.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.f110_gym.f110_env import F110Env 2 | from TrajectoryAidedLearning.Utils.utils import * 3 | from TrajectoryAidedLearning.Utils.HistoryStructs import VehicleStateHistory 4 | 5 | from TrajectoryAidedLearning.Planners.PurePursuit import PurePursuit 6 | from TrajectoryAidedLearning.Planners.AgentPlanners import AgentTester 7 | 8 | 9 | 10 | import torch 11 | import numpy as np 12 | import time 13 | 14 | # settings 15 | SHOW_TRAIN = False 16 | SHOW_TEST = False 17 | # SHOW_TEST = True 18 | VERBOSE = True 19 | LOGGING = True 20 | 21 | 22 | class TestSimulation(): 23 | def __init__(self, run_file: str): 24 | self.run_data = setup_run_list(run_file) 25 | self.conf = load_conf("config_file") 26 | 27 | self.env = None 28 | self.planner = None 29 | 30 | self.n_test_laps = None 31 | self.lap_times = None 32 | self.completed_laps = None 33 | self.prev_obs = None 34 | self.prev_action = None 35 | 36 | self.std_track = None 37 | self.map_name = None 38 | self.reward = None 39 | self.noise_rng = None 40 | 41 | # flags 42 | self.vehicle_state_history = None 43 | 44 | def run_testing_evaluation(self): 45 | for run in self.run_data: 46 | print(run) 47 | print("_________________________________________________________") 48 | print(run.run_name) 49 | print("_________________________________________________________") 50 | seed = run.random_seed + 10*run.n 51 | np.random.seed(seed) # repetition seed 52 | torch.use_deterministic_algorithms(True) 53 | torch.manual_seed(seed) 54 | 55 | if run.noise_std > 0: 56 | self.noise_std = run.noise_std 57 | self.noise_rng = np.random.default_rng(seed=seed) 58 | 59 | self.env = F110Env(map=run.map_name) 60 | self.map_name = run.map_name 61 | 62 | if run.architecture == "PP": 63 | planner = PurePursuit(self.conf, run) 64 | elif run.architecture == "fast": 65 | planner = AgentTester(run, self.conf) 66 | else: raise AssertionError(f"Planner {run.planner} not found") 67 | 68 | if run.test_mode == "Std": self.planner = planner 69 | else: raise AssertionError(f"Test mode {run.test_mode} not found") 70 | 71 | self.vehicle_state_history = VehicleStateHistory(run, "Testing/") 72 | 73 | self.n_test_laps = run.n_test_laps 74 | self.lap_times = [] 75 | self.completed_laps = 0 76 | 77 | eval_dict = self.run_testing() 78 | run_dict = vars(run) 79 | run_dict.update(eval_dict) 80 | 81 | save_conf_dict(run_dict) 82 | 83 | self.env.close_rendering() 84 | 85 | def run_testing(self): 86 | assert self.env != None, "No environment created" 87 | start_time = time.time() 88 | 89 | for i in range(self.n_test_laps): 90 | observation = self.reset_simulation() 91 | 92 | while not observation['colision_done'] and not observation['lap_done']: 93 | action = self.planner.plan(observation) 94 | observation = self.run_step(action) 95 | if SHOW_TEST: self.env.render('human_fast') 96 | 97 | self.planner.lap_complete() 98 | if observation['lap_done']: 99 | if VERBOSE: print(f"Lap {i} Complete in time: {observation['current_laptime']}") 100 | self.lap_times.append(observation['current_laptime']) 101 | self.completed_laps += 1 102 | 103 | if observation['colision_done']: 104 | if VERBOSE: print(f"Lap {i} Crashed in time: {observation['current_laptime']}") 105 | 106 | 107 | if self.vehicle_state_history: self.vehicle_state_history.save_history(i, test_map=self.map_name) 108 | 109 | print(f"Tests are finished in: {time.time() - start_time}") 110 | 111 | success_rate = (self.completed_laps / (self.n_test_laps) * 100) 112 | if len(self.lap_times) > 0: 113 | avg_times, std_dev = np.mean(self.lap_times), np.std(self.lap_times) 114 | else: 115 | avg_times, std_dev = 0, 0 116 | 117 | print(f"Crashes: {self.n_test_laps - self.completed_laps} VS Completes {self.completed_laps} --> {success_rate:.2f} %") 118 | print(f"Lap times Avg: {avg_times} --> Std: {std_dev}") 119 | 120 | eval_dict = {} 121 | eval_dict['success_rate'] = float(success_rate) 122 | eval_dict['avg_times'] = float(avg_times) 123 | eval_dict['std_dev'] = float(std_dev) 124 | 125 | return eval_dict 126 | 127 | # this is an overide 128 | def run_step(self, action): 129 | sim_steps = self.conf.sim_steps 130 | if self.vehicle_state_history: 131 | self.vehicle_state_history.add_action(action) 132 | self.prev_action = action 133 | 134 | sim_steps, done = sim_steps, False 135 | while sim_steps > 0 and not done: 136 | obs, step_reward, done, _ = self.env.step(action[None, :]) 137 | sim_steps -= 1 138 | 139 | observation = self.build_observation(obs, done) 140 | 141 | return observation 142 | 143 | def build_observation(self, obs, done): 144 | """Build observation 145 | 146 | Returns 147 | state: 148 | [0]: x 149 | [1]: y 150 | [2]: yaw 151 | [3]: v 152 | [4]: steering 153 | scan: 154 | Lidar scan beams 155 | 156 | """ 157 | observation = {} 158 | observation['current_laptime'] = obs['lap_times'][0] 159 | observation['scan'] = obs['scans'][0] #TODO: introduce slicing here 160 | 161 | if self.noise_rng: 162 | noise = self.noise_rng.normal(scale=self.noise_std, size=2) 163 | else: noise = np.zeros(2) 164 | pose_x = obs['poses_x'][0] + noise[0] 165 | pose_y = obs['poses_y'][0] + noise[1] 166 | theta = obs['poses_theta'][0] 167 | linear_velocity = obs['linear_vels_x'][0] 168 | steering_angle = obs['steering_deltas'][0] 169 | state = np.array([pose_x, pose_y, theta, linear_velocity, steering_angle]) 170 | 171 | observation['state'] = state 172 | observation['lap_done'] = False 173 | observation['colision_done'] = False 174 | 175 | observation['reward'] = 0.0 176 | if done and obs['lap_counts'][0] == 0: 177 | observation['colision_done'] = True 178 | if self.std_track is not None: 179 | if self.std_track.check_done(observation) and obs['lap_counts'][0] == 0: 180 | observation['colision_done'] = True 181 | 182 | if self.prev_obs is None: observation['progress'] = 0 183 | elif self.prev_obs['lap_done'] == True: observation['progress'] = 0 184 | else: observation['progress'] = max(self.std_track.calculate_progress_percent(state[0:2]), self.prev_obs['progress']) 185 | # self.racing_race_track.plot_vehicle(state[0:2], state[2]) 186 | # taking the max progress 187 | 188 | 189 | if obs['lap_counts'][0] == 1: 190 | observation['lap_done'] = True 191 | 192 | if self.reward: 193 | observation['reward'] = self.reward(observation, self.prev_obs, self.prev_action) 194 | 195 | if self.vehicle_state_history: 196 | self.vehicle_state_history.add_state(obs['full_states'][0]) 197 | 198 | return observation 199 | 200 | def reset_simulation(self): 201 | reset_pose = np.zeros(3)[None, :] 202 | 203 | obs, step_reward, done, _ = self.env.reset(reset_pose) 204 | 205 | if SHOW_TRAIN: self.env.render('human_fast') 206 | 207 | self.prev_obs = None 208 | observation = self.build_observation(obs, done) 209 | # self.prev_obs = observation 210 | if self.std_track is not None: 211 | self.std_track.max_distance = 0.0 212 | 213 | return observation 214 | 215 | 216 | def main(): 217 | # run_file = "PP_speeds" 218 | run_file = "PP_maps8" 219 | # run_file = "Eval_RewardsSlow" 220 | 221 | 222 | sim = TestSimulation(run_file) 223 | sim.run_testing_evaluation() 224 | 225 | 226 | if __name__ == '__main__': 227 | main() 228 | 229 | 230 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/TestSimulationMaps.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.f110_gym.f110_env import F110Env 2 | from TrajectoryAidedLearning.Utils.utils import * 3 | from TrajectoryAidedLearning.Utils.HistoryStructs import VehicleStateHistory 4 | 5 | from TrajectoryAidedLearning.Planners.PurePursuit import PurePursuit 6 | from TrajectoryAidedLearning.Planners.AgentPlanners import AgentTester 7 | 8 | 9 | 10 | import torch 11 | import numpy as np 12 | import time 13 | 14 | # settings 15 | SHOW_TRAIN = False 16 | # SHOW_TEST = True 17 | SHOW_TEST = False 18 | VERBOSE = True 19 | LOGGING = True 20 | 21 | 22 | class TestSimulation(): 23 | def __init__(self, run_file: str): 24 | self.run_data = setup_run_list(run_file) 25 | self.conf = load_conf("config_file") 26 | 27 | self.env = None 28 | self.planner = None 29 | 30 | self.n_test_laps = None 31 | self.lap_times = None 32 | self.completed_laps = None 33 | self.prev_obs = None 34 | self.prev_action = None 35 | 36 | self.std_track = None 37 | self.map_name = None 38 | self.reward = None 39 | self.noise_rng = None 40 | 41 | # flags 42 | self.vehicle_state_history = None 43 | 44 | def run_testing_evaluation(self): 45 | map_names = ["f1_aut", "f1_esp", "f1_gbr", "f1_mco"] 46 | for run in self.run_data: 47 | for map_name in map_names: 48 | test_map = map_name 49 | print(run) 50 | print("_________________________________________________________") 51 | print(run.run_name) 52 | print("_________________________________________________________") 53 | seed = run.random_seed + 10*run.n 54 | np.random.seed(seed) # repetition seed 55 | torch.use_deterministic_algorithms(True) 56 | torch.manual_seed(seed) 57 | 58 | self.env = F110Env(map=test_map) 59 | self.map_name = test_map 60 | 61 | if run.architecture == "PP": 62 | planner = PurePursuit(self.conf, run) 63 | elif run.architecture == "fast": 64 | planner = AgentTester(run, self.conf) 65 | else: raise AssertionError(f"Planner {run.planner} not found") 66 | 67 | if run.test_mode == "Std": self.planner = planner 68 | else: raise AssertionError(f"Test mode {run.test_mode} not found") 69 | 70 | self.vehicle_state_history = VehicleStateHistory(run, f"Testing{test_map[-3:].upper()}/") 71 | 72 | self.n_test_laps = run.n_test_laps 73 | self.lap_times = [] 74 | self.completed_laps = 0 75 | 76 | eval_dict = self.run_testing() 77 | run_dict = vars(run) 78 | run_dict.update(eval_dict) 79 | 80 | save_conf_dict(run_dict, f"testing_results_{test_map[-3:].upper()}") 81 | # 82 | self.env.close_rendering() 83 | 84 | def run_testing(self): 85 | assert self.env != None, "No environment created" 86 | start_time = time.time() 87 | 88 | for i in range(self.n_test_laps): 89 | observation = self.reset_simulation() 90 | 91 | while not observation['colision_done'] and not observation['lap_done']: 92 | action = self.planner.plan(observation) 93 | observation = self.run_step(action) 94 | if SHOW_TEST: self.env.render('human_fast') 95 | 96 | self.planner.lap_complete() 97 | if observation['lap_done']: 98 | if VERBOSE: print(f"Lap {i} Complete in time: {observation['current_laptime']}") 99 | self.lap_times.append(observation['current_laptime']) 100 | self.completed_laps += 1 101 | 102 | if observation['colision_done']: 103 | if VERBOSE: print(f"Lap {i} Crashed in time: {observation['current_laptime']}") 104 | 105 | 106 | if self.vehicle_state_history: self.vehicle_state_history.save_history(i, test_map=self.map_name) 107 | 108 | print(f"Tests are finished in: {time.time() - start_time}") 109 | 110 | success_rate = (self.completed_laps / (self.n_test_laps) * 100) 111 | if len(self.lap_times) > 0: 112 | avg_times, std_dev = np.mean(self.lap_times), np.std(self.lap_times) 113 | else: 114 | avg_times, std_dev = 0, 0 115 | 116 | print(f"Crashes: {self.n_test_laps - self.completed_laps} VS Completes {self.completed_laps} --> {success_rate:.2f} %") 117 | print(f"Lap times Avg: {avg_times} --> Std: {std_dev}") 118 | 119 | eval_dict = {} 120 | eval_dict['success_rate'] = float(success_rate) 121 | eval_dict['avg_times'] = float(avg_times) 122 | eval_dict['std_dev'] = float(std_dev) 123 | 124 | return eval_dict 125 | 126 | # this is an overide 127 | def run_step(self, action): 128 | sim_steps = self.conf.sim_steps 129 | if self.vehicle_state_history: 130 | self.vehicle_state_history.add_action(action) 131 | self.prev_action = action 132 | 133 | sim_steps, done = sim_steps, False 134 | while sim_steps > 0 and not done: 135 | obs, step_reward, done, _ = self.env.step(action[None, :]) 136 | sim_steps -= 1 137 | 138 | observation = self.build_observation(obs, done) 139 | 140 | return observation 141 | 142 | def build_observation(self, obs, done): 143 | """Build observation 144 | 145 | Returns 146 | state: 147 | [0]: x 148 | [1]: y 149 | [2]: yaw 150 | [3]: v 151 | [4]: steering 152 | scan: 153 | Lidar scan beams 154 | 155 | """ 156 | observation = {} 157 | observation['current_laptime'] = obs['lap_times'][0] 158 | observation['scan'] = obs['scans'][0] #TODO: introduce slicing here 159 | 160 | if self.noise_rng: 161 | noise = self.noise_rng.normal(scale=self.noise_std, size=2) 162 | else: noise = np.zeros(2) 163 | pose_x = obs['poses_x'][0] + noise[0] 164 | pose_y = obs['poses_y'][0] + noise[1] 165 | theta = obs['poses_theta'][0] 166 | linear_velocity = obs['linear_vels_x'][0] 167 | steering_angle = obs['steering_deltas'][0] 168 | state = np.array([pose_x, pose_y, theta, linear_velocity, steering_angle]) 169 | 170 | observation['state'] = state 171 | observation['lap_done'] = False 172 | observation['colision_done'] = False 173 | 174 | observation['reward'] = 0.0 175 | if done and obs['lap_counts'][0] == 0: 176 | observation['colision_done'] = True 177 | if self.std_track is not None: 178 | if self.std_track.check_done(observation) and obs['lap_counts'][0] == 0: 179 | observation['colision_done'] = True 180 | 181 | if self.prev_obs is None: observation['progress'] = 0 182 | elif self.prev_obs['lap_done'] == True: observation['progress'] = 0 183 | else: observation['progress'] = max(self.std_track.calculate_progress_percent(state[0:2]), self.prev_obs['progress']) 184 | # self.racing_race_track.plot_vehicle(state[0:2], state[2]) 185 | # taking the max progress 186 | 187 | 188 | if obs['lap_counts'][0] == 1: 189 | observation['lap_done'] = True 190 | 191 | if self.reward: 192 | observation['reward'] = self.reward(observation, self.prev_obs, self.prev_action) 193 | 194 | if self.vehicle_state_history: 195 | self.vehicle_state_history.add_state(obs['full_states'][0]) 196 | 197 | return observation 198 | 199 | def reset_simulation(self): 200 | reset_pose = np.zeros(3)[None, :] 201 | 202 | obs, step_reward, done, _ = self.env.reset(reset_pose) 203 | 204 | if SHOW_TRAIN: self.env.render('human_fast') 205 | 206 | self.prev_obs = None 207 | observation = self.build_observation(obs, done) 208 | # self.prev_obs = observation 209 | if self.std_track is not None: 210 | self.std_track.max_distance = 0.0 211 | 212 | return observation 213 | 214 | 215 | def main(): 216 | run_file = "Cth_maps2" 217 | # run_file = "TAL_maps2" 218 | 219 | 220 | sim = TestSimulation(run_file) 221 | sim.run_testing_evaluation() 222 | 223 | 224 | if __name__ == '__main__': 225 | main() 226 | 227 | 228 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/TrainAgents.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.f110_gym.f110_env import F110Env 2 | from TrajectoryAidedLearning.Utils.utils import * 3 | from TrajectoryAidedLearning.Planners.AgentPlanners import AgentTrainer, AgentTester 4 | import torch 5 | 6 | import numpy as np 7 | import time 8 | from TrajectoryAidedLearning.Utils.RewardSignals import * 9 | from TrajectoryAidedLearning.Utils.StdTrack import StdTrack 10 | 11 | from TrajectoryAidedLearning.Utils.HistoryStructs import VehicleStateHistory 12 | from TrajectoryAidedLearning.TestSimulation import TestSimulation 13 | 14 | # settings 15 | SHOW_TRAIN = False 16 | # SHOW_TRAIN = True 17 | VERBOSE = True 18 | 19 | 20 | 21 | def select_reward_function(run, conf, std_track): 22 | reward = run.reward 23 | if reward == "Progress": 24 | reward_function = ProgressReward(std_track) 25 | elif reward == "Cth": 26 | reward_function = CrossTrackHeadReward(std_track, conf) 27 | elif reward == "TAL": 28 | reward_function = TALearningReward(conf, run) 29 | else: raise Exception("Unknown reward function: " + reward) 30 | 31 | return reward_function 32 | 33 | 34 | class TrainSimulation(TestSimulation): 35 | def __init__(self, run_file): 36 | super().__init__(run_file) 37 | 38 | self.reward = None 39 | self.previous_observation = None 40 | 41 | 42 | def run_training_evaluation(self): 43 | print(self.run_data) 44 | for run in self.run_data: 45 | print(run) 46 | seed = run.random_seed + 10*run.n 47 | np.random.seed(seed) # repetition seed 48 | # torch.set_deterministic(True) 49 | torch.use_deterministic_algorithms(True) 50 | torch.manual_seed(seed) 51 | 52 | self.env = F110Env(map=run.map_name) 53 | self.map_name = run.map_name 54 | self.n_train_steps = run.n_train_steps 55 | 56 | #train 57 | self.std_track = StdTrack(run.map_name) 58 | self.reward = select_reward_function(run, self.conf, self.std_track) 59 | 60 | self.planner = AgentTrainer(run, self.conf) 61 | 62 | self.completed_laps = 0 63 | 64 | self.run_training() 65 | 66 | #Test 67 | self.planner = AgentTester(run, self.conf) 68 | 69 | self.vehicle_state_history = VehicleStateHistory(run, "Testing/") 70 | 71 | self.n_test_laps = run.n_test_laps 72 | 73 | self.lap_times = [] 74 | self.completed_laps = 0 75 | 76 | eval_dict = self.run_testing() 77 | run_dict = vars(run) 78 | run_dict.update(eval_dict) 79 | 80 | save_conf_dict(run_dict) 81 | 82 | conf = vars(self.conf) 83 | conf['path'] = run.path 84 | conf['run_name'] = run.run_name 85 | save_conf_dict(conf, "TrainingConfig") 86 | 87 | self.env.close_rendering() 88 | 89 | def run_training(self): 90 | assert self.env != None, "No environment created" 91 | start_time = time.time() 92 | print(f"Starting Baseline Training: {self.planner.name}") 93 | 94 | lap_counter, crash_counter = 0, 0 95 | observation = self.reset_simulation() 96 | 97 | for i in range(self.n_train_steps): 98 | self.prev_obs = observation 99 | action = self.planner.plan(observation) 100 | observation = self.run_step(action) 101 | 102 | if lap_counter > 0: # don't train on first lap. 103 | self.planner.agent.train() 104 | 105 | if SHOW_TRAIN: self.env.render('human_fast') 106 | 107 | if observation['lap_done'] or observation['colision_done'] or observation['current_laptime'] > self.conf.max_laptime: 108 | self.planner.done_entry(observation) 109 | 110 | if observation['lap_done']: 111 | if VERBOSE: print(f"{i}::Lap Complete {self.completed_laps} -> FinalR: {observation['reward']:.2f} -> LapTime {observation['current_laptime']:.2f} -> TotalReward: {self.planner.t_his.rewards[self.planner.t_his.ptr-1]:.2f} -> Progress: {observation['progress']:.2f}") 112 | 113 | self.completed_laps += 1 114 | 115 | elif observation['colision_done'] or self.std_track.check_done(observation): 116 | 117 | if VERBOSE: print(f"{i}::Crashed -> FinalR: {observation['reward']:.2f} -> LapTime {observation['current_laptime']:.2f} -> TotalReward: {self.planner.t_his.rewards[self.planner.t_his.ptr-1]:.2f} -> Progress: {observation['progress']:.2f}") 118 | crash_counter += 1 119 | 120 | else: 121 | print(f"{i}::LapTime Exceeded -> FinalR: {observation['reward']:.2f} -> LapTime {observation['current_laptime']:.2f} -> TotalReward: {self.planner.t_his.rewards[self.planner.t_his.ptr-1]:.2f} -> Progress: {observation['progress']:.2f}") 122 | 123 | if self.vehicle_state_history: self.vehicle_state_history.save_history(f"train_{lap_counter}", test_map=self.map_name) 124 | lap_counter += 1 125 | 126 | observation = self.reset_simulation() 127 | self.planner.save_training_data() 128 | 129 | 130 | train_time = time.time() - start_time 131 | print(f"Finished Training: {self.planner.name} in {train_time} seconds") 132 | print(f"Crashes: {crash_counter}") 133 | 134 | 135 | print(f"Training finished in: {time.time() - start_time}") 136 | 137 | 138 | 139 | def main(): 140 | # run_file = "Cth_maps" 141 | run_file = "Cth_speeds" 142 | # run_file = "TAL_maps" 143 | # run_file = "TAL_speeds" 144 | # run_file = "Cth_speedMaps" 145 | # run_file = "CthVsProgress" 146 | 147 | 148 | sim = TrainSimulation(run_file) 149 | sim.run_training_evaluation() 150 | 151 | 152 | if __name__ == '__main__': 153 | main() 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/HistoryStructs.py: -------------------------------------------------------------------------------- 1 | import os, shutil 2 | import csv 3 | import numpy as np 4 | from matplotlib import pyplot as plt 5 | from TrajectoryAidedLearning.Utils.utils import * 6 | from matplotlib.ticker import MultipleLocator 7 | 8 | 9 | SIZE = 20000 10 | 11 | 12 | def plot_data(values, moving_avg_period=10, title="Results", figure_n=2): 13 | plt.figure(figure_n) 14 | plt.clf() 15 | plt.title(title) 16 | plt.xlabel('Episode') 17 | plt.ylabel('Duration') 18 | plt.plot(values) 19 | 20 | if len(values) >= moving_avg_period: 21 | moving_avg = true_moving_average(values, moving_avg_period) 22 | plt.plot(moving_avg) 23 | if len(values) >= moving_avg_period*5: 24 | moving_avg = true_moving_average(values, moving_avg_period * 5) 25 | plt.plot(moving_avg) 26 | # plt.pause(0.001) 27 | 28 | 29 | class TrainHistory(): 30 | def __init__(self, run, conf) -> None: 31 | self.path = conf.vehicle_path + run.path + run.run_name 32 | 33 | # training data 34 | self.ptr = 0 35 | self.lengths = np.zeros(SIZE) 36 | self.rewards = np.zeros(SIZE) 37 | self.progresses = np.zeros(SIZE) 38 | self.laptimes = np.zeros(SIZE) 39 | self.t_counter = 0 # total steps 40 | 41 | # espisode data 42 | self.ep_counter = 0 # ep steps 43 | self.ep_reward = 0 44 | 45 | def add_step_data(self, new_r): 46 | self.ep_reward += new_r 47 | self.ep_counter += 1 48 | self.t_counter += 1 49 | 50 | def lap_done(self, reward, progress, show_reward=False): 51 | self.add_step_data(reward) 52 | self.lengths[self.ptr] = self.ep_counter 53 | self.rewards[self.ptr] = self.ep_reward 54 | self.progresses[self.ptr] = progress 55 | self.ptr += 1 56 | 57 | if show_reward: 58 | plt.figure(8) 59 | plt.clf() 60 | plt.plot(self.ep_rewards) 61 | plt.plot(self.ep_rewards, 'x', markersize=10) 62 | plt.title(f"Ep rewards: total: {self.ep_reward:.4f}") 63 | plt.ylim([-1.1, 1.5]) 64 | plt.pause(0.0001) 65 | 66 | self.ep_counter = 0 67 | self.ep_reward = 0 68 | self.ep_rewards = [] 69 | 70 | def print_update(self, plot_reward=True): 71 | if self.ptr < 10: 72 | return 73 | 74 | mean10 = np.mean(self.rewards[self.ptr-10:self.ptr]) 75 | mean100 = np.mean(self.rewards[max(0, self.ptr-100):self.ptr]) 76 | # print(f"Run: {self.t_counter} --> Moving10: {mean10:.2f} --> Moving100: {mean100:.2f} ") 77 | 78 | if plot_reward: 79 | # raise NotImplementedError 80 | plot_data(self.rewards[0:self.ptr], figure_n=2) 81 | 82 | def save_csv_data(self): 83 | data = [] 84 | ptr = self.ptr #exclude the last entry 85 | for i in range(ptr): 86 | data.append([i, self.rewards[i], self.lengths[i], self.progresses[i], self.laptimes[i]]) 87 | save_csv_array(data, self.path + "/training_data_episodes.csv") 88 | 89 | plot_data(self.rewards[0:ptr], figure_n=2) 90 | plt.figure(2) 91 | plt.savefig(self.path + "/training_rewards_episodes.png") 92 | 93 | t_steps = np.cumsum(self.lengths[0:ptr])/100 94 | plt.figure(3) 95 | plt.clf() 96 | 97 | plt.plot(t_steps, self.rewards[0:ptr], '.', color='darkblue', markersize=4) 98 | plt.plot(t_steps, true_moving_average(self.rewards[0:ptr], 20), linewidth='4', color='r') 99 | 100 | plt.xlabel("Training Steps (x100)") 101 | plt.ylabel("Reward per Episode") 102 | 103 | plt.tight_layout() 104 | plt.grid() 105 | plt.savefig(self.path + "/training_rewards_steps.png") 106 | 107 | # plt.figure(4) 108 | # plt.clf() 109 | # plt.plot(t_steps, self.progresses[0:self.ptr], '.', color='darkblue', markersize=4) 110 | # plt.plot(t_steps, true_moving_average(self.progresses[0:self.ptr], 20), linewidth='4', color='r') 111 | 112 | # plt.xlabel("Training Steps (x100)") 113 | # plt.ylabel("Progress") 114 | 115 | # plt.tight_layout() 116 | # plt.grid() 117 | # plt.savefig(self.path + "/training_progress_steps.png") 118 | 119 | # plt.close() 120 | 121 | 122 | 123 | class VehicleStateHistory: 124 | def __init__(self, run, folder): 125 | self.vehicle_name = run.run_name 126 | self.path = "Data/Vehicles/" + run.path + run.run_name + "/" + folder 127 | if not os.path.exists(self.path): 128 | os.mkdir(self.path) 129 | self.states = [] 130 | self.actions = [] 131 | 132 | 133 | def add_state(self, state): 134 | self.states.append(state) 135 | 136 | def add_action(self, action): 137 | self.actions.append(action) 138 | 139 | def save_history(self, lap_n=0, test_map=None): 140 | states = np.array(self.states) 141 | self.actions.append(np.array([0, 0])) # last action to equal lengths 142 | actions = np.array(self.actions) 143 | 144 | lap_history = np.concatenate((states, actions), axis=1) 145 | 146 | if test_map is None: 147 | np.save(self.path + f"Lap_{lap_n}_history_{self.vehicle_name}.npy", lap_history) 148 | else: 149 | np.save(self.path + f"Lap_{lap_n}_history_{self.vehicle_name}_{test_map}.npy", lap_history) 150 | 151 | self.states = [] 152 | self.actions = [] 153 | 154 | def save_history(self, lap_n=0, test_map=None): 155 | states = np.array(self.states) 156 | self.actions.append(np.array([0, 0])) # last action to equal lengths 157 | actions = np.array(self.actions) 158 | 159 | lap_history = np.concatenate((states, actions), axis=1) 160 | 161 | if test_map is None: 162 | np.save(self.path + f"Lap_{lap_n}_history_{self.vehicle_name}.npy", lap_history) 163 | else: 164 | np.save(self.path + f"Lap_{lap_n}_history_{self.vehicle_name}_{test_map}.npy", lap_history) 165 | 166 | self.states = [] 167 | self.actions = [] 168 | 169 | 170 | 171 | class SafetyHistory: 172 | def __init__(self, run): 173 | self.vehicle_name = run.run_name 174 | self.path = "Data/Vehicles/" + run.path + self.vehicle_name + "/SafeHistory/" 175 | os.mkdir(self.path) 176 | 177 | self.planned_actions = [] 178 | self.safe_actions = [] 179 | self.interventions = [] 180 | self.lap_n = 0 181 | 182 | self.interval_counter = 0 183 | self.inter_intervals = [] 184 | self.ep_interventions = 0 185 | self.intervention_list = [] 186 | 187 | def add_actions(self, planned_action, safe_action=None): 188 | self.planned_actions.append(planned_action) 189 | if safe_action is None: 190 | self.safe_actions.append(planned_action) 191 | self.interventions.append(False) 192 | else: 193 | self.safe_actions.append(safe_action) 194 | self.interventions.append(True) 195 | 196 | def add_planned_action(self, planned_action): 197 | self.planned_actions.append(planned_action) 198 | self.safe_actions.append(planned_action) 199 | self.interventions.append(False) 200 | self.interval_counter += 1 201 | 202 | def add_intervention(self, planned_action, safe_action): 203 | self.planned_actions.append(planned_action) 204 | self.safe_actions.append(safe_action) 205 | self.interventions.append(True) 206 | self.inter_intervals.append(self.interval_counter) 207 | self.interval_counter = 0 208 | self.ep_interventions += 1 209 | 210 | def train_lap_complete(self): 211 | self.intervention_list.append(self.ep_interventions) 212 | 213 | print(f"Interventions: {self.ep_interventions} --> {self.inter_intervals}") 214 | 215 | self.ep_interventions = 0 216 | self.inter_intervals = [] 217 | 218 | def plot_safe_history(self): 219 | planned = np.array(self.planned_actions) 220 | safe = np.array(self.safe_actions) 221 | plt.figure(5) 222 | plt.clf() 223 | plt.title("Safe History: steering") 224 | plt.plot(planned[:, 0], color='blue') 225 | plt.plot(safe[:, 0], '-x', color='red') 226 | plt.legend(['Planned Actions', 'Safe Actions']) 227 | plt.ylim([-0.5, 0.5]) 228 | # plt.show() 229 | plt.pause(0.0001) 230 | 231 | plt.figure(6) 232 | plt.clf() 233 | plt.title("Safe History: velocity") 234 | plt.plot(planned[:, 1], color='blue') 235 | plt.plot(safe[:, 1], '-x', color='red') 236 | plt.legend(['Planned Actions', 'Safe Actions']) 237 | plt.ylim([-0.5, 0.5]) 238 | # plt.show() 239 | plt.pause(0.0001) 240 | 241 | self.planned_actions = [] 242 | self.safe_actions = [] 243 | 244 | def save_safe_history(self, training=False): 245 | planned_actions = np.array(self.planned_actions) 246 | safe_actions = np.array(self.safe_actions) 247 | interventions = np.array(self.interventions) 248 | data = np.concatenate((planned_actions, safe_actions, interventions[:, None]), axis=1) 249 | 250 | if training: 251 | np.save(self.path + f"Training_safeHistory_{self.vehicle_name}.npy", data) 252 | else: 253 | np.save(self.path + f"Lap_{self.lap_n}_safeHistory_{self.vehicle_name}.npy", data) 254 | 255 | self.lap_n += 1 256 | 257 | self.planned_actions = [] 258 | self.safe_actions = [] 259 | self.interventions = [] 260 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/RacingTrack.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.RewardSignals.RewardUtils import * 2 | from matplotlib import pyplot as plt 3 | import numpy as np 4 | 5 | 6 | class RacingTrack: 7 | def __init__(self, map_name) -> None: 8 | self.track_pts = None 9 | self.wpts = None 10 | self.ss = None 11 | self.map_name = map_name 12 | self.total_s = None 13 | self.vs = None 14 | 15 | self.max_distance = 0 16 | self.distance_allowance = 1 17 | 18 | self.load_raceline() 19 | 20 | def load_raceline(self): 21 | track = [] 22 | filename = 'maps/' + self.map_name + "_raceline.csv" 23 | with open(filename, 'r') as csvfile: 24 | csvFile = csv.reader(csvfile, quoting=csv.QUOTE_NONNUMERIC) 25 | 26 | for lines in csvFile: 27 | track.append(lines) 28 | 29 | track = np.array(track) 30 | print(f"Track Loaded: {filename}") 31 | 32 | self.wpts = track[:, 1:3] 33 | self.vs = track[:, 5] 34 | 35 | seg_lengths = np.linalg.norm(np.diff(self.wpts, axis=0), axis=1) 36 | self.ss = np.insert(np.cumsum(seg_lengths), 0, 0) 37 | self.total_s = self.ss[-1] 38 | 39 | def plot_wpts(self): 40 | plt.figure(1) 41 | plt.plot(self.wpts[:, 0], self.wpts[:, 1], 'b-') 42 | for i, pt in enumerate(self.wpts): 43 | # plt.plot(pt[0], pt[1], ) 44 | plt.text(pt[0], pt[1], f"{i}") 45 | plt.gca().set_aspect('equal', adjustable='box') 46 | plt.show() 47 | 48 | def calculate_progress(self, point): 49 | idx, dists = self.get_trackline_segment(point) 50 | 51 | x, h = self.interp_pts(idx, dists) 52 | 53 | s = self.ss[idx] + x 54 | 55 | return s 56 | 57 | def calculate_progress_percent(self, point): 58 | s = self.calculate_progress(point) 59 | return s/self.total_s 60 | 61 | def interp_pts(self, idx, dists): 62 | """ 63 | 64 | """ 65 | # finds the reflected distance along the line joining wpt1 and wpt2 66 | # uses Herons formula for the area of a triangle 67 | d_ss = self.ss[idx+1] - self.ss[idx] 68 | d1, d2 = dists[idx], dists[idx+1] 69 | 70 | if d1 < 0.01: # at the first point 71 | x = 0 72 | h = 0 73 | elif d2 < 0.01: # at the second point 74 | x = dists[idx] # the distance to the previous point 75 | h = 0 # there is no distance 76 | else: 77 | # if the point is somewhere along the line 78 | s = (d_ss + d1 + d2)/2 79 | Area_square = (s*(s-d1)*(s-d2)*(s-d_ss)) 80 | Area = Area_square**0.5 81 | h = Area * 2/d_ss 82 | if np.isnan(h): 83 | h = 0 84 | x = (d1**2 - h**2)**0.5 85 | 86 | return x, h 87 | 88 | def get_trackline_segment(self, point): 89 | """Returns the first index representing the line segment that is closest to the point. 90 | 91 | wpt1 = pts[idx] 92 | wpt2 = pts[idx+1] 93 | 94 | dists: the distance from the point to each of the wpts. 95 | """ 96 | dists = np.linalg.norm(point - self.wpts, axis=1) 97 | 98 | min_dist_segment = np.argmin(dists) 99 | if min_dist_segment == 0: 100 | return 0, dists 101 | elif min_dist_segment == len(dists)-1: 102 | return len(dists)-2, dists 103 | 104 | if dists[min_dist_segment+1] < dists[min_dist_segment-1]: 105 | return min_dist_segment, dists 106 | else: 107 | return min_dist_segment - 1, dists 108 | 109 | def get_cross_track_heading(self, point): 110 | idx, dists = self.get_trackline_segment(point) 111 | point_diff = self.wpts[idx+1, :] - self.wpts[idx, :] 112 | trackline_heading = np.arctan2(point_diff[1], point_diff[0]) 113 | 114 | x, h = self.interp_pts(idx, dists) 115 | 116 | return trackline_heading, h 117 | 118 | def get_velocity(self, point): 119 | idx, dists = self.get_trackline_segment(point) 120 | v = self.vs[idx] 121 | 122 | return v 123 | 124 | def plot_vehicle(self, point, theta): 125 | idx, dists = self.get_trackline_segment(point) 126 | point_diff = self.wpts[idx+1, :] - self.wpts[idx, :] 127 | trackline_heading = np.arctan2(point_diff[1], point_diff[0]) 128 | 129 | x, h = self.interp_pts(idx, dists) 130 | 131 | track_pt = self.wpts[idx] + x * np.array([np.cos(trackline_heading), np.sin(trackline_heading)]) 132 | 133 | plt.figure(1) 134 | plt.clf() 135 | size = 1.2 136 | plt.xlim([point[0]-size, point[0]+size]) 137 | plt.ylim([point[1]-size, point[1]+size]) 138 | plt.plot(self.wpts[:,0], self.wpts[:,1], 'b-x', linewidth=2) 139 | plt.plot(self.wpts[idx:idx+2, 0], self.wpts[idx:idx+2, 1], 'r-', linewidth=2) 140 | plt.plot([point[0], track_pt[0]], [point[1], track_pt[1]], 'orange', linewidth=2) 141 | plt.plot(track_pt[0], track_pt[1],'o', color='orange', markersize=6) 142 | 143 | plt.plot(point[0], point[1], 'go', markersize=6) 144 | plt.arrow(point[0], point[1], np.cos(theta), np.sin(theta), color='g', head_width=0.1, head_length=0.1, linewidth=2) 145 | 146 | plt.pause(0.0001) 147 | 148 | 149 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/RewardSignals.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.Utils.RewardUtils import * 2 | 3 | from TrajectoryAidedLearning.Utils.utils import * 4 | from TrajectoryAidedLearning.Utils.StdTrack import StdTrack 5 | 6 | from TrajectoryAidedLearning.Planners.PurePursuit import PurePursuit 7 | import numpy as np 8 | 9 | # rewards functions 10 | class ProgressReward: 11 | def __init__(self, track: StdTrack) -> None: 12 | self.track = track 13 | 14 | def __call__(self, observation, prev_obs, pre_action): 15 | if prev_obs is None: return 0 16 | 17 | if observation['lap_done']: 18 | return 1 # complete 19 | if observation['colision_done']: 20 | return -1 # crash 21 | 22 | 23 | position = observation['state'][0:2] 24 | prev_position = prev_obs['state'][0:2] 25 | theta = observation['state'][2] 26 | 27 | s = self.track.calculate_progress(prev_position) 28 | ss = self.track.calculate_progress(position) 29 | reward = (ss - s) / self.track.total_s 30 | if abs(reward) > 0.5: # happens at end of eps 31 | return 0.001 # assume positive progress near end 32 | 33 | # self.race_track.plot_vehicle(position, theta) 34 | 35 | 36 | reward *= 10 # remove all reward 37 | return reward 38 | 39 | 40 | class CrossTrackHeadReward: 41 | def __init__(self, track: StdTrack, conf): 42 | self.track = track 43 | self.r_veloctiy = 1 44 | self.r_distance = 1 45 | self.max_v = conf.max_v # used for scaling. 46 | 47 | def __call__(self, observation, prev_obs, pre_action): 48 | if observation['lap_done']: 49 | return 1 # complete 50 | if observation['colision_done']: 51 | return -1 # crash 52 | 53 | position = observation['state'][0:2] 54 | theta = observation['state'][2] 55 | heading, distance = self.track.get_cross_track_heading(position) 56 | # self.race_track.plot_vehicle(position, theta) 57 | 58 | d_heading = abs(robust_angle_difference_rad(heading, theta)) 59 | r_heading = np.cos(d_heading) * self.r_veloctiy # velocity 60 | r_heading *= (observation['state'][3] / self.max_v) 61 | 62 | r_distance = distance * self.r_distance 63 | 64 | reward = r_heading - r_distance 65 | reward = max(reward, 0) 66 | # reward *= 0.1 67 | return reward 68 | 69 | 70 | 71 | class TALearningReward: 72 | def __init__(self, conf, run): 73 | run.pp_speed_mode = "raceline" 74 | run.raceline = True 75 | self.pp = PurePursuit(conf, run, False) 76 | 77 | self.beta_c = 0.4 78 | self.beta_steer_weight = 0.4 79 | self.beta_velocity_weight = 0.4 80 | 81 | self.max_steer_diff = 0.8 82 | self.max_velocity_diff = 2.0 83 | 84 | # self.beta_c = 1 85 | # self.beta_steer_weight = 1 86 | # self.beta_velocity_weight = 1 87 | 88 | # self.max_steer_diff = 0.4 89 | # self.max_velocity_diff = 1.0 90 | # self.max_velocity_diff = 4.0 91 | 92 | def __call__(self, observation, prev_obs, action): 93 | if prev_obs is None: return 0 94 | 95 | if observation['lap_done']: 96 | return 1 # complete 97 | if observation['colision_done']: 98 | return -1 # crash 99 | 100 | pp_act = self.pp.plan(prev_obs) 101 | 102 | steer_reward = (abs(pp_act[0] - action[0]) / self.max_steer_diff) * self.beta_steer_weight 103 | 104 | throttle_reward = (abs(pp_act[1] - action[1]) / self.max_velocity_diff) * self.beta_velocity_weight 105 | 106 | reward = self.beta_c - steer_reward - throttle_reward 107 | reward = max(reward, 0) # limit at 0 108 | 109 | reward *= 0.5 110 | 111 | return reward 112 | 113 | 114 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/RewardUtils.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import csv 4 | import math, cmath 5 | 6 | 7 | def get_distance(x1=[0, 0], x2=[0, 0]): 8 | d = [0.0, 0.0] 9 | for i in range(2): 10 | d[i] = x1[i] - x2[i] 11 | return np.linalg.norm(d) 12 | 13 | def find_closest_pt(pt, wpts): 14 | """ 15 | Returns the two closes points in order along wpts 16 | """ 17 | dists = [get_distance(pt, wpt) for wpt in wpts] 18 | min_i = np.argmin(dists) 19 | d_i = dists[min_i] 20 | if min_i == len(dists) - 1: 21 | min_i -= 1 22 | if dists[max(min_i -1, 0) ] > dists[min_i+1]: 23 | p_i = wpts[min_i] 24 | p_ii = wpts[min_i+1] 25 | d_i = dists[min_i] 26 | d_ii = dists[min_i+1] 27 | else: 28 | p_i = wpts[min_i-1] 29 | p_ii = wpts[min_i] 30 | d_i = dists[min_i-1] 31 | d_ii = dists[min_i] 32 | 33 | return p_i, p_ii, d_i, d_ii 34 | 35 | def get_tiangle_h(a, b, c): 36 | s = (a + b+ c) / 2 37 | A = np.sqrt(s*(s-a)*(s-b)*(s-c)) 38 | h = 2 * A / c 39 | 40 | return h 41 | 42 | def distance_potential(s, s_p, end, beta=0.2, scale=0.5): 43 | prev_dist = get_distance(s[0:2], end) 44 | cur_dist = get_distance(s_p[0:2], end) 45 | d_dis = (prev_dist - cur_dist) / scale 46 | 47 | return d_dis * beta 48 | 49 | def find_reward(s_p): 50 | if s_p['collisions'][0] == 1: 51 | return -1 52 | elif s_p['lap_counts'][0] == 1: 53 | return 1 54 | return 0 55 | 56 | 57 | def get_gradient(x1=[0, 0], x2=[0, 0]): 58 | t = (x1[1] - x2[1]) 59 | b = (x1[0] - x2[0]) 60 | if b != 0: 61 | return t / b 62 | return 1000000 # near infinite gradient. 63 | 64 | 65 | def get_bearing(x1=[0, 0], x2=[0, 0]): 66 | grad = get_gradient(x1, x2) 67 | dx = x2[0] - x1[0] 68 | th_start_end = np.arctan(grad) 69 | if dx == 0: 70 | if x2[1] - x1[1] > 0: 71 | th_start_end = 0 72 | else: 73 | th_start_end = np.pi 74 | elif th_start_end > 0: 75 | if dx > 0: 76 | th_start_end = np.pi / 2 - th_start_end 77 | else: 78 | th_start_end = -np.pi/2 - th_start_end 79 | else: 80 | if dx > 0: 81 | th_start_end = np.pi / 2 - th_start_end 82 | else: 83 | th_start_end = - np.pi/2 - th_start_end 84 | 85 | return th_start_end 86 | 87 | def sub_angles_complex(a1, a2): 88 | real = math.cos(a1) * math.cos(a2) + math.sin(a1) * math.sin(a2) 89 | im = - math.cos(a1) * math.sin(a2) + math.sin(a1) * math.cos(a2) 90 | 91 | cpx = complex(real, im) 92 | phase = cmath.phase(cpx) 93 | 94 | return phase 95 | 96 | 97 | 98 | #TODO: njit these function 99 | def robust_angle_difference_degree(x, y): 100 | """Returns the difference between two angles in DEGREES 101 | r = x - y""" 102 | x = np.deg2rad(x) 103 | y = np.deg2rad(y) 104 | r = np.arctan2(np.sin(x-y), np.cos(x-y)) 105 | return np.rad2deg(r) 106 | 107 | def robust_angle_difference_rad(x, y): 108 | """Returns the difference between two angles in RADIANS 109 | r = x - y""" 110 | return np.arctan2(np.sin(x-y), np.cos(x-y)) 111 | 112 | 113 | if __name__ == '__main__': 114 | test_angle_diff() 115 | pass 116 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/StdTrack.py: -------------------------------------------------------------------------------- 1 | from TrajectoryAidedLearning.Utils.RewardUtils import * 2 | from matplotlib import pyplot as plt 3 | 4 | # Track base 5 | class StdTrack: 6 | def __init__(self, map_name) -> None: 7 | self.wpts = None 8 | self.ss = None 9 | self.map_name = map_name 10 | self.total_s = None 11 | 12 | self.max_distance = 0 13 | self.distance_allowance = 1 14 | 15 | self.load_centerline() 16 | 17 | def load_centerline(self): 18 | # filename = 'map_data/' + self.map_name + '_std.csv' 19 | filename = 'maps/' + self.map_name + '_centerline.csv' 20 | xs, ys, w_rs, w_ls = [0], [0], [], [] 21 | with open(filename, 'r') as file: 22 | csvFile = csv.reader(file) 23 | 24 | for i, lines in enumerate(csvFile): 25 | if i ==0: 26 | continue 27 | xs.append(float(lines[0])) 28 | ys.append(float(lines[1])) 29 | w_rs.append(float(lines[2])) 30 | w_ls.append(float(lines[3])) 31 | xs[-1] = 0 32 | ys[-1] = 0 33 | self.xs = np.array(xs)[:, None] 34 | self.ys = np.array(ys)[:, None] 35 | self.centre_length = len(xs) 36 | 37 | self.wpts = np.vstack((xs, ys)).T 38 | 39 | diffs = np.diff(self.wpts, axis=0) 40 | seg_lengths = np.linalg.norm(diffs, axis=1) 41 | self.ss = np.cumsum(seg_lengths) 42 | self.ss = np.insert(self.ss, 0, 0) 43 | 44 | self.total_s = self.ss[-1] 45 | 46 | def plot_wpts(self): 47 | plt.figure(1) 48 | plt.plot(self.wpts[:, 0], self.wpts[:, 1], 'b-') 49 | for i, pt in enumerate(self.wpts): 50 | # plt.plot(pt[0], pt[1], ) 51 | plt.text(pt[0], pt[1], f"{i}") 52 | plt.gca().set_aspect('equal', adjustable='box') 53 | plt.show() 54 | 55 | def calculate_progress(self, point): 56 | idx, dists = self.get_trackline_segment(point) 57 | 58 | x, h = self.interp_pts(idx, dists) 59 | 60 | s = self.ss[idx] + x 61 | 62 | return s 63 | 64 | def calculate_progress_percent(self, point): 65 | s = self.calculate_progress(point) 66 | return s/self.total_s 67 | 68 | def interp_pts(self, idx, dists): 69 | """ 70 | 71 | """ 72 | # finds the reflected distance along the line joining wpt1 and wpt2 73 | # uses Herons formula for the area of a triangle 74 | d_ss = self.ss[idx+1] - self.ss[idx] 75 | d1, d2 = dists[idx], dists[idx+1] 76 | 77 | if d1 < 0.01: # at the first point 78 | x = 0 79 | h = 0 80 | elif d2 < 0.01: # at the second point 81 | x = dists[idx] # the distance to the previous point 82 | h = 0 # there is no distance 83 | else: 84 | # if the point is somewhere along the line 85 | s = (d_ss + d1 + d2)/2 86 | Area_square = (s*(s-d1)*(s-d2)*(s-d_ss)) 87 | Area = Area_square**0.5 88 | h = Area * 2/d_ss 89 | if np.isnan(h): 90 | h = 0 91 | x = (d1**2 - h**2)**0.5 92 | 93 | return x, h 94 | 95 | def get_trackline_segment(self, point): 96 | """Returns the first index representing the line segment that is closest to the point. 97 | 98 | wpt1 = pts[idx] 99 | wpt2 = pts[idx+1] 100 | 101 | dists: the distance from the point to each of the wpts. 102 | """ 103 | dists = np.linalg.norm(point - self.wpts, axis=1) 104 | 105 | min_dist_segment = np.argmin(dists) 106 | if min_dist_segment == 0: 107 | return 0, dists 108 | elif min_dist_segment == len(dists)-1: 109 | return len(dists)-2, dists 110 | 111 | if dists[min_dist_segment+1] < dists[min_dist_segment-1]: 112 | return min_dist_segment, dists 113 | else: 114 | return min_dist_segment - 1, dists 115 | 116 | def get_cross_track_heading(self, point): 117 | idx, dists = self.get_trackline_segment(point) 118 | point_diff = self.wpts[idx+1, :] - self.wpts[idx, :] 119 | trackline_heading = np.arctan2(point_diff[1], point_diff[0]) 120 | 121 | x, h = self.interp_pts(idx, dists) 122 | 123 | return trackline_heading, h 124 | 125 | def plot_vehicle(self, point, theta): 126 | idx, dists = self.get_trackline_segment(point) 127 | point_diff = self.wpts[idx+1, :] - self.wpts[idx, :] 128 | trackline_heading = np.arctan2(point_diff[1], point_diff[0]) 129 | 130 | x, h = self.interp_pts(idx, dists) 131 | 132 | track_pt = self.wpts[idx] + x * np.array([np.cos(trackline_heading), np.sin(trackline_heading)]) 133 | 134 | plt.figure(1) 135 | plt.clf() 136 | size = 1.2 137 | plt.xlim([point[0]-size, point[0]+size]) 138 | plt.ylim([point[1]-size, point[1]+size]) 139 | plt.plot(self.wpts[:,0], self.wpts[:,1], 'b-x', linewidth=2) 140 | plt.plot(self.wpts[idx:idx+2, 0], self.wpts[idx:idx+2, 1], 'r-', linewidth=2) 141 | plt.plot([point[0], track_pt[0]], [point[1], track_pt[1]], 'orange', linewidth=2) 142 | plt.plot(track_pt[0], track_pt[1],'o', color='orange', markersize=6) 143 | 144 | plt.plot(point[0], point[1], 'go', markersize=6) 145 | plt.arrow(point[0], point[1], np.cos(theta), np.sin(theta), color='g', head_width=0.1, head_length=0.1, linewidth=2) 146 | 147 | plt.pause(0.0001) 148 | 149 | def check_done(self, observation): 150 | position = observation['state'][0:2] 151 | s = self.calculate_progress(position) 152 | 153 | if s <= (self.max_distance - self.distance_allowance) and self.max_distance < 0.8*self.total_s and s > 0.1: 154 | # check if I went backwards, unless the max distance is almost finished and that it isn't starting 155 | return True # made negative progress 156 | self.max_distance = max(self.max_distance, s) 157 | 158 | return False 159 | 160 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/TD3.py: -------------------------------------------------------------------------------- 1 | from os import stat 2 | import numpy as np 3 | from matplotlib import pyplot as plt 4 | import random 5 | 6 | import torch 7 | import torch.nn as nn 8 | import torch.nn.functional as F 9 | import torch.optim as optim 10 | 11 | MEMORY_SIZE = 100000 12 | 13 | 14 | # hyper parameters 15 | BATCH_SIZE = 100 16 | GAMMA = 0.99 17 | tau = 0.005 18 | NOISE = 0.2 19 | NOISE_CLIP = 0.5 20 | EXPLORE_NOISE = 0.1 21 | POLICY_FREQUENCY = 2 22 | POLICY_NOISE = 0.2 23 | 24 | 25 | class SmartBufferTD3(object): 26 | def __init__(self, max_size=1000000, state_dim=14, act_dim=1): 27 | self.max_size = max_size 28 | self.state_dim = state_dim 29 | self.act_dim = act_dim 30 | self.ptr = 0 31 | 32 | self.states = np.empty((max_size, state_dim)) 33 | self.actions = np.empty((max_size, act_dim)) 34 | self.next_states = np.empty((max_size, state_dim)) 35 | self.rewards = np.empty((max_size, 1)) 36 | self.dones = np.empty((max_size, 1)) 37 | 38 | def add(self, s, a, s_p, r, d): 39 | self.states[self.ptr] = s 40 | self.actions[self.ptr] = a 41 | self.next_states[self.ptr] = s_p 42 | self.rewards[self.ptr] = r 43 | self.dones[self.ptr] = d 44 | 45 | self.ptr += 1 46 | 47 | if self.ptr == self.max_size-1: self.ptr = 0 #! crisis 48 | # if self.ptr == 99999: self.ptr = 0 #! crisis 49 | 50 | def sample(self, batch_size): 51 | ind = np.random.randint(0, self.ptr-1, size=batch_size) 52 | states = np.empty((batch_size, self.state_dim)) 53 | actions = np.empty((batch_size, self.act_dim)) 54 | next_states = np.empty((batch_size, self.state_dim)) 55 | rewards = np.empty((batch_size, 1)) 56 | dones = np.empty((batch_size, 1)) 57 | 58 | for i, j in enumerate(ind): 59 | states[i] = self.states[j] 60 | actions[i] = self.actions[j] 61 | next_states[i] = self.next_states[j] 62 | rewards[i] = self.rewards[j] 63 | dones[i] = self.dones[j] 64 | 65 | return states, actions, next_states, rewards, dones 66 | 67 | def size(self): 68 | return self.ptr 69 | 70 | 71 | class Actor(nn.Module): 72 | def __init__(self, state_dim, action_dim, max_action, h_size): 73 | super(Actor, self).__init__() 74 | 75 | self.l1 = nn.Linear(state_dim, h_size) 76 | self.l2 = nn.Linear(h_size, h_size) 77 | self.l3 = nn.Linear(h_size, action_dim) 78 | 79 | self.max_action = max_action 80 | 81 | def forward(self, x): 82 | x = F.relu(self.l1(x)) 83 | x = F.relu(self.l2(x)) 84 | x = self.max_action * torch.tanh(self.l3(x)) 85 | return x 86 | 87 | class Critic(nn.Module): 88 | def __init__(self, state_dim, action_dim, h_size): 89 | super(Critic, self).__init__() 90 | 91 | # Q1 architecture 92 | self.l1 = nn.Linear(state_dim + action_dim, h_size) 93 | self.l2 = nn.Linear(h_size, h_size) 94 | self.l3 = nn.Linear(h_size, 1) 95 | 96 | # Q2 architecture 97 | self.l4 = nn.Linear(state_dim + action_dim, h_size) 98 | self.l5 = nn.Linear(h_size, h_size) 99 | self.l6 = nn.Linear(h_size, 1) 100 | 101 | def forward(self, x, u): 102 | xu = torch.cat([x, u], 1) 103 | 104 | x1 = F.relu(self.l1(xu)) 105 | x1 = F.relu(self.l2(x1)) 106 | x1 = self.l3(x1) 107 | 108 | x2 = F.relu(self.l4(xu)) 109 | x2 = F.relu(self.l5(x2)) 110 | x2 = self.l6(x2) 111 | return x1, x2 112 | 113 | def Q1(self, x, u): 114 | xu = torch.cat([x, u], 1) 115 | 116 | x1 = F.relu(self.l1(xu)) 117 | x1 = F.relu(self.l2(x1)) 118 | x1 = self.l3(x1) 119 | return x1 120 | 121 | 122 | 123 | class TD3(object): 124 | def __init__(self, state_dim, action_dim, max_action, name): 125 | self.name = name 126 | self.state_dim = state_dim 127 | self.max_action = max_action 128 | self.act_dim = action_dim 129 | 130 | self.actor = None 131 | self.actor_target = None 132 | self.actor_optimizer = None 133 | 134 | self.critic = None 135 | self.critic_target = None 136 | self.critic_optimizer = None 137 | 138 | self.replay_buffer = SmartBufferTD3(state_dim=state_dim, act_dim=action_dim) 139 | 140 | def create_agent(self, h_size): 141 | state_dim = self.state_dim 142 | action_dim = self.act_dim 143 | max_action = self.max_action 144 | self.actor = Actor(state_dim, action_dim, max_action, h_size) 145 | self.actor_target = Actor(state_dim, action_dim, max_action, h_size) 146 | self.actor_target.load_state_dict(self.actor.state_dict()) 147 | self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=1e-3) 148 | 149 | self.critic = Critic(state_dim, action_dim, h_size) 150 | self.critic_target = Critic(state_dim, action_dim, h_size) 151 | self.critic_target.load_state_dict(self.critic.state_dict()) 152 | self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=1e-3) 153 | 154 | def select_action(self, state, noise=0.1): 155 | return self.act(state, noise=noise) 156 | 157 | def act(self, state, noise=0.1): 158 | state = torch.FloatTensor(state.reshape(1, -1)) 159 | 160 | action = self.actor(state).data.numpy().flatten() 161 | if noise != 0: 162 | action = (action + np.random.normal(0, noise, size=self.act_dim)) 163 | 164 | return action.clip(-self.max_action, self.max_action) 165 | 166 | def get_critic_value(self, state, action): 167 | state = torch.FloatTensor(state) 168 | action = torch.FloatTensor(action) 169 | 170 | current_Q1, current_Q2 = self.critic(state[None, :], action[None, :]) 171 | ret = current_Q1.detach().item() 172 | 173 | return ret 174 | 175 | def train(self, iterations=2): 176 | if self.replay_buffer.size() < BATCH_SIZE * 5: 177 | return 0 178 | for it in range(iterations): 179 | # Sample replay buffer 180 | x, u, y, r, d = self.replay_buffer.sample(BATCH_SIZE) 181 | state = torch.FloatTensor(x) 182 | action = torch.FloatTensor(u) 183 | next_state = torch.FloatTensor(y) 184 | done = torch.FloatTensor(1 - d) 185 | reward = torch.FloatTensor(r) 186 | 187 | # Select action according to policy and add clipped noise 188 | noise = torch.FloatTensor(u).data.normal_(0, POLICY_NOISE) 189 | noise = noise.clamp(-NOISE_CLIP, NOISE_CLIP) 190 | next_action = (self.actor_target(next_state) + noise).clamp(-self.max_action, self.max_action) 191 | 192 | # Compute the target Q value 193 | target_Q1, target_Q2 = self.critic_target(next_state, next_action) 194 | target_Q = torch.min(target_Q1, target_Q2) 195 | target_Q = reward + (done * GAMMA * target_Q).detach() 196 | 197 | # Get current Q estimates 198 | current_Q1, current_Q2 = self.critic(state, action) 199 | 200 | # Compute critic loss 201 | critic_loss = F.mse_loss(current_Q1, target_Q) + F.mse_loss(current_Q2, target_Q) 202 | 203 | # Optimize the critic 204 | self.critic_optimizer.zero_grad() 205 | critic_loss.backward() 206 | self.critic_optimizer.step() 207 | 208 | # Delayed policy updates 209 | if it % POLICY_FREQUENCY == 0: 210 | actor_loss = -self.critic.Q1(state, self.actor(state)).mean() 211 | 212 | self.actor_optimizer.zero_grad() 213 | actor_loss.backward() 214 | self.actor_optimizer.step() 215 | 216 | # Update the frozen target models 217 | for param, target_param in zip(self.critic.parameters(), self.critic_target.parameters()): 218 | target_param.data.copy_(tau * param.data + (1 - tau) * target_param.data) 219 | 220 | for param, target_param in zip(self.actor.parameters(), self.actor_target.parameters()): 221 | target_param.data.copy_(tau * param.data + (1 - tau) * target_param.data) 222 | 223 | total_loss = actor_loss + critic_loss 224 | 225 | return total_loss 226 | 227 | def save(self, directory="./saves"): 228 | filename = self.name 229 | 230 | torch.save(self.actor, '%s/%s_actor.pth' % (directory, filename)) 231 | torch.save(self.critic, '%s/%s_critic.pth' % (directory, filename)) 232 | torch.save(self.actor_target, '%s/%s_actor_target.pth' % (directory, filename)) 233 | torch.save(self.critic_target, '%s/%s_critic_target.pth' % (directory, filename)) 234 | 235 | def load(self, directory="./saves"): 236 | filename = self.name 237 | self.actor = torch.load('%s/%s_actor.pth' % (directory, filename)) 238 | self.critic = torch.load('%s/%s_critic.pth' % (directory, filename)) 239 | self.actor_target = torch.load('%s/%s_actor_target.pth' % (directory, filename)) 240 | self.critic_target = torch.load('%s/%s_critic_target.pth' % (directory, filename)) 241 | 242 | print("Agent Loaded") 243 | 244 | def try_load(self, load=True, h_size=300, path=None): 245 | if load: 246 | try: 247 | self.load(path) 248 | except Exception as e: 249 | print(f"Exception: {e}") 250 | print(f"Unable to load model") 251 | pass 252 | else: 253 | print(f"Not loading - restarting training") 254 | self.create_agent(h_size) 255 | 256 | self.actor_optimizer = torch.optim.Adam(self.actor.parameters(), lr=1e-3) 257 | self.critic_optimizer = torch.optim.Adam(self.critic.parameters(), lr=1e-3) 258 | -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/TrajectoryAidedLearning/Utils/__init__.py -------------------------------------------------------------------------------- /TrajectoryAidedLearning/Utils/utils.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import csv 3 | import os 4 | from argparse import Namespace 5 | import shutil 6 | import numpy as np 7 | from numba import njit 8 | from matplotlib import pyplot as plt 9 | 10 | def save_conf_dict(dictionary, save_name=None): 11 | if save_name is None: 12 | save_name = dictionary["run_name"] 13 | path = "Data/Vehicles/" + dictionary["path"] + dictionary["run_name"] + f"/{save_name}_record.yaml" 14 | with open(path, 'w') as file: 15 | yaml.dump(dictionary, file) 16 | 17 | def load_conf(fname): 18 | full_path = "config/" + fname + '.yaml' 19 | with open(full_path) as file: 20 | conf_dict = yaml.load(file, Loader=yaml.FullLoader) 21 | 22 | conf = Namespace(**conf_dict) 23 | 24 | return conf 25 | 26 | def load_yaml_dict(fname): 27 | full_path = "config/" + fname + '.yaml' 28 | with open(full_path) as file: 29 | conf_dict = yaml.load(file, Loader=yaml.FullLoader) 30 | 31 | return conf_dict 32 | 33 | 34 | 35 | def init_file_struct(path): 36 | if os.path.exists(path): 37 | try: 38 | os.rmdir(path) 39 | except: 40 | shutil.rmtree(path) 41 | os.mkdir(path) 42 | 43 | @njit(cache=True) 44 | def limit_phi(phi): 45 | while phi > np.pi: 46 | phi = phi - 2*np.pi 47 | while phi < -np.pi: 48 | phi = phi + 2*np.pi 49 | return phi 50 | 51 | def init_file_struct(path): 52 | if os.path.exists(path): 53 | try: 54 | os.rmdir(path) 55 | except: 56 | shutil.rmtree(path) 57 | os.mkdir(path) 58 | 59 | def init_reward_struct(path): 60 | if os.path.exists(path): 61 | return 62 | os.mkdir(path) 63 | 64 | plotting_pallete2 = ["#922B21", "#1F618D", "#117A65", "#B7950B", "#6C3483", "#A04000", "#117A65"] 65 | pp = ["#CB4335", "#2874A6", "#229954", "#D4AC0D", "#884EA0", "#BA4A00", "#17A589"] 66 | path_orange = "#E67E22" 67 | 68 | pp_light = ["#EC7063", "#5499C7", "#58D68D", "#F4D03F", "#AF7AC5"] 69 | pp_dark = ["#943126", "#1A5276", "#1D8348", "#9A7D0A", "#633974"] 70 | pp_darkest = ["#78281F", "#154360", "#186A3B", "#7D6608", "#512E5F"] 71 | 72 | 73 | def plot_pallet(): 74 | plt.figure(1) 75 | for i in range(len(pp)): 76 | plt.plot([i,i], [0,1], color=pp[i], linewidth=10) 77 | plt.figure(2) 78 | for i in range(len(plotting_pallete2)): 79 | plt.plot([i,i], [0,1], color=plotting_pallete2[i], linewidth=10) 80 | plt.show() 81 | 82 | def true_moving_average(data, period): 83 | if len(data) < period: 84 | return np.zeros_like(data) 85 | ret = np.convolve(data, np.ones(period), 'same') / period 86 | # t_end = np.convolve(data, np.ones(period), 'valid') / (period) 87 | # t_end = t_end[-1] # last valid value 88 | for i in range(period): # start 89 | t = np.convolve(data, np.ones(i+2), 'valid') / (i+2) 90 | ret[i] = t[0] 91 | for i in range(period): 92 | length = int(round((i + period)/2)) 93 | t = np.convolve(data, np.ones(length), 'valid') / length 94 | ret[-i-1] = t[-1] 95 | return ret 96 | 97 | 98 | def setup_run_list(run_file): 99 | full_path = "config/" + run_file + '.yaml' 100 | with open(full_path) as file: 101 | run_dict = yaml.load(file, Loader=yaml.FullLoader) 102 | 103 | 104 | run_list = [] 105 | try: 106 | start_n = run_dict['start_n'] 107 | except KeyError: 108 | start_n = 0 109 | for rep in range(start_n, run_dict['n']): 110 | for run in run_dict['runs']: 111 | # base is to copy everything from the original 112 | for key in run_dict.keys(): 113 | if key not in run.keys() and key != "runs": 114 | run[key] = run_dict[key] 115 | 116 | # only have to add what isn't already there 117 | set_n = run['set_n'] 118 | max_speed = run['max_speed'] 119 | run["n"] = rep 120 | if run['architecture'] != "PP": 121 | run['run_name'] = f"{run['architecture']}_{run['train_mode']}_{run['test_mode']}_{run['reward']}_{run['map_name']}_{max_speed}_{set_n}_{rep}" 122 | else: 123 | run['run_name'] = f"{run['architecture']}_PP_{run['test_mode']}_PP_{run['map_name']}_{max_speed}_{set_n}_{rep}" 124 | run['path'] = f"{run['test_name']}/" 125 | 126 | run_list.append(Namespace(**run)) 127 | 128 | 129 | init_reward_struct("Data/Vehicles/" + run_list[0].path) 130 | 131 | return run_list 132 | 133 | @njit(cache=True) 134 | def calculate_speed(delta, f_s=0.8, max_v=7): 135 | b = 0.523 136 | g = 9.81 137 | l_d = 0.329 138 | 139 | if abs(delta) < 0.03: 140 | return max_v 141 | if abs(delta) > 0.4: 142 | return 0 143 | 144 | V = f_s * np.sqrt(b*g*l_d/np.tan(abs(delta))) 145 | 146 | V = min(V, max_v) 147 | 148 | return V 149 | 150 | def calculate_steering(v): 151 | b = 0.523 152 | g = 9.81 153 | L = 0.329 154 | 155 | d = np.arctan(L*b*g/(v**2)) 156 | 157 | d = np.clip(d, 0, 0.4) 158 | 159 | # note always positive d return 160 | 161 | return d 162 | 163 | 164 | def save_csv_array(data, filename): 165 | with open(filename, 'w') as file: 166 | writer = csv.writer(file) 167 | writer.writerows(data) 168 | 169 | def moving_average(data, period): 170 | return np.convolve(data, np.ones(period), 'same') / period 171 | 172 | if __name__ == '__main__': 173 | 174 | plot_pallet() -------------------------------------------------------------------------------- /TrajectoryAidedLearning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/TrajectoryAidedLearning/__init__.py -------------------------------------------------------------------------------- /TrajectoryAidedLearning/f110_gym/__init__.py: -------------------------------------------------------------------------------- 1 | from gym.envs.registration import register 2 | register( 3 | id='f110-v1', 4 | entry_point='f110_gym.envs:F110Env', 5 | ) -------------------------------------------------------------------------------- /TrajectoryAidedLearning/f110_gym/collision_models.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng 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 | 23 | 24 | 25 | """ 26 | Prototype of Utility functions and GJK algorithm for Collision checks between vehicles 27 | Originally from https://github.com/kroitor/gjk.c 28 | Author: Hongrui Zheng 29 | """ 30 | 31 | import numpy as np 32 | from numba import njit 33 | 34 | @njit(cache=True) 35 | def perpendicular(pt): 36 | """ 37 | Return a 2-vector's perpendicular vector 38 | 39 | Args: 40 | pt (np.ndarray, (2,)): input vector 41 | 42 | Returns: 43 | pt (np.ndarray, (2,)): perpendicular vector 44 | """ 45 | temp = pt[0] 46 | pt[0] = pt[1] 47 | pt[1] = -1*temp 48 | return pt 49 | 50 | 51 | @njit(cache=True) 52 | def tripleProduct(a, b, c): 53 | """ 54 | Return triple product of three vectors 55 | 56 | Args: 57 | a, b, c (np.ndarray, (2,)): input vectors 58 | 59 | Returns: 60 | (np.ndarray, (2,)): triple product 61 | """ 62 | ac = a.dot(c) 63 | bc = b.dot(c) 64 | return b*ac - a*bc 65 | 66 | 67 | @njit(cache=True) 68 | def avgPoint(vertices): 69 | """ 70 | Return the average point of multiple vertices 71 | 72 | Args: 73 | vertices (np.ndarray, (n, 2)): the vertices we want to find avg on 74 | 75 | Returns: 76 | avg (np.ndarray, (2,)): average point of the vertices 77 | """ 78 | return np.sum(vertices, axis=0)/vertices.shape[0] 79 | 80 | 81 | @njit(cache=True) 82 | def indexOfFurthestPoint(vertices, d): 83 | """ 84 | Return the index of the vertex furthest away along a direction in the list of vertices 85 | 86 | Args: 87 | vertices (np.ndarray, (n, 2)): the vertices we want to find avg on 88 | 89 | Returns: 90 | idx (int): index of the furthest point 91 | """ 92 | return np.argmax(vertices.dot(d)) 93 | 94 | 95 | @njit(cache=True) 96 | def support(vertices1, vertices2, d): 97 | """ 98 | Minkowski sum support function for GJK 99 | 100 | Args: 101 | vertices1 (np.ndarray, (n, 2)): vertices of the first body 102 | vertices2 (np.ndarray, (n, 2)): vertices of the second body 103 | d (np.ndarray, (2, )): direction to find the support along 104 | 105 | Returns: 106 | support (np.ndarray, (n, 2)): Minkowski sum 107 | """ 108 | i = indexOfFurthestPoint(vertices1, d) 109 | j = indexOfFurthestPoint(vertices2, -d) 110 | return vertices1[i] - vertices2[j] 111 | 112 | 113 | @njit(cache=True) 114 | def collision(vertices1, vertices2): 115 | """ 116 | GJK test to see whether two bodies overlap 117 | 118 | Args: 119 | vertices1 (np.ndarray, (n, 2)): vertices of the first body 120 | vertices2 (np.ndarray, (n, 2)): vertices of the second body 121 | 122 | Returns: 123 | overlap (boolean): True if two bodies collide 124 | """ 125 | index = 0 126 | simplex = np.empty((3, 2)) 127 | 128 | position1 = avgPoint(vertices1) 129 | position2 = avgPoint(vertices2) 130 | 131 | d = position1 - position2 132 | 133 | if d[0] == 0 and d[1] == 0: 134 | d[0] = 1.0 135 | 136 | a = support(vertices1, vertices2, d) 137 | simplex[index, :] = a 138 | 139 | if d.dot(a) <= 0: 140 | return False 141 | 142 | d = -a 143 | 144 | iter_count = 0 145 | while iter_count < 1e3: 146 | a = support(vertices1, vertices2, d) 147 | index += 1 148 | simplex[index, :] = a 149 | if d.dot(a) <= 0: 150 | return False 151 | 152 | ao = -a 153 | 154 | if index < 2: 155 | b = simplex[0, :] 156 | ab = b-a 157 | d = tripleProduct(ab, ao, ab) 158 | if np.linalg.norm(d) < 1e-10: 159 | d = perpendicular(ab) 160 | continue 161 | 162 | b = simplex[1, :] 163 | c = simplex[0, :] 164 | ab = b-a 165 | ac = c-a 166 | 167 | acperp = tripleProduct(ab, ac, ac) 168 | 169 | if acperp.dot(ao) >= 0: 170 | d = acperp 171 | else: 172 | abperp = tripleProduct(ac, ab, ab) 173 | if abperp.dot(ao) < 0: 174 | return True 175 | simplex[0, :] = simplex[1, :] 176 | d = abperp 177 | 178 | simplex[1, :] = simplex[2, :] 179 | index -= 1 180 | 181 | iter_count += 1 182 | return False 183 | 184 | @njit(cache=True) 185 | def collision_multiple(vertices): 186 | """ 187 | Check pair-wise collisions for all provided vertices 188 | 189 | Args: 190 | vertices (np.ndarray (num_bodies, 4, 2)): all vertices for checking pair-wise collision 191 | 192 | Returns: 193 | collisions (np.ndarray (num_vertices, )): whether each body is in collision 194 | collision_idx (np.ndarray (num_vertices, )): which index of other body is each index's body is in collision, -1 if not in collision 195 | """ 196 | collisions = np.zeros((vertices.shape[0], )) 197 | collision_idx = -1 * np.ones((vertices.shape[0], )) 198 | # looping over all pairs 199 | for i in range(vertices.shape[0]-1): 200 | for j in range(i+1, vertices.shape[0]): 201 | # check collision 202 | vi = np.ascontiguousarray(vertices[i, :, :]) 203 | vj = np.ascontiguousarray(vertices[j, :, :]) 204 | ij_collision = collision(vi, vj) 205 | # fill in results 206 | if ij_collision: 207 | collisions[i] = 1. 208 | collisions[j] = 1. 209 | collision_idx[i] = j 210 | collision_idx[j] = i 211 | 212 | return collisions, collision_idx 213 | 214 | """ 215 | Utility functions for getting vertices by pose and shape 216 | """ 217 | 218 | @njit(cache=True) 219 | def get_trmtx(pose): 220 | """ 221 | Get transformation matrix of vehicle frame -> global frame 222 | 223 | Args: 224 | pose (np.ndarray (3, )): current pose of the vehicle 225 | 226 | return: 227 | H (np.ndarray (4, 4)): transformation matrix 228 | """ 229 | x = pose[0] 230 | y = pose[1] 231 | th = pose[2] 232 | cos = np.cos(th) 233 | sin = np.sin(th) 234 | H = np.array([[cos, -sin, 0., x], [sin, cos, 0., y], [0., 0., 1., 0.], [0., 0., 0., 1.]]) 235 | return H 236 | 237 | @njit(cache=True) 238 | def get_vertices(pose, length, width): 239 | """ 240 | Utility function to return vertices of the car body given pose and size 241 | 242 | Args: 243 | pose (np.ndarray, (3, )): current world coordinate pose of the vehicle 244 | length (float): car length 245 | width (float): car width 246 | 247 | Returns: 248 | vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body 249 | """ 250 | H = get_trmtx(pose) 251 | rl = H.dot(np.asarray([[-length/2],[width/2],[0.], [1.]])).flatten() 252 | rr = H.dot(np.asarray([[-length/2],[-width/2],[0.], [1.]])).flatten() 253 | fl = H.dot(np.asarray([[length/2],[width/2],[0.], [1.]])).flatten() 254 | fr = H.dot(np.asarray([[length/2],[-width/2],[0.], [1.]])).flatten() 255 | rl = rl/rl[3] 256 | rr = rr/rr[3] 257 | fl = fl/fl[3] 258 | fr = fr/fr[3] 259 | vertices = np.asarray([[rl[0], rl[1]], [rr[0], rr[1]], [fr[0], fr[1]], [fl[0], fl[1]]]) 260 | return vertices 261 | 262 | 263 | """ 264 | Unit tests for GJK collision checks 265 | Author: Hongrui Zheng 266 | """ 267 | 268 | import time 269 | import unittest 270 | 271 | class CollisionTests(unittest.TestCase): 272 | def setUp(self): 273 | # test params 274 | np.random.seed(1234) 275 | 276 | # Collision check body 277 | self.vertices1 = np.asarray([[4,11.],[5,5],[9,9],[10,10]]) 278 | 279 | # car size 280 | self.length = 0.32 281 | self.width = 0.22 282 | 283 | def test_get_vert(self): 284 | test_pose = np.array([2.3, 6.7, 0.8]) 285 | vertices = get_vertices(test_pose, self.length, self.width) 286 | rect = np.vstack((vertices, vertices[0,:])) 287 | import matplotlib.pyplot as plt 288 | plt.scatter(test_pose[0], test_pose[1], c='red') 289 | plt.plot(rect[:, 0], rect[:, 1]) 290 | plt.xlim([1, 4]) 291 | plt.ylim([5, 8]) 292 | plt.axes().set_aspect('equal') 293 | plt.show() 294 | self.assertTrue(vertices.shape == (4, 2)) 295 | 296 | def test_get_vert_fps(self): 297 | test_pose = np.array([2.3, 6.7, 0.8]) 298 | start = time.time() 299 | for _ in range(1000): 300 | vertices = get_vertices(test_pose, self.length, self.width) 301 | elapsed = time.time() - start 302 | fps = 1000/elapsed 303 | print('get vertices fps:', fps) 304 | self.assertTrue(fps>500) 305 | 306 | def test_random_collision(self): 307 | # perturb the body by a small amount and make sure it all collides with the original body 308 | for _ in range(1000): 309 | a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 310 | b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 311 | self.assertTrue(collision(a,b)) 312 | 313 | def test_multiple_collisions(self): 314 | a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 315 | b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 316 | c = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 317 | d = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 318 | e = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 319 | f = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 320 | g = self.vertices1 + 10. 321 | allv = np.stack((a,b,c,d,e,f,g)) 322 | collisions, collision_idx = collision_multiple(allv) 323 | self.assertTrue(np.all(collisions == np.array([1., 1., 1., 1., 1., 1., 0.]))) 324 | self.assertTrue(np.all(collision_idx == np.array([5., 5., 5., 5., 5., 4., -1.]))) 325 | 326 | def test_fps(self): 327 | # also perturb the body but mainly want to test GJK speed 328 | start = time.time() 329 | for _ in range(1000): 330 | a = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 331 | b = self.vertices1 + np.random.normal(size=(self.vertices1.shape))/100. 332 | collision(a, b) 333 | elapsed = time.time() - start 334 | fps = 1000/elapsed 335 | print('gjk fps:', fps) 336 | self.assertTrue(fps>500) 337 | 338 | if __name__ == '__main__': 339 | unittest.main() -------------------------------------------------------------------------------- /config/CthVsProgress.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "CthVsProgress" 3 | 4 | architecture: "fast" 5 | n_scans: 2 6 | train_mode: "Std" 7 | test_mode: "Std" 8 | 9 | n: 5 10 | set_n: 1 11 | 12 | random_seed: 10000 13 | 14 | n_train_steps: 100000 15 | # n_test_laps: 20 16 | n_test_laps: 50 17 | 18 | 19 | runs: 20 | - map_name: "f1_esp" 21 | reward: "Cth" 22 | max_speed: 5 23 | - map_name: "f1_mco" 24 | reward: "Cth" 25 | max_speed: 5 26 | 27 | - map_name: "f1_esp" 28 | reward: "Progress" 29 | max_speed: 5 30 | - map_name: "f1_mco" 31 | reward: "Progress" 32 | max_speed: 5 33 | 34 | # - map_name: "f1_esp" 35 | # reward: "Cth" 36 | # max_speed: 8 37 | # - map_name: "f1_mco" 38 | # reward: "Cth" 39 | # max_speed: 8 40 | 41 | # - map_name: "f1_esp" 42 | # reward: "Progress" 43 | # max_speed: 8 44 | # - map_name: "f1_mco" 45 | # reward: "Progress" 46 | # max_speed: 8 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /config/Cth_maps.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "Cth_maps2" 3 | 4 | architecture: "fast" 5 | 6 | n_scans: 2 7 | train_mode: "Std" 8 | test_mode: "Std" 9 | 10 | start_n: 0 11 | n: 5 12 | set_n: 1 13 | 14 | random_seed: 10000 15 | 16 | n_train_steps: 100000 17 | n_test_laps: 20 18 | # n_test_laps: 50 19 | 20 | reward: "Cth" 21 | max_speed: 6 22 | 23 | runs: 24 | # - map_name: "f1_esp" 25 | # - map_name: "f1_mco" 26 | # - map_name: "f1_aut" 27 | - map_name: "f1_gbr" 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /config/Cth_maps2.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "Cth_maps" 3 | 4 | architecture: "fast" 5 | 6 | n_scans: 2 7 | train_mode: "Std" 8 | test_mode: "Std" 9 | 10 | start_n: 0 11 | n: 5 12 | set_n: 1 13 | 14 | random_seed: 10000 15 | 16 | n_train_steps: 100000 17 | n_test_laps: 20 18 | # n_test_laps: 50 19 | 20 | reward: "Cth" 21 | max_speed: 6 22 | 23 | runs: 24 | # - map_name: "f1_esp" 25 | # - map_name: "f1_mco" 26 | # - map_name: "f1_aut" 27 | - map_name: "f1_gbr" 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /config/Cth_speedMaps.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "Cth_speedMaps" 3 | 4 | architecture: "fast" 5 | 6 | n_scans: 2 7 | train_mode: "Std" 8 | test_mode: "Std" 9 | 10 | n: 3 11 | set_n: 1 12 | 13 | random_seed: 10000 14 | 15 | n_train_steps: 100000 16 | n_test_laps: 20 17 | # n_test_laps: 50 18 | 19 | map_name: "f1_esp" 20 | reward: "Cth" 21 | 22 | runs: 23 | - max_speed: 4 24 | map_name: "f1_mco" 25 | - max_speed: 5 26 | map_name: "f1_mco" 27 | - max_speed: 6 28 | map_name: "f1_mco" 29 | - max_speed: 7 30 | map_name: "f1_mco" 31 | - max_speed: 8 32 | map_name: "f1_mco" 33 | 34 | - max_speed: 4 35 | map_name: "f1_gbr" 36 | - max_speed: 5 37 | map_name: "f1_gbr" 38 | - max_speed: 6 39 | map_name: "f1_gbr" 40 | - max_speed: 7 41 | map_name: "f1_gbr" 42 | - max_speed: 8 43 | map_name: "f1_gbr" 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /config/Cth_speeds.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "Cth_speeds" 3 | 4 | architecture: "fast" 5 | 6 | n_scans: 2 7 | train_mode: "Std" 8 | test_mode: "Std" 9 | 10 | n: 4 11 | set_n: 1 12 | 13 | random_seed: 10000 14 | 15 | n_train_steps: 100000 16 | n_test_laps: 20 17 | # n_test_laps: 50 18 | 19 | map_name: "f1_esp" 20 | reward: "Cth" 21 | 22 | runs: 23 | # - max_speed: 4 24 | # - max_speed: 5 25 | # - max_speed: 6 26 | - max_speed: 7 27 | - max_speed: 8 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /config/PP_maps8.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "PP_maps8" 3 | 4 | architecture: "PP" 5 | pp_speed_mode: "raceline" 6 | raceline: True 7 | 8 | n_scans: 2 9 | train_mode: "Std" 10 | test_mode: "Std" 11 | 12 | n: 1 13 | set_n: 1 14 | 15 | random_seed: 10000 16 | 17 | n_train_steps: 100000 18 | n_test_laps: 2 19 | # n_test_laps: 50 20 | noise_std: 0.0 21 | 22 | reward: "PP" 23 | max_speed: 8 24 | 25 | runs: 26 | - map_name: "f1_esp" 27 | - map_name: "f1_aut" 28 | - map_name: "f1_mco" 29 | - map_name: "f1_gbr" 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /config/PP_speeds.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "PP_speeds" 3 | 4 | architecture: "PP" 5 | pp_speed_mode: "raceline" 6 | raceline: True 7 | 8 | n_scans: 2 9 | train_mode: "Std" 10 | test_mode: "Std" 11 | 12 | n: 1 13 | set_n: 1 14 | 15 | random_seed: 10000 16 | 17 | n_train_steps: 100000 18 | n_test_laps: 2 19 | # n_test_laps: 50 20 | noise_std: 0.0 21 | 22 | map_name: "f1_esp" 23 | reward: "PP" 24 | 25 | runs: 26 | - max_speed: 4 27 | - max_speed: 5 28 | - max_speed: 6 29 | - max_speed: 7 30 | - max_speed: 8 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /config/TAL_maps.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "TAL_mapsTest" 3 | # test_name: "TAL_maps8" 4 | 5 | architecture: "fast" 6 | n_scans: 2 7 | train_mode: "Std" 8 | test_mode: "Std" 9 | 10 | start_n: 0 11 | n: 5 12 | set_n: 5 13 | 14 | random_seed: 10000 15 | 16 | n_train_steps: 100000 17 | n_test_laps: 20 18 | # n_test_laps: 50 19 | 20 | # max_speed: 8 21 | max_speed: 6 22 | reward: "TAL" 23 | 24 | runs: 25 | - map_name: "f1_esp" 26 | - map_name: "f1_mco" 27 | - map_name: "f1_aut" 28 | - map_name: "f1_gbr" 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /config/TAL_maps2.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "TAL_maps2" 3 | 4 | architecture: "fast" 5 | n_scans: 2 6 | train_mode: "Std" 7 | test_mode: "Std" 8 | 9 | start_n: 0 10 | n: 5 11 | set_n: 5 12 | 13 | random_seed: 10000 14 | 15 | n_train_steps: 100000 16 | n_test_laps: 20 17 | # n_test_laps: 50 18 | 19 | # max_speed: 8 20 | max_speed: 6 21 | reward: "TAL" 22 | 23 | runs: 24 | # - map_name: "f1_esp" 25 | # - map_name: "f1_mco" 26 | # - map_name: "f1_aut" 27 | - map_name: "f1_gbr" 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /config/TAL_speeds.yaml: -------------------------------------------------------------------------------- 1 | 2 | test_name: "TAL_speeds" 3 | 4 | architecture: "fast" 5 | n_scans: 2 6 | train_mode: "Std" 7 | test_mode: "Std" 8 | 9 | n: 4 10 | set_n: 1 11 | 12 | random_seed: 10000 13 | 14 | n_train_steps: 100000 15 | n_test_laps: 20 16 | # n_test_laps: 50 17 | 18 | map_name: "f1_esp" 19 | reward: "TAL" 20 | 21 | runs: 22 | - max_speed: 4 23 | - max_speed: 5 24 | - max_speed: 6 25 | - max_speed: 7 26 | # - max_speed: 8 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /config/config_file.yaml: -------------------------------------------------------------------------------- 1 | # Vehicle params 2 | max_v: 8 3 | max_a: 8.5 4 | max_steer: 0.4 5 | max_d_dot: 3.2 6 | mu: 0.523 7 | m: 3.47 8 | g: 9.81 9 | l_r: 0.171 10 | l_f: 0.158 11 | 12 | # agent parameters 13 | n_beams: 20 # number of beams used by agent 14 | range_finder_scale: 10 # scaling of beams for NN 15 | h_size: 100 # size of hidden layers in NN 16 | 17 | #simulator 18 | sim_steps: 10 # Number of simulator steps per planning step 19 | max_steps: 1000 20 | max_laptime: 240 21 | 22 | # PurePursuit planner values 23 | lookahead: 1.5 24 | # lookahead: 0.82 25 | v_gain: 0.9 26 | v_min_plan: 1 # minimum speed for selecting a steering angle to prevent instability 27 | 28 | r_velocity: 1 29 | r_distance: 0.5 30 | constant_reward: 1 31 | 32 | r_time: 0.05 33 | 34 | #save_paths 35 | vehicle_path: "Data/Vehicles/" 36 | kernel_path: "Data/Kernels/" 37 | dynamics_path: "Data/Dynamics/" 38 | 39 | lookahead_time_step: 0.2 # Planning timestep used the supervisor 40 | 41 | 42 | -------------------------------------------------------------------------------- /maps/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/.gitkeep -------------------------------------------------------------------------------- /maps/f1_aut.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/f1_aut.png -------------------------------------------------------------------------------- /maps/f1_aut.yaml: -------------------------------------------------------------------------------- 1 | image: f1_aut.png 2 | resolution: 0.050000 3 | origin: [-10.5, -22.0, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.2 7 | 8 | -------------------------------------------------------------------------------- /maps/f1_aut_wide.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/f1_aut_wide.png -------------------------------------------------------------------------------- /maps/f1_aut_wide.yaml: -------------------------------------------------------------------------------- 1 | image: f1_aut_wide.png 2 | # image: f1_aut_wide_filled.png 3 | resolution: 0.050000 4 | origin: [-10, -22.6, 0] 5 | # origin: [-4, -8.9, 0] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.2 9 | 10 | -------------------------------------------------------------------------------- /maps/f1_esp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/f1_esp.png -------------------------------------------------------------------------------- /maps/f1_esp.yaml: -------------------------------------------------------------------------------- 1 | image: f1_esp.png 2 | resolution: 0.050000 3 | origin: [-38.5, -24.4, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.2 7 | 8 | -------------------------------------------------------------------------------- /maps/f1_gbr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/f1_gbr.png -------------------------------------------------------------------------------- /maps/f1_gbr.yaml: -------------------------------------------------------------------------------- 1 | image: f1_gbr.png 2 | resolution: 0.050000 3 | origin: [-22, -7.7, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.2 7 | 8 | -------------------------------------------------------------------------------- /maps/f1_mco.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BDEvan5/TrajectoryAidedLearning/2d1352a9aef9b533cb9c570750926a654296b810/maps/f1_mco.png -------------------------------------------------------------------------------- /maps/f1_mco.yaml: -------------------------------------------------------------------------------- 1 | image: f1_mco.png 2 | resolution: 0.050000 3 | origin: [-15, -48.6, 0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.2 7 | 8 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | VERSION = '0.0.1' 4 | DESCRIPTION = 'My F110 PhD results' 5 | LONG_DESCRIPTION = 'f110 planning agents for safe autonomous racing' 6 | 7 | # Setting up 8 | setup( 9 | # the name must match the folder name 'verysimplemodule' 10 | name="TrajectoryAidedLearning", 11 | version=VERSION, 12 | author="Benjamin Evans", 13 | author_email="", 14 | description=DESCRIPTION, 15 | long_description=LONG_DESCRIPTION, 16 | packages=find_packages(), 17 | install_requires=[], # add any additional packages that 18 | # needs to be installed along with your package. Eg: 'caer' 19 | 20 | keywords=['python', 'autonomous racing'], 21 | classifiers= [ 22 | "Development Status :: 3 - Alpha", 23 | "Intended Audience :: Education", 24 | "Programming Language :: Python :: 3", 25 | "Operating System :: Linux", 26 | ] 27 | ) 28 | --------------------------------------------------------------------------------