├── frictionmap ├── __init__.py └── src │ ├── __init__.py │ ├── plot_frictionmap_grid.py │ ├── reftrack_functions.py │ └── plot_frictionmap_data.py ├── helper_funcs_glob ├── __init__.py └── src │ ├── __init__.py │ ├── interp_track.py │ ├── export_traj_race.py │ ├── import_track.py │ ├── calc_min_bound_dists.py │ ├── export_traj_ltpl.py │ ├── prep_track.py │ ├── result_plots.py │ └── check_traj.py ├── opt_mintime_traj ├── powertrain_src │ ├── __init__.py │ ├── component_losses.PNG │ ├── component_temperatures.PNG │ ├── src │ │ ├── __init__.py │ │ ├── Inverter.py │ │ ├── Radiators.py │ │ └── EMachine.py │ └── Readme.md ├── __init__.py ├── var_friction_berlin.png ├── src │ ├── __init__.py │ ├── friction_map_interface.py │ ├── friction_map_plot.py │ ├── export_mintime_solution.py │ ├── extract_friction_coeffs.py │ └── approx_friction_map.py └── Readme.md ├── maps ├── Spa_map.png ├── Budapest_map.png ├── Montreal_map.png ├── Shanghai_map.png ├── Hockenheim_map.png ├── Melbourne_map.png ├── Spielberg_map.png ├── pingpong_clean.pgm ├── Silverstone_map.png ├── e7_floor3_small.pgm ├── e7_floor5_large.pgm ├── e7_floor5_square.pgm ├── e7_floor5_large_RAW.pgm ├── pingpong_clean.yaml ├── e7_floor3_small.yaml ├── e7_floor5_large.yaml ├── e7_floor5_square.yaml ├── Spa_map.yaml ├── Shanghai_map.yaml ├── Budapest_map.yaml ├── Melbourne_map.yaml ├── Montreal_map.yaml ├── Spielberg_map.yaml ├── Hockenheim_map.yaml └── Silverstone_map.yaml ├── opt_raceline_berlin.png ├── requirements.txt ├── inputs ├── veh_dyn_info │ ├── ax_max_machines.csv │ └── ggv.csv └── tracks │ ├── rounded_rectangle.csv │ ├── pingpong_clean.csv │ └── handling_track.csv ├── .gitignore ├── LICENSE ├── main_gen_frictionmap.py ├── params ├── f110.ini └── racecar.ini └── Readme.md /frictionmap/__init__.py: -------------------------------------------------------------------------------- 1 | import frictionmap.src 2 | -------------------------------------------------------------------------------- /helper_funcs_glob/__init__.py: -------------------------------------------------------------------------------- 1 | import helper_funcs_glob.src 2 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/__init__.py: -------------------------------------------------------------------------------- 1 | import opt_mintime_traj.powertrain_src.src 2 | -------------------------------------------------------------------------------- /maps/Spa_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Spa_map.png -------------------------------------------------------------------------------- /opt_mintime_traj/__init__.py: -------------------------------------------------------------------------------- 1 | import opt_mintime_traj.src 2 | import opt_mintime_traj.powertrain_src 3 | -------------------------------------------------------------------------------- /maps/Budapest_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Budapest_map.png -------------------------------------------------------------------------------- /maps/Montreal_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Montreal_map.png -------------------------------------------------------------------------------- /maps/Shanghai_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Shanghai_map.png -------------------------------------------------------------------------------- /maps/Hockenheim_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Hockenheim_map.png -------------------------------------------------------------------------------- /maps/Melbourne_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Melbourne_map.png -------------------------------------------------------------------------------- /maps/Spielberg_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Spielberg_map.png -------------------------------------------------------------------------------- /maps/pingpong_clean.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/pingpong_clean.pgm -------------------------------------------------------------------------------- /opt_raceline_berlin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/opt_raceline_berlin.png -------------------------------------------------------------------------------- /maps/Silverstone_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/Silverstone_map.png -------------------------------------------------------------------------------- /maps/e7_floor3_small.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/e7_floor3_small.pgm -------------------------------------------------------------------------------- /maps/e7_floor5_large.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/e7_floor5_large.pgm -------------------------------------------------------------------------------- /maps/e7_floor5_square.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/e7_floor5_square.pgm -------------------------------------------------------------------------------- /maps/e7_floor5_large_RAW.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/maps/e7_floor5_large_RAW.pgm -------------------------------------------------------------------------------- /opt_mintime_traj/var_friction_berlin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/opt_mintime_traj/var_friction_berlin.png -------------------------------------------------------------------------------- /frictionmap/src/__init__.py: -------------------------------------------------------------------------------- 1 | import frictionmap.src.reftrack_functions 2 | import frictionmap.src.plot_frictionmap_data 3 | import frictionmap.src.plot_frictionmap_grid 4 | -------------------------------------------------------------------------------- /maps/pingpong_clean.yaml: -------------------------------------------------------------------------------- 1 | image: pingpong.pgm 2 | mode: trinary 3 | resolution: 0.1 4 | origin: [-13.1, -8.16, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/component_losses.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/opt_mintime_traj/powertrain_src/component_losses.PNG -------------------------------------------------------------------------------- /maps/e7_floor3_small.yaml: -------------------------------------------------------------------------------- 1 | image: e7_floor3_small.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-50.1, -27.3, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /maps/e7_floor5_large.yaml: -------------------------------------------------------------------------------- 1 | image: e7_floor3_large.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-50.1, -27.3, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /maps/e7_floor5_square.yaml: -------------------------------------------------------------------------------- 1 | image: e7_floor5_square.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-18.8, -34.1, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/component_temperatures.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CL2-UWaterloo/Raceline-Optimization/HEAD/opt_mintime_traj/powertrain_src/component_temperatures.PNG -------------------------------------------------------------------------------- /maps/Spa_map.yaml: -------------------------------------------------------------------------------- 1 | image: Spa_map.png 2 | resolution: 0.09267 3 | origin: [-76.27387694462932,-147.33397683885576, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Shanghai_map.yaml: -------------------------------------------------------------------------------- 1 | image: Shanghai_map.png 2 | resolution: 0.06505 3 | origin: [-75.7934346866365,-35.95358066976158, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Budapest_map.yaml: -------------------------------------------------------------------------------- 1 | image: Budapest_map.png 2 | resolution: 0.06446 3 | origin: [-60.229461062734515,-21.68761063512184, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Melbourne_map.yaml: -------------------------------------------------------------------------------- 1 | image: Melbourne_map.png 2 | resolution: 0.09009 3 | origin: [-81.75675943118459,-49.98378618463543, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Montreal_map.yaml: -------------------------------------------------------------------------------- 1 | image: Montreal_map.png 2 | resolution: 0.07134 3 | origin: [-89.38488117741169,-31.937534079976547, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Spielberg_map.yaml: -------------------------------------------------------------------------------- 1 | image: Spielberg_map.png 2 | resolution: 0.05796 3 | origin: [-84.85359914210505,-36.30299725862132, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Hockenheim_map.yaml: -------------------------------------------------------------------------------- 1 | image: Hockenheim_map.png 2 | resolution: 0.06702 3 | origin: [-19.911213834397966,-50.87622999687852, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /maps/Silverstone_map.yaml: -------------------------------------------------------------------------------- 1 | image: Silverstone_map.png 2 | resolution: 0.07712 3 | origin: [-43.82482139791572,-52.30388391024793, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.45 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/src/__init__.py: -------------------------------------------------------------------------------- 1 | import opt_mintime_traj.powertrain_src.src.Battery 2 | import opt_mintime_traj.powertrain_src.src.EMachine 3 | import opt_mintime_traj.powertrain_src.src.Inverter 4 | import opt_mintime_traj.powertrain_src.src.Radiators 5 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.18.1 2 | matplotlib==3.3.1 3 | git+https://github.com/CL2-UWaterloo/trajectory_planning_helpers.git 4 | # the following is required for minimum time optimization only 5 | casadi==3.5.1 6 | scipy==1.3.3 7 | scikit-learn==0.23.1 8 | scikit-image 9 | PyYAML 10 | pandas 11 | argparse -------------------------------------------------------------------------------- /inputs/veh_dyn_info/ax_max_machines.csv: -------------------------------------------------------------------------------- 1 | # v_mps,ax_max_machines_mps2 2 | 0.0,5.3 3 | 4.0,5.3 4 | 8.0,5.3 5 | 12.0,5.3 6 | 16.0,5.3 7 | 20.0,5.3 8 | 24.0,5.3 9 | 28.0,5.3 10 | 32.0,5.3 11 | 36.0,5.3 12 | 40.0,5.1 13 | 44.0,5.0 14 | 48.0,4.6 15 | 52.0,4.1 16 | 56.0,3.7 17 | 60.0,2.7 18 | 66.0,2.2 19 | 72.0,1.5 20 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/__init__.py: -------------------------------------------------------------------------------- 1 | import opt_mintime_traj.src.opt_mintime 2 | import opt_mintime_traj.src.result_plots_mintime 3 | import opt_mintime_traj.src.export_mintime_solution 4 | import opt_mintime_traj.src.approx_friction_map 5 | import opt_mintime_traj.src.extract_friction_coeffs 6 | import opt_mintime_traj.src.friction_map_interface 7 | import opt_mintime_traj.src.friction_map_plot 8 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/__init__.py: -------------------------------------------------------------------------------- 1 | import helper_funcs_glob.src.interp_track 2 | import helper_funcs_glob.src.calc_min_bound_dists 3 | import helper_funcs_glob.src.check_traj 4 | import helper_funcs_glob.src.export_traj_race 5 | import helper_funcs_glob.src.export_traj_ltpl 6 | import helper_funcs_glob.src.import_track 7 | import helper_funcs_glob.src.result_plots 8 | import helper_funcs_glob.src.prep_track 9 | -------------------------------------------------------------------------------- /inputs/veh_dyn_info/ggv.csv: -------------------------------------------------------------------------------- 1 | # v_mps,ax_max_mps2,ay_max_mps2 2 | 0.0,12.0,12.0 3 | 4.0,12.0,12.0 4 | 8.0,12.0,12.0 5 | 12.0,12.0,12.0 6 | 16.0,12.0,12.0 7 | 20.0,12.0,12.0 8 | 24.0,12.0,12.0 9 | 28.0,12.0,12.0 10 | 32.0,12.0,12.0 11 | 36.0,12.0,12.0 12 | 40.0,12.0,12.0 13 | 44.0,12.0,12.0 14 | 48.0,12.0,12.0 15 | 52.0,12.0,12.0 16 | 56.0,12.0,12.0 17 | 60.0,12.0,12.0 18 | 66.0,12.0,12.0 19 | 72.0,12.0,12.0 20 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/Readme.md: -------------------------------------------------------------------------------- 1 | # Powertrain Behavior 2 | By switching on the powertrain behavior as explained in both `Readme.md` files on the top level of this repository and 3 | in the directory `/opt_mintime_traj` you can consider the powertrain behavior (thermal behavior, power losses, 4 | state of charge) during the trajectory optimization. This feature gets especially interesting when dealing with multiple 5 | consecutive race laps, see main `Readme.md` `Step 6`. 6 | 7 | By specifying the option `/params/racecar.ini:simple_loss = True`, simple power loss approximations of the 8 | powertrain components are used. The option `/params/racecar.ini:simple_loss = False` considers a more detailed 9 | powertrain description. 10 | 11 | The results can look like the following plot. It shows the temperatures of the 12 | - electric machines , 13 | - battery , 14 | - inverters , 15 | - cooling liquid for the motor-inverter circuit , 16 | - cooling liquid for the battery circuit . 17 | 18 | The powertrain signals can look like the following two plots, describing the components' temperatures and the 19 | corresponding power losses, when using the detailed powertrain descriptions: 20 | 21 | ![Powertrain component temperatures whilst driving one race lap on the Berlin (Germany) Formel E track.](component_temperatures.PNG) 22 | 23 | ![Powertrain component losses whilst driving one race lap on the Berlin (Germany) Formel E track.](component_losses.PNG) 24 | 25 | # References 26 | Powertrain Behavior\ 27 | Herrmann, Passigato, Betz, Lienkamp\ 28 | Minimum Race-Time Planning-Strategy for an Autonomous Electric Racecar\ 29 | DOI: 10.1109/ITSC45102.2020.9294681\ 30 | Preprint: https://arxiv.org/abs/2005.07127 \ 31 | Contact person: [Thomas Herrmann](mailto:thomas.herrmann@tum.de). 32 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/interp_track.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | 5 | def interp_track(reftrack: np.ndarray, 6 | stepsize_approx: float = 1.0) -> np.ndarray: 7 | """ 8 | Created by: 9 | Alexander Heilmeier 10 | 11 | Documentation: 12 | Use linear interpolation between track points to create new points with equal distances. 13 | 14 | Inputs: 15 | reftrack: array containing the track information that shell be interpolated [x, y, w_tr_right, w_tr_left]. 16 | stepsize_approx: desired stepsize for the interpolation 17 | 18 | Outputs: 19 | reftrack_interp: interpolated reference track (unclosed) 20 | """ 21 | 22 | # ------------------------------------------------------------------------------------------------------------------ 23 | # FUNCTION BODY ---------------------------------------------------------------------------------------------------- 24 | # ------------------------------------------------------------------------------------------------------------------ 25 | 26 | reftrack_cl = np.vstack((reftrack, reftrack[0])) 27 | 28 | # calculate element lengths (euclidian distance) 29 | el_lenghts = np.sqrt(np.sum(np.power(np.diff(reftrack_cl[:, :2], axis=0), 2), axis=1)) 30 | 31 | # sum up total distance (from start) to every element 32 | dists_cum = np.cumsum(el_lenghts) 33 | dists_cum = np.insert(dists_cum, 0, 0.0) 34 | 35 | # calculate desired lenghts depending on specified stepsize (+1 because last element is included) 36 | no_points_interp = math.ceil(dists_cum[-1] / stepsize_approx) + 1 37 | dists_interp = np.linspace(0.0, dists_cum[-1], no_points_interp) 38 | 39 | # interpolate closed track points 40 | reftrack_interp_cl = np.zeros((no_points_interp, 4)) 41 | reftrack_interp_cl[:, 0] = np.interp(dists_interp, dists_cum, reftrack_cl[:, 0]) 42 | reftrack_interp_cl[:, 1] = np.interp(dists_interp, dists_cum, reftrack_cl[:, 1]) 43 | reftrack_interp_cl[:, 2] = np.interp(dists_interp, dists_cum, reftrack_cl[:, 2]) 44 | reftrack_interp_cl[:, 3] = np.interp(dists_interp, dists_cum, reftrack_cl[:, 3]) 45 | 46 | # remove closed points 47 | reftrack_interp = reftrack_interp_cl[:-1] 48 | 49 | return reftrack_interp 50 | 51 | 52 | # testing -------------------------------------------------------------------------------------------------------------- 53 | if __name__ == "__main__": 54 | pass 55 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/export_traj_race.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import uuid 3 | import hashlib 4 | 5 | 6 | def export_traj_race(file_paths: dict, 7 | traj_race: np.ndarray) -> None: 8 | """ 9 | Created by: 10 | Alexander Heilmeier 11 | 12 | Documentation: 13 | This function is used to export the generated trajectory into a file. The generated files get an unique UUID and a 14 | hash of the ggv diagram to be able to check it later. 15 | 16 | Inputs: 17 | file_paths: paths for input and output files {ggv_file, traj_race_export, traj_ltpl_export, lts_export} 18 | traj_race: race trajectory [s_m, x_m, y_m, psi_rad, kappa_radpm, vx_mps, ax_mps2] 19 | """ 20 | 21 | # create random UUID 22 | rand_uuid = str(uuid.uuid4()) 23 | 24 | # hash ggv file with SHA1 25 | if "ggv_file" in file_paths: 26 | with open(file_paths["ggv_file"], 'br') as fh: 27 | ggv_content = fh.read() 28 | else: 29 | ggv_content = np.array([]) 30 | ggv_hash = hashlib.sha1(ggv_content).hexdigest() 31 | 32 | # write UUID and GGV hash into file 33 | with open(file_paths["traj_race_export"], 'w') as fh: 34 | fh.write("# " + rand_uuid + "\n") 35 | fh.write("# " + ggv_hash + "\n") 36 | 37 | # export race trajectory 38 | header = "s_m; x_m; y_m; psi_rad; kappa_radpm; vx_mps; ax_mps2" 39 | fmt = "%.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f" 40 | with open(file_paths["traj_race_export"], 'ab') as fh: 41 | np.savetxt(fh, traj_race, fmt=fmt, header=header) 42 | 43 | def export_traj_race_f110(file_paths: dict, 44 | traj_race: np.ndarray) -> None: 45 | """ 46 | Created by: 47 | Steven Gong 48 | 49 | Documentation: 50 | This function is used to export the generated trajectory into a csv file. 51 | We only need the x,y and velocity profile in the case of the f110 pure pursuit. 52 | 53 | Inputs: 54 | file_paths: paths for input and output files {ggv_file, traj_race_export, traj_ltpl_export, lts_export} 55 | traj_race: race trajectory [s_m, x_m, y_m, psi_rad, kappa_radpm, vx_mps, ax_mps2] 56 | """ 57 | 58 | # export race trajectory 59 | fmt = "%.7f,%.7f,%.7f" 60 | with open(file_paths["traj_race_export"], 'ab') as fh: 61 | np.savetxt(fh, traj_race[:, [1,2,5]], fmt=fmt, comments='') 62 | 63 | # testing -------------------------------------------------------------------------------------------------------------- 64 | if __name__ == "__main__": 65 | pass 66 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | .DS_Store 3 | __pycache__/ 4 | *.py[cod] 5 | *$py.class 6 | 7 | # C extensions 8 | *.so 9 | 10 | # Distribution / packaging 11 | .Python 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | pip-wheel-metadata/ 25 | share/python-wheels/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | MANIFEST 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .nox/ 45 | .coverage 46 | .coverage.* 47 | .cache 48 | nosetests.xml 49 | coverage.xml 50 | *.cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # celery beat schedule file 95 | celerybeat-schedule 96 | 97 | # SageMath parsed files 98 | *.sage.py 99 | 100 | # Environments 101 | .env 102 | .venv 103 | env/ 104 | venv/ 105 | ENV/ 106 | env.bak/ 107 | venv.bak/ 108 | 109 | opt_energy_constraints/src/\.mypy_cache/ 110 | 111 | comp_approx/DATA\.mat 112 | 113 | *.mat 114 | 115 | venv/ 116 | 117 | comp_approx/data/ 118 | 119 | # Spyder project settings 120 | .spyderproject 121 | .spyproject 122 | 123 | # Rope project settings 124 | .ropeproject 125 | 126 | # mkdocs documentation 127 | /site 128 | 129 | # mypy 130 | .mypy_cache/ 131 | .dmypy.json 132 | dmypy.json 133 | 134 | # Pyre type checker 135 | .pyre/ 136 | 137 | # Pycharm stuff 138 | .idea 139 | 140 | # repo specific 141 | outputs 142 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/import_track.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def import_track(file_path: str, 5 | imp_opts: dict, 6 | width_veh: float) -> np.ndarray: 7 | """ 8 | Created by: 9 | Alexander Heilmeier 10 | Modified by: 11 | Thomas Herrmann 12 | 13 | Documentation: 14 | This function includes the algorithm part connected to the import of the track. 15 | 16 | Inputs: 17 | file_path: file path of track.csv containing [x_m,y_m,w_tr_right_m,w_tr_left_m] 18 | imp_opts: import options showing if a new starting point should be set or if the direction should be reversed 19 | width_veh: vehicle width required to check against track width 20 | 21 | Outputs: 22 | reftrack_imp: imported track [x_m, y_m, w_tr_right_m, w_tr_left_m] 23 | """ 24 | 25 | # load data from csv file 26 | csv_data_temp = np.loadtxt(file_path, comments='#', delimiter=',') 27 | 28 | # get coords and track widths out of array 29 | if np.shape(csv_data_temp)[1] == 3: 30 | refline_ = csv_data_temp[:, 0:2] 31 | w_tr_r = csv_data_temp[:, 2] / 2 32 | w_tr_l = w_tr_r 33 | 34 | elif np.shape(csv_data_temp)[1] == 4: 35 | refline_ = csv_data_temp[:, 0:2] 36 | w_tr_r = csv_data_temp[:, 2] 37 | w_tr_l = csv_data_temp[:, 3] 38 | 39 | elif np.shape(csv_data_temp)[1] == 5: # omit z coordinate in this case 40 | refline_ = csv_data_temp[:, 0:2] 41 | w_tr_r = csv_data_temp[:, 3] 42 | w_tr_l = csv_data_temp[:, 4] 43 | 44 | else: 45 | raise IOError("Track file cannot be read!") 46 | 47 | refline_ = np.tile(refline_, (imp_opts["num_laps"], 1)) 48 | w_tr_r = np.tile(w_tr_r, imp_opts["num_laps"]) 49 | w_tr_l = np.tile(w_tr_l, imp_opts["num_laps"]) 50 | 51 | # assemble to a single array 52 | reftrack_imp = np.column_stack((refline_, w_tr_r, w_tr_l)) 53 | 54 | # check if imported centerline should be flipped, i.e. reverse direction 55 | if imp_opts["flip_imp_track"]: 56 | reftrack_imp = np.flipud(reftrack_imp) 57 | 58 | # check if imported centerline should be reordered for a new starting point 59 | if imp_opts["set_new_start"]: 60 | ind_start = np.argmin(np.power(reftrack_imp[:, 0] - imp_opts["new_start"][0], 2) 61 | + np.power(reftrack_imp[:, 1] - imp_opts["new_start"][1], 2)) 62 | reftrack_imp = np.roll(reftrack_imp, reftrack_imp.shape[0] - ind_start, axis=0) 63 | 64 | # check minimum track width for vehicle width plus a small safety margin 65 | w_tr_min = np.amin(reftrack_imp[:, 2] + reftrack_imp[:, 3]) 66 | 67 | if w_tr_min < width_veh + 0.5: 68 | print("WARNING: Minimum track width %.2fm is close to or smaller than vehicle width!" % np.amin(w_tr_min)) 69 | 70 | return reftrack_imp 71 | 72 | 73 | # testing -------------------------------------------------------------------------------------------------------------- 74 | if __name__ == "__main__": 75 | pass 76 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/calc_min_bound_dists.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | 5 | def calc_min_bound_dists(trajectory: np.ndarray, 6 | bound1: np.ndarray, 7 | bound2: np.ndarray, 8 | length_veh: float, 9 | width_veh: float) -> np.ndarray: 10 | """ 11 | Created by: 12 | Alexander Heilmeier 13 | 14 | Documentation: 15 | Calculate minimum distance between vehicle and track boundaries for every trajectory point. Vehicle dimensions are 16 | taken into account for this calculation. Vehicle orientation is assumed to be the same as the heading of the 17 | trajectory. 18 | 19 | Inputs: 20 | trajectory: array containing the trajectory information. Required are x, y, psi for every point 21 | bound1/2: arrays containing the track boundaries [x, y] 22 | length_veh: real vehicle length in m 23 | width_veh: real vehicle width in m 24 | 25 | Outputs: 26 | min_dists: minimum distance to boundaries for every trajectory point 27 | """ 28 | 29 | # ------------------------------------------------------------------------------------------------------------------ 30 | # CALCULATE MINIMUM DISTANCES -------------------------------------------------------------------------------------- 31 | # ------------------------------------------------------------------------------------------------------------------ 32 | 33 | bounds = np.vstack((bound1, bound2)) 34 | 35 | # calculate static vehicle edge positions [x, y] for psi = 0 36 | fl = np.array([-width_veh / 2, length_veh / 2]) 37 | fr = np.array([width_veh / 2, length_veh / 2]) 38 | rl = np.array([-width_veh / 2, -length_veh / 2]) 39 | rr = np.array([width_veh / 2, -length_veh / 2]) 40 | 41 | # loop through all the raceline points 42 | min_dists = np.zeros(trajectory.shape[0]) 43 | mat_rot = np.zeros((2, 2)) 44 | 45 | for i in range(trajectory.shape[0]): 46 | mat_rot[0, 0] = math.cos(trajectory[i, 3]) 47 | mat_rot[0, 1] = -math.sin(trajectory[i, 3]) 48 | mat_rot[1, 0] = math.sin(trajectory[i, 3]) 49 | mat_rot[1, 1] = math.cos(trajectory[i, 3]) 50 | 51 | # calculate positions of vehicle edges 52 | fl_ = trajectory[i, 1:3] + np.matmul(mat_rot, fl) 53 | fr_ = trajectory[i, 1:3] + np.matmul(mat_rot, fr) 54 | rl_ = trajectory[i, 1:3] + np.matmul(mat_rot, rl) 55 | rr_ = trajectory[i, 1:3] + np.matmul(mat_rot, rr) 56 | 57 | # get minimum distances of vehicle edges to any boundary point 58 | fl__mindist = np.sqrt(np.power(bounds[:, 0] - fl_[0], 2) + np.power(bounds[:, 1] - fl_[1], 2)) 59 | fr__mindist = np.sqrt(np.power(bounds[:, 0] - fr_[0], 2) + np.power(bounds[:, 1] - fr_[1], 2)) 60 | rl__mindist = np.sqrt(np.power(bounds[:, 0] - rl_[0], 2) + np.power(bounds[:, 1] - rl_[1], 2)) 61 | rr__mindist = np.sqrt(np.power(bounds[:, 0] - rr_[0], 2) + np.power(bounds[:, 1] - rr_[1], 2)) 62 | 63 | # save overall minimum distance of current vehicle position 64 | min_dists[i] = np.amin((fl__mindist, fr__mindist, rl__mindist, rr__mindist)) 65 | 66 | return min_dists 67 | 68 | 69 | # testing -------------------------------------------------------------------------------------------------------------- 70 | if __name__ == "__main__": 71 | pass 72 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/export_traj_ltpl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import uuid 3 | import hashlib 4 | 5 | 6 | def export_traj_ltpl(file_paths: dict, 7 | spline_lengths_opt, 8 | trajectory_opt, 9 | reftrack, 10 | normvec_normalized, 11 | alpha_opt) -> None: 12 | """ 13 | Created by: 14 | Tim Stahl 15 | Alexander Heilmeier 16 | 17 | Documentation: 18 | This function is used to export the generated trajectory into a file for further usage in the local trajectory 19 | planner on the car (including map information via normal vectors and bound widths). The generated files get an 20 | unique UUID and a hash of the ggv diagram to be able to check it later. 21 | 22 | The stored trajectory has the following columns: 23 | [x_ref_m, y_ref_m, width_right_m, width_left_m, x_normvec_m, y_normvec_m, alpha_m, s_racetraj_m, psi_racetraj_rad, 24 | kappa_racetraj_radpm, vx_racetraj_mps, ax_racetraj_mps2] 25 | 26 | Inputs: 27 | file_paths: paths for input and output files {ggv_file, traj_race_export, traj_ltpl_export, lts_export} 28 | spline_lengths_opt: lengths of the splines on the raceline in m 29 | trajectory_opt: generated race trajectory 30 | reftrack: track definition [x_m, y_m, w_tr_right_m, w_tr_left_m] 31 | normvec_normalized: normalized normal vectors on the reference line [x_m, y_m] 32 | alpha_opt: solution vector of the opt. problem containing the lateral shift in m for every ref-point 33 | """ 34 | 35 | # convert trajectory to desired format 36 | s_raceline_preinterp_cl = np.cumsum(spline_lengths_opt) 37 | s_raceline_preinterp_cl = np.insert(s_raceline_preinterp_cl, 0, 0.0) 38 | 39 | psi_normvec = [] 40 | kappa_normvec = [] 41 | vx_normvec = [] 42 | ax_normvec = [] 43 | 44 | for s in list(s_raceline_preinterp_cl[:-1]): 45 | # get closest point on trajectory_opt 46 | idx = (np.abs(trajectory_opt[:, 0] - s)).argmin() 47 | 48 | # get data at this index and append 49 | psi_normvec.append(trajectory_opt[idx, 3]) 50 | kappa_normvec.append(trajectory_opt[idx, 4]) 51 | vx_normvec.append(trajectory_opt[idx, 5]) 52 | ax_normvec.append(trajectory_opt[idx, 6]) 53 | 54 | traj_ltpl = np.column_stack((reftrack, 55 | normvec_normalized, 56 | alpha_opt, 57 | s_raceline_preinterp_cl[:-1], 58 | psi_normvec, 59 | kappa_normvec, 60 | vx_normvec, 61 | ax_normvec)) 62 | traj_ltpl_cl = np.vstack((traj_ltpl, traj_ltpl[0])) 63 | traj_ltpl_cl[-1, 7] = s_raceline_preinterp_cl[-1] 64 | 65 | # create random UUID 66 | rand_uuid = str(uuid.uuid4()) 67 | 68 | # hash ggv file with SHA1 69 | if "ggv_file" in file_paths: 70 | with open(file_paths["ggv_file"], 'br') as fh: 71 | ggv_content = fh.read() 72 | else: 73 | ggv_content = np.array([]) 74 | ggv_hash = hashlib.sha1(ggv_content).hexdigest() 75 | 76 | # write UUID and GGV hash into file 77 | with open(file_paths["traj_ltpl_export"], 'w') as fh: 78 | fh.write("# " + rand_uuid + "\n") 79 | fh.write("# " + ggv_hash + "\n") 80 | 81 | # export trajectory data for local planner 82 | header = "x_ref_m; y_ref_m; width_right_m; width_left_m; x_normvec_m; y_normvec_m; " \ 83 | "alpha_m; s_racetraj_m; psi_racetraj_rad; kappa_racetraj_radpm; vx_racetraj_mps; ax_racetraj_mps2" 84 | fmt = "%.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f" 85 | with open(file_paths["traj_ltpl_export"], 'ab') as fh: 86 | np.savetxt(fh, traj_ltpl, fmt=fmt, header=header) 87 | 88 | 89 | # testing -------------------------------------------------------------------------------------------------------------- 90 | if __name__ == "__main__": 91 | pass 92 | -------------------------------------------------------------------------------- /inputs/tracks/rounded_rectangle.csv: -------------------------------------------------------------------------------- 1 | # x_m,y_m,w_tr_right_m,w_tr_left_m 2 | 0,0,5,5 3 | 1.22E-16,2,5,5 4 | 2.45E-16,4,5,5 5 | 3.67E-16,6,5,5 6 | 4.90E-16,8,5,5 7 | 6.12E-16,10,5,5 8 | 7.35E-16,12,5,5 9 | 8.57E-16,14,5,5 10 | 9.80E-16,16,5,5 11 | 1.10E-15,18,5,5 12 | 1.22E-15,20,5,5 13 | 1.35E-15,22,5,5 14 | 1.47E-15,24,5,5 15 | 1.59E-15,26,5,5 16 | 1.71E-15,28,5,5 17 | 1.84E-15,30,5,5 18 | 1.96E-15,32,5,5 19 | 2.08E-15,34,5,5 20 | 2.20E-15,36,5,5 21 | 2.33E-15,38,5,5 22 | 2.45E-15,40,5,5 23 | 2.57E-15,42,5,5 24 | 2.69E-15,44,5,5 25 | 2.82E-15,46,5,5 26 | 2.94E-15,48,5,5 27 | 3.06E-15,50,5,5 28 | 3.18E-15,52,5,5 29 | 3.31E-15,54,5,5 30 | 3.43E-15,56,5,5 31 | 3.55E-15,58,5,5 32 | 3.67E-15,60,5,5 33 | 3.80E-15,62,5,5 34 | 3.92E-15,64,5,5 35 | 4.04E-15,66,5,5 36 | 4.16E-15,68,5,5 37 | 4.29E-15,70,5,5 38 | 4.41E-15,72,5,5 39 | 4.53E-15,74,5,5 40 | 4.65E-15,76,5,5 41 | 4.78E-15,78,5,5 42 | 4.90E-15,80,5,5 43 | 5.02E-15,82,5,5 44 | 5.14E-15,84,5,5 45 | 5.27E-15,86,5,5 46 | 5.39E-15,88,5,5 47 | 5.51E-15,90,5,5 48 | 5.63E-15,92,5,5 49 | 5.76E-15,94,5,5 50 | 5.88E-15,96,5,5 51 | 6.00E-15,98,5,5 52 | 6.12E-15,100,5,5 53 | -0.062448,101.25,5,5 54 | -0.24917,102.48,5,5 55 | -0.55829,103.69,5,5 56 | -0.98674,104.87,5,5 57 | -1.5302,105.99,5,5 58 | -2.1833,107.06,5,5 59 | -2.9395,108.05,5,5 60 | -3.7912,108.97,5,5 61 | -4.7299,109.79,5,5 62 | -5.7462,110.52,5,5 63 | -6.83,111.14,5,5 64 | -7.9705,111.65,5,5 65 | -9.1563,112.04,5,5 66 | -10.375,112.32,5,5 67 | -11.616,112.47,5,5 68 | -13.616,112.47,5,5 69 | -15.616,112.47,5,5 70 | -17.616,112.47,5,5 71 | -19.616,112.47,5,5 72 | -21.616,112.47,5,5 73 | -23.616,112.47,5,5 74 | -25.616,112.47,5,5 75 | -27.616,112.47,5,5 76 | -29.616,112.47,5,5 77 | -31.616,112.47,5,5 78 | -32.864,112.41,5,5 79 | -34.099,112.22,5,5 80 | -35.31,111.91,5,5 81 | -36.484,111.48,5,5 82 | -37.609,110.94,5,5 83 | -38.674,110.29,5,5 84 | -39.669,109.53,5,5 85 | -40.583,108.68,5,5 86 | -41.407,107.74,5,5 87 | -42.134,106.72,5,5 88 | -42.756,105.64,5,5 89 | -43.266,104.5,5,5 90 | -43.66,103.31,5,5 91 | -43.934,102.09,5,5 92 | -44.084,100.85,5,5 93 | -44.084,98.853,5,5 94 | -44.084,96.853,5,5 95 | -44.084,94.853,5,5 96 | -44.084,92.853,5,5 97 | -44.084,90.853,5,5 98 | -44.084,88.853,5,5 99 | -44.084,86.853,5,5 100 | -44.084,84.853,5,5 101 | -44.084,82.853,5,5 102 | -44.084,80.853,5,5 103 | -44.084,78.853,5,5 104 | -44.084,76.853,5,5 105 | -44.084,74.853,5,5 106 | -44.084,72.853,5,5 107 | -44.084,70.853,5,5 108 | -44.084,68.853,5,5 109 | -44.084,66.853,5,5 110 | -44.084,64.853,5,5 111 | -44.084,62.853,5,5 112 | -44.084,60.853,5,5 113 | -44.084,58.853,5,5 114 | -44.084,56.853,5,5 115 | -44.084,54.853,5,5 116 | -44.084,52.853,5,5 117 | -44.084,50.853,5,5 118 | -44.084,48.853,5,5 119 | -44.084,46.853,5,5 120 | -44.084,44.853,5,5 121 | -44.084,42.853,5,5 122 | -44.084,40.853,5,5 123 | -44.084,38.853,5,5 124 | -44.084,36.853,5,5 125 | -44.084,34.853,5,5 126 | -44.084,32.853,5,5 127 | -44.084,30.853,5,5 128 | -44.084,28.853,5,5 129 | -44.084,26.853,5,5 130 | -44.084,24.853,5,5 131 | -44.084,22.853,5,5 132 | -44.084,20.853,5,5 133 | -44.084,18.853,5,5 134 | -44.084,16.853,5,5 135 | -44.084,14.853,5,5 136 | -44.084,12.853,5,5 137 | -44.084,10.853,5,5 138 | -44.084,8.8529,5,5 139 | -44.084,6.8529,5,5 140 | -44.084,4.8529,5,5 141 | -44.084,2.8529,5,5 142 | -44.084,0.8529,5,5 143 | -44.022,-0.39502,5,5 144 | -43.835,-1.6305,5,5 145 | -43.526,-2.8411,5,5 146 | -43.098,-4.0148,5,5 147 | -42.554,-5.1399,5,5 148 | -41.901,-6.2051,5,5 149 | -41.145,-7.1998,5,5 150 | -40.293,-8.114,5,5 151 | -39.355,-8.9387,5,5 152 | -38.338,-9.6655,5,5 153 | -37.254,-10.287,5,5 154 | -36.114,-10.798,5,5 155 | -34.928,-11.192,5,5 156 | -33.709,-11.465,5,5 157 | -32.469,-11.616,5,5 158 | -30.469,-11.616,5,5 159 | -28.469,-11.616,5,5 160 | -26.469,-11.616,5,5 161 | -24.469,-11.616,5,5 162 | -22.469,-11.616,5,5 163 | -20.469,-11.616,5,5 164 | -18.469,-11.616,5,5 165 | -16.469,-11.616,5,5 166 | -14.469,-11.616,5,5 167 | -12.469,-11.616,5,5 168 | -11.221,-11.553,5,5 169 | -9.9853,-11.367,5,5 170 | -8.7747,-11.057,5,5 171 | -7.601,-10.629,5,5 172 | -6.4759,-10.086,5,5 173 | -5.4107,-9.4325,5,5 174 | -4.416,-8.6763,5,5 175 | -3.5017,-7.8246,5,5 176 | -2.6771,-6.8859,5,5 177 | -1.9503,-5.8696,5,5 178 | -1.3286,-4.7857,5,5 179 | -0.8182,-3.6453,5,5 180 | -0.42421,-2.4595,5,5 181 | -0.15057,-1.2404,5,5 182 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/friction_map_interface.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial import cKDTree 3 | import json 4 | 5 | 6 | class FrictionMapInterface: 7 | """ 8 | Created by: 9 | Leonhard Hermansdorfer 10 | 11 | Documentation: 12 | This class loads the friction map (*_tpamap.csv) and the corresponding data (*_tpadata.json) and provides an 13 | interface to fetch friction data for a requested position on the race track. 14 | 15 | NOTE: Naming of map and data file has to be consistent! Everything replaced by '*' has to be identical in 16 | order to load correct data to a given map. 17 | 18 | The following data must be available for the friction map: 19 | tpa_map: csv-file containing the map information (x,y-coordinates of each grid cell; 20 | '*_tpamap.csv' located in inputs folder) 21 | tpa_data: json-file containing specific data for each grid cell (e.g. coefficient of friction); 22 | '*_tpadata.json' located in inputs folder) 23 | """ 24 | 25 | def __init__(self, 26 | tpamap_path: str, 27 | tpadata_path: str) -> None: 28 | 29 | # load friction map (only contains x,y coordinates and the corresponding grid cell indices) and 30 | # friction data (contains coefficient of friction for each grid cell adressed by its index) 31 | 32 | # load friction map file and set up cKDtree for grid representation 33 | map_coords = np.loadtxt(tpamap_path, comments='#', delimiter=';') 34 | self.tpa_map = cKDTree(map_coords) 35 | 36 | # load friction data file and set up dictionary with grid cell index as key as mue value as value 37 | with open(tpadata_path, 'r') as fh: 38 | tpadata_dict_string = json.load(fh) 39 | 40 | self.tpa_data = {int(k): np.asarray(v) for k, v in tpadata_dict_string.items()} 41 | 42 | def get_friction_singlepos(self, 43 | positions: np.ndarray) -> np.array: 44 | """ 45 | This function returns the friction value mue for a given position. 46 | 47 | Inputs: 48 | positions: x,y coordinate(s) in meters from origin for position of requested friction value(s) 49 | [[x_0, y_0], [x_1, y_1], ...] (multiple coordinate points allowed) 50 | 51 | Outputs: 52 | mue_singlepos: array with coefficient of friction for requested positions (same number) 53 | [[mue_0], [mue_1], [mue_2], ...]] 54 | """ 55 | 56 | # check input 57 | if positions.size == 0: 58 | return np.asarray([]) 59 | 60 | # query requested positions to get indices of grid cells containing the corresponding mue values 61 | _, idxs = self.tpa_map.query(positions) 62 | 63 | # get mue-value(s) from dictionary 64 | mue_singlepos = [] 65 | 66 | for idx in idxs: 67 | mue_singlepos.append(self.tpa_data[idx]) 68 | 69 | return np.asarray(mue_singlepos) 70 | 71 | 72 | # testing -------------------------------------------------------------------------------------------------------------- 73 | if __name__ == '__main__': 74 | import os 75 | 76 | module_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 77 | inputs_path = os.path.join(module_path, "inputs", "frictionmaps") 78 | 79 | tpamap_path_ = os.path.join(inputs_path, "berlin_2018_tpamap.csv") 80 | tpadata_path_ = os.path.join(inputs_path, "berlin_2018_tpadata.json") 81 | 82 | mapint = FrictionMapInterface(tpamap_path=tpamap_path_, 83 | tpadata_path=tpadata_path_) 84 | 85 | position_ = np.asarray([[100.0, -80.0], 86 | [160.0, 560.0], 87 | [133.0, 20.0], 88 | [122.0, 10.0], 89 | [110.0, 64.0], 90 | [131.0, 45.0], 91 | [113.0, -58.0], 92 | [111.0, -21.0]]) 93 | 94 | mue = mapint.get_friction_singlepos(position_) 95 | print(mue) 96 | 97 | position_ = np.asarray([[0.0, 0.0]]) 98 | _ = mapint.get_friction_singlepos(position_) 99 | 100 | position_ = np.random.rand(300, 2) 101 | _ = mapint.get_friction_singlepos(position_) 102 | 103 | position_ = np.asarray([]) 104 | _ = mapint.get_friction_singlepos(position_) 105 | 106 | print('INFO: FrictionMapInterface tests passed!') 107 | -------------------------------------------------------------------------------- /inputs/tracks/pingpong_clean.csv: -------------------------------------------------------------------------------- 1 | # x_m,y_m,w_tr_right_m,w_tr_left_m 2 | -2.8000,0.8400,1.0705,1.0705 3 | -2.7000,0.9400,1.0705,1.0705 4 | -2.5000,1.1400,1.0705,1.0705 5 | -2.4000,1.2400,1.0705,1.0705 6 | -2.3000,1.4400,1.0705,1.0705 7 | -2.2000,1.5400,1.0705,1.0705 8 | -2.0000,1.6400,1.0705,1.0705 9 | -1.9000,1.7400,1.0705,1.0705 10 | -1.7000,1.7400,1.0705,1.0705 11 | -1.6000,1.8400,1.0705,1.0705 12 | -1.4000,2.0400,1.0705,1.0705 13 | -1.3000,2.1400,1.0705,1.0705 14 | -1.1000,2.2400,1.0705,1.0705 15 | -0.9000,2.3400,1.0705,1.0705 16 | -0.8000,2.3400,1.0705,1.0705 17 | -0.6000,2.4400,1.0705,1.0705 18 | -0.5000,2.5400,1.0705,1.0705 19 | -0.3000,2.5400,1.0705,1.0705 20 | -0.2000,2.6400,1.0705,1.0705 21 | 0.0000,2.6400,1.0705,1.0705 22 | 0.1000,2.7400,1.0705,1.0705 23 | 0.3000,2.6400,1.0705,1.0705 24 | 0.4000,2.5400,1.0705,1.0705 25 | 0.5000,2.4400,1.0705,1.0705 26 | 0.6000,2.3400,1.0705,1.0705 27 | 0.7000,2.3400,1.0705,1.0705 28 | 0.8000,2.2400,1.0705,1.0705 29 | 0.9000,2.2400,1.0705,1.0705 30 | 1.0000,2.0400,1.0705,1.0705 31 | 1.1000,1.9400,1.0705,1.0705 32 | 1.2000,1.7400,1.0705,1.0705 33 | 1.3000,1.6400,1.0705,1.0705 34 | 1.4000,1.4400,1.0705,1.0705 35 | 1.5000,1.4400,1.0705,1.0705 36 | 1.5000,1.2400,1.0705,1.0705 37 | 1.6000,1.0400,1.0705,1.0705 38 | 1.7000,0.9400,1.0705,1.0705 39 | 1.7000,0.7400,1.0705,1.0705 40 | 1.8000,0.6400,1.0705,1.0705 41 | 1.9000,0.4400,1.0705,1.0705 42 | 1.9000,0.3400,1.0705,1.0705 43 | 1.7000,0.1400,1.0705,1.0705 44 | 1.7000,0.0400,1.0705,1.0705 45 | 1.6000,-0.1600,1.0705,1.0705 46 | 1.6000,-0.2600,1.0705,1.0705 47 | 1.5000,-0.4600,1.0705,1.0705 48 | 1.4000,-0.6600,1.0705,1.0705 49 | 1.4000,-0.7600,1.0705,1.0705 50 | 1.4000,-0.9600,1.0705,1.0705 51 | 1.4000,-1.0600,1.0705,1.0705 52 | 1.3000,-1.2600,1.0705,1.0705 53 | 1.3000,-1.3600,1.0705,1.0705 54 | 1.1000,-1.5600,1.0705,1.0705 55 | 1.1000,-1.6600,1.0705,1.0705 56 | 0.9000,-1.8600,1.0705,1.0705 57 | 0.9000,-1.9600,1.0705,1.0705 58 | 0.8000,-2.1600,1.0705,1.0705 59 | 0.8000,-2.2600,1.0705,1.0705 60 | 0.7000,-2.4600,1.0705,1.0705 61 | 0.6000,-2.6600,1.0705,1.0705 62 | 0.6000,-2.7600,1.0705,1.0705 63 | 0.4000,-2.9600,1.0705,1.0705 64 | 0.3000,-2.9600,1.0705,1.0705 65 | 0.2000,-3.1600,1.0705,1.0705 66 | 0.1000,-3.2600,1.0705,1.0705 67 | 0.0000,-3.4600,1.0705,1.0705 68 | -0.1000,-3.5600,1.0705,1.0705 69 | -0.3000,-3.7600,1.0705,1.0705 70 | -0.3000,-3.7600,1.0705,1.0705 71 | -0.5000,-3.9600,1.0705,1.0705 72 | -0.6000,-4.1600,1.0705,1.0705 73 | -0.7000,-4.2600,1.0705,1.0705 74 | -0.9000,-4.3600,1.0705,1.0705 75 | -1.0000,-4.4600,1.0705,1.0705 76 | -1.2000,-4.6600,1.0705,1.0705 77 | -1.3000,-4.7600,1.0705,1.0705 78 | -1.5000,-4.8600,1.0705,1.0705 79 | -1.6000,-4.9600,1.0705,1.0705 80 | -1.7000,-5.0600,1.0705,1.0705 81 | -1.8000,-5.1600,1.0705,1.0705 82 | -2.0000,-5.2600,1.0705,1.0705 83 | -2.2000,-5.3600,1.0705,1.0705 84 | -2.2000,-5.3600,1.0705,1.0705 85 | -2.4000,-5.3600,1.0705,1.0705 86 | -2.5000,-5.4600,1.0705,1.0705 87 | -2.7000,-5.4600,1.0705,1.0705 88 | -2.8000,-5.4600,1.0705,1.0705 89 | -3.0000,-5.3600,1.0705,1.0705 90 | -3.1000,-5.4600,1.0705,1.0705 91 | -3.3000,-5.4600,1.0705,1.0705 92 | -3.4000,-5.4600,1.0705,1.0705 93 | -3.6000,-5.3600,1.0705,1.0705 94 | -3.8000,-5.2600,1.0705,1.0705 95 | -3.9000,-5.2600,1.0705,1.0705 96 | -4.1000,-5.1600,1.0705,1.0705 97 | -4.2000,-5.1600,1.0705,1.0705 98 | -4.3000,-5.0600,1.0705,1.0705 99 | -4.4000,-5.0600,1.0705,1.0705 100 | -4.5000,-4.9600,1.0705,1.0705 101 | -4.6000,-4.8600,1.0705,1.0705 102 | -4.7000,-4.6600,1.0705,1.0705 103 | -4.8000,-4.5600,1.0705,1.0705 104 | -4.9000,-4.4600,1.0705,1.0705 105 | -5.0000,-4.3600,1.0705,1.0705 106 | -5.1000,-4.1600,1.0705,1.0705 107 | -5.1000,-3.9600,1.0705,1.0705 108 | -5.2000,-3.8600,1.0705,1.0705 109 | -5.2000,-3.6600,1.0705,1.0705 110 | -5.3000,-3.5600,1.0705,1.0705 111 | -5.3000,-3.4600,1.0705,1.0705 112 | -5.4000,-3.3600,1.0705,1.0705 113 | -5.4000,-3.1600,1.0705,1.0705 114 | -5.5000,-3.0600,1.0705,1.0705 115 | -5.4000,-2.8600,1.0705,1.0705 116 | -5.4000,-2.7600,1.0705,1.0705 117 | -5.3000,-2.5600,1.0705,1.0705 118 | -5.2000,-2.3600,1.0705,1.0705 119 | -5.2000,-2.2600,1.0705,1.0705 120 | -5.1000,-2.0600,1.0705,1.0705 121 | -5.1000,-1.9600,1.0705,1.0705 122 | -4.9000,-1.7600,1.0705,1.0705 123 | -4.9000,-1.6600,1.0705,1.0705 124 | -4.8000,-1.5600,1.0705,1.0705 125 | -4.7000,-1.4600,1.0705,1.0705 126 | -4.5000,-1.2600,1.0705,1.0705 127 | -4.5000,-1.1600,1.0705,1.0705 128 | -4.3000,-0.9600,1.0705,1.0705 129 | -4.1000,-0.7600,1.0705,1.0705 130 | -4.0000,-0.6600,1.0705,1.0705 131 | -3.8000,-0.4600,1.0705,1.0705 132 | -3.8000,-0.3600,1.0705,1.0705 133 | -3.7000,-0.1600,1.0705,1.0705 134 | -3.6000,-0.0600,1.0705,1.0705 135 | -3.4000,0.1400,1.0705,1.0705 136 | -3.3000,0.2400,1.0705,1.0705 137 | -3.2000,0.4400,1.0705,1.0705 138 | -3.1000,0.5400,1.0705,1.0705 139 | -2.9000,0.7400,1.0705,1.0705 140 | -------------------------------------------------------------------------------- /frictionmap/src/plot_frictionmap_grid.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from scipy.spatial import Voronoi, voronoi_plot_2d 4 | from scipy.spatial import cKDTree 5 | import os.path 6 | import frictionmap 7 | 8 | """ 9 | Created by: 10 | Leonhard Hermansdorfer 11 | 12 | Created on: 13 | 01.12.2018 14 | 15 | Documentation: 16 | To plot a friction map from an already existing file, adjust trackname and filenames at the bottom of this file and 17 | run this file directly. 18 | """ 19 | 20 | 21 | def plot_voronoi_fromFile(track_name: str, 22 | filename_frictionmap: str) -> None: 23 | """ 24 | Documentation 25 | This function loads the friction map file (*_tpamap.csv') and creates all variables necessary for plotting the 26 | friction map as a grid without the corresponding friction data. 27 | 28 | Input 29 | :param track_name: name of the race track 30 | :param filename_frictionmap: filename of the file containing the friction map ('*_tpamap.csv'') 31 | 32 | Output 33 | --- 34 | """ 35 | 36 | # Path Management -------------------------------------------------------------------------------------------------- 37 | 38 | path2module = os.path.dirname(os.path.abspath(__file__)).split('frictionmap')[0] 39 | path2reftrack_file = os.path.join(path2module, 'inputs', 'tracks', track_name + '.csv') 40 | filepath_frictionmap = os.path.join(path2module, 'inputs', 'frictionmaps', filename_frictionmap) 41 | 42 | # Read Files ------------------------------------------------------------------------------------------------------- 43 | 44 | # load reference line and calculate track boundaries 45 | reftrack = frictionmap.src.reftrack_functions.load_reftrack(path2track=path2reftrack_file) 46 | trackbound_right, trackbound_left = frictionmap.src.reftrack_functions.calc_trackboundaries(reftrack=reftrack) 47 | 48 | # load friction map 49 | with open(filepath_frictionmap, 'rb') as fh: 50 | map_coordinates = np.loadtxt(fh, comments='#', delimiter=';') 51 | tpamap_loaded = cKDTree(map_coordinates) 52 | 53 | # call function to plot friction map 54 | plot_voronoi_fromVariable(tree=tpamap_loaded, 55 | refline=reftrack[:, :2], 56 | trackbound_right=trackbound_right, 57 | trackbound_left=trackbound_left) 58 | 59 | 60 | def plot_voronoi_fromVariable(tree: cKDTree, 61 | refline: np.array, 62 | trackbound_right: np.array, 63 | trackbound_left: np.array,) -> None: 64 | """ 65 | Documentation 66 | This function plots the friction map as a grid without the corresponding friction data. 67 | 68 | Input 69 | :param tree: cKDTree object containing the coordinates of the friction map 70 | :param refline: array consisting of the x-,y-coordinates of the reference line 71 | :param trackbound_right: array consisting of the x-,y-coordinates of the right track boundary 72 | :param trackbound_left: array consisting of the x-,y-coordinates of the left track boundary 73 | 74 | Output 75 | --- 76 | """ 77 | 78 | print("INFO: Plotting friction map grid - 2 plots...") 79 | 80 | # plot 1 81 | tree_points = tree.data 82 | 83 | plt.figure() 84 | plt.scatter(tree_points[:, 0], tree_points[:, 1]) 85 | plt.plot(refline[:, 0], refline[:, 1], 'r') 86 | 87 | plt.axis('equal') 88 | plt.title('grid coordinates and reference line') 89 | plt.xlabel('x in meters') 90 | plt.ylabel('y in meters') 91 | 92 | plt.show() 93 | 94 | # plot 2 95 | vor = Voronoi(tree_points[:, 0:2]) 96 | 97 | voronoi_plot_2d(vor, show_vertices=False) 98 | plt.plot(refline[:, 0], refline[:, 1], 'r') 99 | plt.plot(trackbound_left[:, 0], trackbound_left[:, 1], 'b') 100 | plt.plot(trackbound_right[:, 0], trackbound_right[:, 1], 'b') 101 | 102 | plt.axis('equal') 103 | plt.xlim(np.amin(refline[:, 0]) - 100.0, np.amax(refline[:, 0]) + 100.0) 104 | plt.ylim(np.amin(refline[:, 1]) - 100.0, np.amax(refline[:, 1]) + 100.0) 105 | plt.title('grid cells, reference line and track boundaries') 106 | plt.xlabel('x in meters') 107 | plt.ylabel('y in meters') 108 | 109 | plt.show() 110 | 111 | 112 | # ---------------------------------------------------------------------------------------------------------------------- 113 | # testing -------------------------------------------------------------------------------------------------------------- 114 | # ---------------------------------------------------------------------------------------------------------------------- 115 | if __name__ == '__main__': 116 | 117 | track_name = 'modena_2019' 118 | filename_tpamap = 'modena2019_tpamap.csv' 119 | 120 | plot_voronoi_fromFile(track_name, filename_tpamap) 121 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/prep_track.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import trajectory_planning_helpers as tph 3 | import sys 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | def prep_track(reftrack_imp: np.ndarray, 8 | reg_smooth_opts: dict, 9 | stepsize_opts: dict, 10 | debug: bool = True, 11 | min_width: float = None) -> tuple: 12 | """ 13 | Created by: 14 | Alexander Heilmeier 15 | 16 | Documentation: 17 | This function prepares the inserted reference track for optimization. 18 | 19 | Inputs: 20 | reftrack_imp: imported track [x_m, y_m, w_tr_right_m, w_tr_left_m] 21 | reg_smooth_opts: parameters for the spline approximation 22 | stepsize_opts: dict containing the stepsizes before spline approximation and after spline interpolation 23 | debug: boolean showing if debug messages should be printed 24 | min_width: [m] minimum enforced track width (None to deactivate) 25 | 26 | Outputs: 27 | reftrack_interp: track after smoothing and interpolation [x_m, y_m, w_tr_right_m, w_tr_left_m] 28 | normvec_normalized_interp: normalized normal vectors on the reference line [x_m, y_m] 29 | a_interp: LES coefficients when calculating the splines 30 | coeffs_x_interp: spline coefficients of the x-component 31 | coeffs_y_interp: spline coefficients of the y-component 32 | """ 33 | 34 | # ------------------------------------------------------------------------------------------------------------------ 35 | # INTERPOLATE REFTRACK AND CALCULATE INITIAL SPLINES --------------------------------------------------------------- 36 | # ------------------------------------------------------------------------------------------------------------------ 37 | 38 | # smoothing and interpolating reference track 39 | reftrack_interp = tph.spline_approximation. \ 40 | spline_approximation(track=reftrack_imp, 41 | k_reg=reg_smooth_opts["k_reg"], 42 | s_reg=reg_smooth_opts["s_reg"], 43 | stepsize_prep=stepsize_opts["stepsize_prep"], 44 | stepsize_reg=stepsize_opts["stepsize_reg"], 45 | debug=debug) 46 | 47 | # calculate splines 48 | refpath_interp_cl = np.vstack((reftrack_interp[:, :2], reftrack_interp[0, :2])) 49 | 50 | coeffs_x_interp, coeffs_y_interp, a_interp, normvec_normalized_interp = tph.calc_splines.\ 51 | calc_splines(path=refpath_interp_cl) 52 | 53 | # ------------------------------------------------------------------------------------------------------------------ 54 | # CHECK SPLINE NORMALS FOR CROSSING POINTS ------------------------------------------------------------------------- 55 | # ------------------------------------------------------------------------------------------------------------------ 56 | 57 | normals_crossing = tph.check_normals_crossing.check_normals_crossing(track=reftrack_interp, 58 | normvec_normalized=normvec_normalized_interp, 59 | horizon=10) 60 | 61 | if normals_crossing: 62 | bound_1_tmp = reftrack_interp[:, :2] + normvec_normalized_interp * np.expand_dims(reftrack_interp[:, 2], axis=1) 63 | bound_2_tmp = reftrack_interp[:, :2] - normvec_normalized_interp * np.expand_dims(reftrack_interp[:, 3], axis=1) 64 | 65 | plt.figure() 66 | 67 | plt.plot(reftrack_interp[:, 0], reftrack_interp[:, 1], 'k-') 68 | for i in range(bound_1_tmp.shape[0]): 69 | temp = np.vstack((bound_1_tmp[i], bound_2_tmp[i])) 70 | plt.plot(temp[:, 0], temp[:, 1], "r-", linewidth=0.7) 71 | 72 | plt.grid() 73 | ax = plt.gca() 74 | ax.set_aspect("equal", "datalim") 75 | plt.xlabel("east in m") 76 | plt.ylabel("north in m") 77 | plt.title("Error: at least one pair of normals is crossed!") 78 | 79 | plt.show() 80 | 81 | raise IOError("At least two spline normals are crossed, check input or increase smoothing factor!") 82 | 83 | # ------------------------------------------------------------------------------------------------------------------ 84 | # ENFORCE MINIMUM TRACK WIDTH (INFLATE TIGHTER SECTIONS UNTIL REACHED) --------------------------------------------- 85 | # ------------------------------------------------------------------------------------------------------------------ 86 | 87 | manipulated_track_width = False 88 | 89 | if min_width is not None: 90 | for i in range(reftrack_interp.shape[0]): 91 | cur_width = reftrack_interp[i, 2] + reftrack_interp[i, 3] 92 | 93 | if cur_width < min_width: 94 | manipulated_track_width = True 95 | 96 | # inflate to both sides equally 97 | reftrack_interp[i, 2] += (min_width - cur_width) / 2 98 | reftrack_interp[i, 3] += (min_width - cur_width) / 2 99 | 100 | if manipulated_track_width: 101 | print("WARNING: Track region was smaller than requested minimum track width -> Applied artificial inflation in" 102 | " order to match the requirements!", file=sys.stderr) 103 | 104 | return reftrack_interp, normvec_normalized_interp, a_interp, coeffs_x_interp, coeffs_y_interp 105 | 106 | 107 | # testing -------------------------------------------------------------------------------------------------------------- 108 | if __name__ == "__main__": 109 | pass 110 | -------------------------------------------------------------------------------- /frictionmap/src/reftrack_functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import matplotlib.pyplot as plt 4 | 5 | """ 6 | Created by: 7 | Leonhard Hermansdorfer 8 | 9 | Created on: 10 | 20.12.2018 11 | """ 12 | 13 | 14 | def load_reftrack(path2track: str) -> np.array: 15 | """ 16 | Documentation 17 | This function loads the track file. 18 | 19 | Input 20 | :param path2track: absolute path to reference track file 21 | 22 | Output 23 | :return reftrack reference track containing x-,y-coordinates and trackwidths to the right and left of 24 | the reference line [x_m, y_m, trackwidth_right_m, trackwidth_left_m] 25 | """ 26 | 27 | with open(path2track, 'r') as fh: 28 | reftrack = np.genfromtxt(fh, delimiter=',') 29 | 30 | return reftrack 31 | 32 | 33 | def check_isclosed_refline(refline: np.ndarray) -> bool: 34 | """ 35 | Documentation 36 | This function checks whether the given reference line is a closed or open circuit. 37 | 38 | Input 39 | :param refline: reference line [x_m, y_m] 40 | 41 | Output 42 | :return bool_isclosed_refline boolean indicating whether the track is closed / a circuit (=True) or not (=False) 43 | """ 44 | 45 | # user input 46 | max_dist_isclosed = 10.0 # [m] 47 | 48 | # determine if reference track is expected as closed loop 49 | dist_last2first = math.sqrt((refline[-1, 0] - refline[0, 0]) ** 2 + (refline[-1, 1] - refline[0, 1]) ** 2) 50 | 51 | # if track is closed, add first row of reference line at the bottom. Otherwise, extrapolate last coordinate and add 52 | if dist_last2first <= max_dist_isclosed: 53 | bool_isclosed_refline = True 54 | 55 | else: 56 | bool_isclosed_refline = False 57 | 58 | return bool_isclosed_refline 59 | 60 | 61 | def calc_trackboundaries(reftrack: np.ndarray) -> tuple: 62 | """ 63 | Documentation 64 | This function calculates the actual coordinates of both track boundaries specified by the reference line and the 65 | corresponding trackwidths. 66 | 67 | Input 68 | :param reftrack: reference track [x_m, y_m, trackwidth_right_m, trackwidth_left_m] 69 | 70 | Output 71 | :return track_boundary_right x-,y-coordinates of right trackboundary (from reference line in driving direction) 72 | :return track_boundary_left x-,y-coordinates of left trackboundary (from reference line in driving direction) 73 | """ 74 | 75 | refline_normvecs = calc_refline_normvecs(refline=reftrack[:, :2]) 76 | track_boundary_right = reftrack[:, :2] + refline_normvecs[:, :2] * np.expand_dims(reftrack[:, 2], axis=1) 77 | track_boundary_left = reftrack[:, :2] - refline_normvecs[:, :2] * np.expand_dims(reftrack[:, 3], axis=1) 78 | 79 | return track_boundary_right, track_boundary_left 80 | 81 | 82 | def calc_refline_normvecs(refline: np.ndarray) -> np.array: 83 | """ 84 | Documentation 85 | This function calculates the normal vectors of the reference line at each coordinate (pointing towards the right in 86 | the direction of driving). 87 | 88 | Input 89 | :param refline: reference line [x_m, y_m] 90 | 91 | Output 92 | :return refline_normvecs reference line normal vectors [x_m, y_m] 93 | """ 94 | 95 | bool_isclosed_refline = check_isclosed_refline(refline=refline) 96 | 97 | if bool_isclosed_refline: 98 | refline = np.vstack((refline[-1], refline, refline[0])) 99 | 100 | refline_grad = np.gradient(refline[:, :3], axis=0) 101 | 102 | # z-vector for calculating cross product to get normal vector 103 | z = np.array([0.0, 0.0, 1.0]) 104 | 105 | refline_crossproduct = np.cross(refline_grad, z) 106 | 107 | norm_factors = np.divide(1.0, np.linalg.norm(refline_crossproduct, axis=1)) 108 | 109 | refline_normvecs = refline_crossproduct * norm_factors[:, None] 110 | 111 | if bool_isclosed_refline: 112 | refline_normvecs = np.delete(refline_normvecs, 0, axis=0) 113 | refline_normvecs = np.delete(refline_normvecs, -1, axis=0) 114 | 115 | return refline_normvecs 116 | 117 | 118 | def plot_refline(reftrack: np.ndarray) -> None: 119 | """ 120 | Documentation 121 | This function plots the reference line and its normal vectors at each coordinate. 122 | 123 | Input 124 | :param reftrack: reference track [x_m, y_m, trackwidth_right_m, trackwidth_left_m] 125 | 126 | Output 127 | --- 128 | """ 129 | 130 | # get normal vectors 131 | refline_normvecs = calc_refline_normvecs(refline=reftrack[:, :2]) 132 | 133 | # calculate track boundaries 134 | plt.figure() 135 | plt.plot(reftrack[:, 0], reftrack[:, 1]) 136 | 137 | for row in range(0, refline_normvecs.shape[0]): 138 | plt.plot([reftrack[row, 0], 139 | reftrack[row, 0] + (-refline_normvecs[row, 0] * reftrack[row, 3])], 140 | [reftrack[row, 1], 141 | reftrack[row, 1] + (-refline_normvecs[row, 1] * reftrack[row, 3])], 'g') 142 | 143 | plt.plot([reftrack[row, 0], 144 | reftrack[row, 0] + (refline_normvecs[row, 0] * reftrack[row, 2])], 145 | [reftrack[row, 1], 146 | reftrack[row, 1] + (refline_normvecs[row, 1] * reftrack[row, 2])], 'r') 147 | 148 | plt.grid() 149 | plt.title('Reference line and normal vectors') 150 | plt.xlabel('x in meters') 151 | plt.ylabel('y in meters') 152 | plt.axis('equal') 153 | 154 | plt.show() 155 | 156 | 157 | # ---------------------------------------------------------------------------------------------------------------------- 158 | # testing -------------------------------------------------------------------------------------------------------------- 159 | # ---------------------------------------------------------------------------------------------------------------------- 160 | if __name__ == '__main__': 161 | pass 162 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/friction_map_plot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import json 4 | import matplotlib.pyplot as plt 5 | from scipy.spatial import cKDTree 6 | 7 | 8 | def friction_map_plot(filepath_tpamap: str, 9 | filepath_tpadata: str, 10 | filepath_referenceline: str): 11 | """ 12 | Created by: 13 | Leonhard Hermansdorfer 14 | 15 | Documentation: 16 | Function to visualize the friction map data and the reference line for a given race track. 17 | 18 | The friction map is located in "/inputs/frictionmaps/TRACKNAME_tpamap.csv" 19 | The fricton map data is located in "/inputs/frictionmaps/TRACKNAME_tpadata.json" 20 | The reference line is located in "/inputs/tracks/TRACKNAME.csv" 21 | 22 | Inputs: 23 | filepath_tpamap: path to friction map representing the race track of interest (*_tpamap.csv) 24 | filepath_tpadata: path to corresponding friction data of the above specified map (*_tpadata.json) 25 | filepath_referenceline: path to corresponding reference line of the above specified friction map 26 | """ 27 | 28 | # ------------------------------------------------------------------------------------------------------------------ 29 | # LOAD DATA FROM FILES --------------------------------------------------------------------------------------------- 30 | # ------------------------------------------------------------------------------------------------------------------ 31 | 32 | print('INFO: Loading friction map data...') 33 | 34 | # load reference line from csv-file 35 | referenceline = np.loadtxt(filepath_referenceline, comments='#', delimiter=';') 36 | 37 | # load friction map (tpamap) from csv-file 38 | map_coordinates = np.loadtxt(filepath_tpamap, comments='#', delimiter=';') 39 | tpamap_loaded = cKDTree(map_coordinates) 40 | 41 | # load friction data corresponding to the chosen friction map 42 | with open(filepath_tpadata, 'r') as fh: 43 | tpadata_dict_string = json.load(fh) 44 | tpadata_loaded = {int(k): np.asarray(v) for k, v in tpadata_dict_string.items()} 45 | 46 | print('INFO: Friction map data loaded successfully!') 47 | 48 | # ------------------------------------------------------------------------------------------------------------------ 49 | # PREPARE DATA FOR PLOTTING ---------------------------------------------------------------------------------------- 50 | # ------------------------------------------------------------------------------------------------------------------ 51 | 52 | print('INFO: Preprocessing friction map data for visualization... (takes some time)') 53 | 54 | # read values from dict 55 | list_coord = tpamap_loaded.data[tpamap_loaded.indices] 56 | 57 | list_mue = [] 58 | 59 | for idx in tpamap_loaded.indices: 60 | list_mue.append(tpadata_loaded[idx]) 61 | 62 | # recalculate stepsize of friction map 63 | x_stepsize = abs(list_coord[1, 0] - list_coord[0, 0]) 64 | y_stepsize = abs(list_coord[1, 1] - list_coord[0, 1]) 65 | 66 | # load coordinate values from friction map 67 | tree_points = tpamap_loaded.data 68 | 69 | # determine min/max of coordinate values in both directions to set up 2d array for countourf plotting 70 | x_min = math.floor(np.amin(tree_points[:, 0])) 71 | x_max = math.ceil(np.amax(tree_points[:, 0])) 72 | 73 | y_min = math.floor(np.amin(tree_points[:, 1])) 74 | y_max = math.ceil(np.amax(tree_points[:, 1])) 75 | 76 | x_vals = np.arange(x_min - (20.0 * x_stepsize), x_max + (19.0 * x_stepsize), x_stepsize) 77 | y_vals = np.arange(y_min - (20.0 * y_stepsize), y_max + (19.0 * y_stepsize), y_stepsize) 78 | 79 | # set up an empty 2d array which is then filled wiich corresponding mue values 80 | z = np.full((y_vals.shape[0], x_vals.shape[0]), fill_value=np.nan) 81 | 82 | # plot 2D array 83 | for row, mue in zip(list_coord, list_mue): 84 | index_column = int((row[0] - min(x_vals)) / x_stepsize) 85 | index_row = int((-row[1] + max(y_vals)) / y_stepsize) 86 | 87 | z[index_row, index_column] = mue 88 | 89 | # ------------------------------------------------------------------------------------------------------------------ 90 | # CREATE PLOT ------------------------------------------------------------------------------------------------------ 91 | # ------------------------------------------------------------------------------------------------------------------ 92 | 93 | print('INFO: Plotting friction map data...') 94 | 95 | plt.figure() 96 | 97 | # plot reference line 98 | plt.plot(referenceline[:, 0], referenceline[:, 1], 'r') 99 | 100 | # set axis limits 101 | plt.xlim(min(referenceline[:, 0]) - 100.0, max(referenceline[:, 0]) + 100.0) 102 | plt.ylim(min(referenceline[:, 1]) - 100.0, max(referenceline[:, 1]) + 100.0) 103 | 104 | # plot 2D contour representing the friction data (mue-values) 105 | plt.contourf(x_vals, np.flipud(y_vals), z) 106 | 107 | # set up plot 108 | plt.colorbar(label='mue-values') 109 | plt.title('mue-values of the racetrack') 110 | plt.xlabel("east in m") 111 | plt.ylabel("north in m") 112 | plt.axis('equal') 113 | 114 | plt.show() 115 | 116 | 117 | # testing -------------------------------------------------------------------------------------------------------------- 118 | if __name__ == '__main__': 119 | import os.path 120 | 121 | module_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 122 | tpamap_path = os.path.join(module_path, 'inputs', 'frictionmaps', 'berlin_2018_tpamap.csv') 123 | tpadata_path = os.path.join(module_path, 'inputs', 'frictionmaps', 'berlin_2018_varmue08-12_tpadata.json') 124 | referenceline_path = os.path.join(module_path, 'inputs', 'tracks', 'berlin_2018.csv') 125 | 126 | friction_map_plot(filepath_tpamap=tpamap_path, 127 | filepath_tpadata=tpadata_path, 128 | filepath_referenceline=referenceline_path) 129 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/export_mintime_solution.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | 5 | def export_mintime_solution(file_path: str, 6 | pars: dict, 7 | s: np.ndarray, 8 | t: np.ndarray, 9 | x: np.ndarray, 10 | u: np.ndarray, 11 | tf: np.ndarray, 12 | ax: np.ndarray, 13 | ay: np.ndarray, 14 | atot: np.ndarray, 15 | w0: np.ndarray, 16 | lam_x0: np.ndarray, 17 | lam_g0: np.ndarray, 18 | pwr: dict = None) -> None: 19 | 20 | """ 21 | Created by: 22 | Fabian Christ 23 | 24 | Modified by: 25 | Thomas Herrmann (thomas.herrmann@tum.de) 26 | 27 | Documentation: 28 | This function is used to export the solution of the time-optimal trajectory planning into several csv files. 29 | 30 | Inputs: 31 | file_path: path for the output files 32 | t: solution for the time along the reference line (at corresponding station s) 33 | s: station along the reference line (at corresponding time t) 34 | x: solution for the state variables (at corresponding time t / station s) 35 | u: solution for the control variables (at corresponding time t / station s) 36 | tf: solution for the tire forces (at corresponding time t / station s) 37 | ax: solution for the longitudinal acceleration (at corresponding time t / station s) 38 | ay: solution for the lateral acceleration (at corresponding time t / station s) 39 | atot: solution for the total acceleration (at corresponding time t / station s) 40 | w0: solution for all decision variables (for warm starting the nonlinear program) 41 | lam_x0: solution for the lagrange multipliers (for warm starting the nonlinear program) 42 | lam_g0: solution for the lagrange multipliers (for warm starting the nonlinear program) 43 | """ 44 | 45 | # save state variables 46 | if pars["pwr_params_mintime"]["pwr_behavior"]: 47 | header_x = ("s_m; t_s; v_mps; beta_rad; omega_z_radps; n_m; xi_rad; " 48 | "machine.temp_mot_dC; batt.temp_batt_dC; inverter.temp_inv_dC; " 49 | "radiators.temp_cool_mi_dC; radiators.temp_cool_b_dC; batt.soc_batt") 50 | fmt_x = "%.1f; %.3f; %.2f; %.5f; %.5f; %.5f; %.5f; %.2f; %.2f; %.2f; %.2f; %.2f; %.5f;" 51 | states = np.column_stack((s, t, x)) 52 | np.savetxt(os.path.join(file_path, 'states.csv'), states, fmt=fmt_x, header=header_x) 53 | else: 54 | header_x = "s_m; t_s; v_mps; beta_rad; omega_z_radps; n_m; xi_rad" 55 | fmt_x = "%.1f; %.3f; %.2f; %.5f; %.5f; %.5f; %.5f" 56 | states = np.column_stack((s, t, x)) 57 | np.savetxt(os.path.join(file_path, 'states.csv'), states, fmt=fmt_x, header=header_x) 58 | 59 | # save control variables 60 | header_u = "s_m; t_s; delta_rad; f_drive_N; f_brake_N; gamma_y_N" 61 | fmt_u = "%.1f; %.3f; %.5f; %.1f; %.1f; %.1f" 62 | controls = np.column_stack((s[:-1], t[:-1], u)) 63 | np.savetxt(os.path.join(file_path, 'controls.csv'), controls, fmt=fmt_u, header=header_u) 64 | 65 | # save tire forces 66 | header_tf = "s_m; t_s; f_x_fl_N; f_y_fl_N; f_z_fl_N; f_x_fr_N; f_y_fr_N; f_z_fr_N;" \ 67 | "f_x_rl_N; f_y_rl_N; f_z_rl_N; f_x_rr_N;f_y_rr_N; f_z_rr_N;" 68 | fmt_tf = "%.1f; %.3f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f; %.1f" 69 | tire_forces = np.column_stack((s, t, tf)) 70 | np.savetxt(os.path.join(file_path, 'tire_forces.csv'), tire_forces, fmt=fmt_tf, header=header_tf) 71 | 72 | # save accelerations 73 | header_a = "s_m; t_s; ax_mps2; ay_mps2; atot_mps2" 74 | fmt_a = "%.1f; %.3f; %.3f; %.3f; %.3f" 75 | accelerations = np.column_stack((s, t, ax, ay, atot)) 76 | np.savetxt(os.path.join(file_path, 'accelerations.csv'), accelerations, fmt=fmt_a, header=header_a) 77 | 78 | # save power losses 79 | if pars["pwr_params_mintime"]["pwr_behavior"]: 80 | if pars["pwr_params_mintime"]["simple_loss"]: 81 | header_pwr_l = \ 82 | ("s_m; t_s; " 83 | "P_loss_1machine_kW; " 84 | "P_loss_1inverter_kW; " 85 | "P_loss_batt_kW; P_out_batt_kW") 86 | fmt_pwr_l = ("%.1f; %.3f; " 87 | "%.2f; " 88 | "%.2f; " 89 | "%.2f; %.2f") 90 | pwr_losses = \ 91 | np.column_stack((s[:-1], t[:-1], 92 | pwr["machine"].p_loss_total, 93 | pwr["inverter"].p_loss_total, 94 | pwr["batt"].p_loss_total, pwr["batt"].p_out_batt)) 95 | else: 96 | header_pwr_l = \ 97 | ("s_m; t_s; " 98 | "P_loss_1machine_kW; P_loss_copper_1machine_kW; " 99 | "P_loss_statorIron_1machine_kW; P_loss_rotor_1machine_kW; " 100 | "P_loss_1inverter_kW; P_loss_switch_1inverter_kW; P_loss_cond_1inverter; " 101 | "P_loss_batt_kW; P_out_batt_kW") 102 | fmt_pwr_l = ("%.1f; %.3f; " 103 | "%.2f; %.2f; " 104 | "%.2f; %.2f; " 105 | "%.2f; %.2f; %.2f; " 106 | "%.2f; %.2f") 107 | pwr_losses = \ 108 | np.column_stack((s[:-1], t[:-1], 109 | pwr["machine"].p_loss_total, pwr["machine"].p_loss_copper, 110 | pwr["machine"].p_loss_stator_iron, pwr["machine"].p_loss_rotor, 111 | pwr["inverter"].p_loss_total, pwr["inverter"].p_loss_switch, pwr["inverter"].p_loss_cond, 112 | pwr["batt"].p_loss_total, pwr["batt"].p_out_batt)) 113 | 114 | np.savetxt(os.path.join(file_path, 'power_losses.csv'), pwr_losses, fmt=fmt_pwr_l, header=header_pwr_l) 115 | 116 | # save solution of decision variables and lagrange multipliers 117 | np.savetxt(os.path.join(file_path, 'w0.csv'), w0, delimiter=';') 118 | np.savetxt(os.path.join(file_path, 'lam_x0.csv'), lam_x0, delimiter=';') 119 | np.savetxt(os.path.join(file_path, 'lam_g0.csv'), lam_g0, delimiter=';') 120 | 121 | 122 | # testing -------------------------------------------------------------------------------------------------------------- 123 | if __name__ == "__main__": 124 | pass 125 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/extract_friction_coeffs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import math 4 | import trajectory_planning_helpers as tph 5 | import opt_mintime_traj 6 | 7 | 8 | def extract_friction_coeffs(reftrack: np.ndarray, 9 | normvectors: np.ndarray, 10 | tpamap_path: str, 11 | tpadata_path: str, 12 | pars: dict, 13 | dn: float, 14 | print_debug: bool, 15 | plot_debug: bool) -> tuple: 16 | """ 17 | Created by: 18 | Fabian Christ 19 | 20 | Documentation: 21 | Extracting friction coefficients on a fine grid on the normal vectors along the racetrack from the provided 22 | friction map. 23 | 24 | Inputs: 25 | reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m] 26 | normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component] 27 | tpamap_path: file path to tpa map (required for friction map loading) 28 | tpadata_path: file path to tpa data (required for friction map loading) 29 | pars: parameters dictionary 30 | dn: distance of equidistant points on normal vectors for extracting the friction coefficients 31 | print_debug: determines if debug prints are shown 32 | plot_debug: determines if debug plots are shown 33 | 34 | Outputs: 35 | n: lateral distance of equidistant points on normal vectors along the racetrack 36 | mue_fl: grid of friction coefficients along the racetrack (left front wheel) 37 | mue_fr: grid of friction coefficients along the racetrack (right front wheel) 38 | mue_rl: grid of friction coefficients along the racetrack (left rear wheel) 39 | mue_rr: grid of friction coefficients along the racetrack (right rear wheel) 40 | """ 41 | 42 | # ------------------------------------------------------------------------------------------------------------------ 43 | # PREPARATION ------------------------------------------------------------------------------------------------------ 44 | # ------------------------------------------------------------------------------------------------------------------ 45 | 46 | # track data 47 | reftrack_cl = np.vstack([reftrack, reftrack[0]]) 48 | refline_cl = reftrack_cl[:, :2] 49 | track_width_right_cl = reftrack_cl[:, 2] 50 | track_width_left_cl = reftrack_cl[:, 3] 51 | normvectors_cl = np.vstack([normvectors, normvectors[0]]) 52 | tang_vec = np.asarray([-normvectors_cl[:, 1], normvectors_cl[:, 0]]).T 53 | 54 | # number of steps along the reference line 55 | num_steps = len(reftrack_cl[:, 0]) 56 | 57 | # vehicle data 58 | width = pars["optim_opts"]["width_opt"] 59 | wb_f = pars["vehicle_params_mintime"]["wheelbase_front"] 60 | wb_r = pars["vehicle_params_mintime"]["wheelbase_rear"] 61 | 62 | # initialize map interface 63 | map_interface = opt_mintime_traj.src.friction_map_interface.FrictionMapInterface(tpamap_path=tpamap_path, 64 | tpadata_path=tpadata_path) 65 | 66 | # initialize solution 67 | n = [] 68 | mue_fl = [] 69 | mue_fr = [] 70 | mue_rl = [] 71 | mue_rr = [] 72 | 73 | # plot position of extracted friction coefficients 74 | if plot_debug: 75 | plt.figure(0) 76 | 77 | # ------------------------------------------------------------------------------------------------------------------ 78 | # CALCULATION ------------------------------------------------------------------------------------------------------ 79 | # ------------------------------------------------------------------------------------------------------------------ 80 | 81 | for i in range(num_steps): 82 | # number of required points on the normal vector to the left and right side 83 | num_right = math.floor((track_width_right_cl[i] - 0.5 * width - 0.5) / dn) 84 | num_left = math.floor((track_width_left_cl[i] - 0.5 * width - 0.5) / dn) 85 | 86 | # vector of lateral distances on the normal vector for the vehicle's center of gravity 87 | n_pos = np.linspace(-dn * num_right, dn * num_left, num_right + num_left + 1) 88 | n.append(n_pos) 89 | 90 | # initialize xy coordinates for each point on the normal and each tire 91 | xy = np.zeros((8, num_right + num_left + 1)) 92 | 93 | # calculate xy coordinates for each point on the normal and each tire 94 | for j in range(num_right + num_left + 1): 95 | xy[0:2, j] = (refline_cl[i, :] + wb_f * tang_vec[i, :]) - (n_pos[j] + 0.5 * width) * normvectors_cl[i, :] 96 | xy[2:4, j] = (refline_cl[i, :] + wb_f * tang_vec[i, :]) - (n_pos[j] - 0.5 * width) * normvectors_cl[i, :] 97 | xy[4:6, j] = (refline_cl[i, :] - wb_r * tang_vec[i, :]) - (n_pos[j] + 0.5 * width) * normvectors_cl[i, :] 98 | xy[6:8, j] = (refline_cl[i, :] - wb_r * tang_vec[i, :]) - (n_pos[j] - 0.5 * width) * normvectors_cl[i, :] 99 | 100 | # get friction coefficients for these coordinates 101 | mue_fl.append(map_interface.get_friction_singlepos(xy[0:2, :].T)) 102 | mue_fr.append(map_interface.get_friction_singlepos(xy[2:4, :].T)) 103 | mue_rl.append(map_interface.get_friction_singlepos(xy[4:6, :].T)) 104 | mue_rr.append(map_interface.get_friction_singlepos(xy[6:8, :].T)) 105 | 106 | if print_debug: 107 | tph.progressbar.progressbar(i, num_steps, 'Extraction of Friction Coefficients from Friction Map') 108 | 109 | if plot_debug: 110 | plt.plot(xy[0, :], xy[1, :], '.') 111 | plt.plot(xy[2, :], xy[3, :], '.') 112 | plt.plot(xy[4, :], xy[5, :], '.') 113 | plt.plot(xy[6, :], xy[7, :], '.') 114 | 115 | if plot_debug: 116 | bound_r = reftrack[:, :2] + normvectors * np.expand_dims(reftrack[:, 2], 1) 117 | bound_l = reftrack[:, :2] - normvectors * np.expand_dims(reftrack[:, 3], 1) 118 | plt.plot(reftrack[:, 0], reftrack[:, 1], color='grey') 119 | plt.plot(bound_r[:, 0], bound_r[:, 1], color='black') 120 | plt.plot(bound_l[:, 0], bound_l[:, 1], color='black') 121 | plt.title('Extraction of Friction Coefficients from Friction Map') 122 | plt.grid() 123 | plt.axis('equal') 124 | plt.show() 125 | 126 | return n, mue_fl, mue_fr, mue_rl, mue_rr 127 | 128 | 129 | # testing -------------------------------------------------------------------------------------------------------------- 130 | if __name__ == "__main__": 131 | pass 132 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/result_plots.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import trajectory_planning_helpers 5 | 6 | 7 | def result_plots(plot_opts: dict, 8 | width_veh_opt: float, 9 | width_veh_real: float, 10 | refline: np.ndarray, 11 | bound1_imp: np.ndarray, 12 | bound2_imp: np.ndarray, 13 | bound1_interp: np.ndarray, 14 | bound2_interp: np.ndarray, 15 | trajectory: np.ndarray) -> None: 16 | """ 17 | Created by: 18 | Alexander Heilmeier 19 | 20 | Documentation: 21 | This function plots several figures containing relevant trajectory information after trajectory optimization. 22 | 23 | Inputs: 24 | plot_opts: dict containing the information which figures to plot 25 | width_veh_opt: vehicle width used during optimization in m 26 | width_veh_real: real vehicle width in m 27 | refline: contains the reference line coordinates [x_m, y_m] 28 | bound1_imp: first track boundary (as imported) (mostly right) [x_m, y_m] 29 | bound2_imp: second track boundary (as imported) (mostly left) [x_m, y_m] 30 | bound1_interp: first track boundary (interpolated) (mostly right) [x_m, y_m] 31 | bound2_interp: second track boundary (interpolated) (mostly left) [x_m, y_m] 32 | trajectory: trajectory data [s_m, x_m, y_m, psi_rad, kappa_radpm, vx_mps, ax_mps2] 33 | """ 34 | 35 | if plot_opts["raceline"]: 36 | # calculate vehicle boundary points (including safety margin in vehicle width) 37 | normvec_normalized_opt = trajectory_planning_helpers.calc_normal_vectors.\ 38 | calc_normal_vectors(trajectory[:, 3]) 39 | 40 | veh_bound1_virt = trajectory[:, 1:3] + normvec_normalized_opt * width_veh_opt / 2 41 | veh_bound2_virt = trajectory[:, 1:3] - normvec_normalized_opt * width_veh_opt / 2 42 | 43 | veh_bound1_real = trajectory[:, 1:3] + normvec_normalized_opt * width_veh_real / 2 44 | veh_bound2_real = trajectory[:, 1:3] - normvec_normalized_opt * width_veh_real / 2 45 | 46 | point1_arrow = refline[0] 47 | point2_arrow = refline[3] 48 | vec_arrow = point2_arrow - point1_arrow 49 | 50 | # plot track including optimized path 51 | plt.figure() 52 | plt.plot(refline[:, 0], refline[:, 1], "k--", linewidth=0.7) 53 | plt.plot(veh_bound1_virt[:, 0], veh_bound1_virt[:, 1], "b", linewidth=0.5) 54 | plt.plot(veh_bound2_virt[:, 0], veh_bound2_virt[:, 1], "b", linewidth=0.5) 55 | plt.plot(veh_bound1_real[:, 0], veh_bound1_real[:, 1], "c", linewidth=0.5) 56 | plt.plot(veh_bound2_real[:, 0], veh_bound2_real[:, 1], "c", linewidth=0.5) 57 | plt.plot(bound1_interp[:, 0], bound1_interp[:, 1], "k-", linewidth=0.7) 58 | plt.plot(bound2_interp[:, 0], bound2_interp[:, 1], "k-", linewidth=0.7) 59 | plt.plot(trajectory[:, 1], trajectory[:, 2], "r-", linewidth=0.7) 60 | 61 | if plot_opts["imported_bounds"] and bound1_imp is not None and bound2_imp is not None: 62 | plt.plot(bound1_imp[:, 0], bound1_imp[:, 1], "y-", linewidth=0.7) 63 | plt.plot(bound2_imp[:, 0], bound2_imp[:, 1], "y-", linewidth=0.7) 64 | 65 | plt.grid() 66 | ax = plt.gca() 67 | # Set Arrow Size 68 | ax.arrow(point1_arrow[0], point1_arrow[1], vec_arrow[0], vec_arrow[1], 69 | head_width=2.0, head_length=2.0, fc='g', ec='g') 70 | ax.set_aspect("equal", "datalim") 71 | plt.xlabel("east in m") 72 | plt.ylabel("north in m") 73 | plt.show() 74 | 75 | if plot_opts["raceline_curv"]: 76 | # plot curvature profile 77 | plt.figure() 78 | plt.plot(trajectory[:, 0], trajectory[:, 4]) 79 | plt.grid() 80 | plt.xlabel("distance in m") 81 | plt.ylabel("curvature in rad/m") 82 | plt.show() 83 | 84 | if plot_opts["racetraj_vel_3d"]: 85 | scale_x = 1.0 86 | scale_y = 1.0 87 | scale_z = 0.3 # scale z axis such that it does not appear stretched 88 | 89 | # create 3d plot 90 | fig = plt.figure() 91 | ax = fig.gca(projection='3d') 92 | 93 | # recast get_proj function to use scaling factors for the axes 94 | ax.get_proj = lambda: np.dot(Axes3D.get_proj(ax), np.diag([scale_x, scale_y, scale_z, 1.0])) 95 | 96 | # plot raceline and boundaries 97 | ax.plot(refline[:, 0], refline[:, 1], "k--", linewidth=0.7) 98 | ax.plot(bound1_interp[:, 0], bound1_interp[:, 1], 0.0, "k-", linewidth=0.7) 99 | ax.plot(bound2_interp[:, 0], bound2_interp[:, 1], 0.0, "k-", linewidth=0.7) 100 | ax.plot(trajectory[:, 1], trajectory[:, 2], "r-", linewidth=0.7) 101 | 102 | ax.grid() 103 | ax.set_aspect("auto") 104 | ax.set_xlabel("east in m") 105 | ax.set_ylabel("north in m") 106 | 107 | # plot velocity profile in 3D 108 | ax.plot(trajectory[:, 1], trajectory[:, 2], trajectory[:, 5], color="k") 109 | ax.set_zlabel("velocity in m/s") 110 | 111 | # plot vertical lines visualizing acceleration and deceleration zones 112 | ind_stepsize = int(np.round(plot_opts["racetraj_vel_3d_stepsize"] / trajectory[1, 0] - trajectory[0, 0])) 113 | if ind_stepsize < 1: 114 | ind_stepsize = 1 115 | 116 | cur_ind = 0 117 | no_points_traj_vdc = np.shape(trajectory)[0] 118 | 119 | while cur_ind < no_points_traj_vdc - 1: 120 | x_tmp = [trajectory[cur_ind, 1], trajectory[cur_ind, 1]] 121 | y_tmp = [trajectory[cur_ind, 2], trajectory[cur_ind, 2]] 122 | z_tmp = [0.0, trajectory[cur_ind, 5]] # plot line with height depending on velocity 123 | 124 | # get proper color for line depending on acceleration 125 | if trajectory[cur_ind, 6] > 0.0: 126 | col = "g" 127 | elif trajectory[cur_ind, 6] < 0.0: 128 | col = "r" 129 | else: 130 | col = "gray" 131 | 132 | # plot line 133 | ax.plot(x_tmp, y_tmp, z_tmp, color=col) 134 | 135 | # increment index 136 | cur_ind += ind_stepsize 137 | 138 | plt.show() 139 | 140 | if plot_opts["spline_normals"]: 141 | plt.figure() 142 | 143 | plt.plot(refline[:, 0], refline[:, 1], 'k-') 144 | for i in range(bound1_interp.shape[0]): 145 | temp = np.vstack((bound1_interp[i], bound2_interp[i])) 146 | plt.plot(temp[:, 0], temp[:, 1], "r-", linewidth=0.7) 147 | 148 | plt.grid() 149 | ax = plt.gca() 150 | ax.set_aspect("equal", "datalim") 151 | plt.xlabel("east in m") 152 | plt.ylabel("north in m") 153 | 154 | plt.show() 155 | 156 | 157 | # testing -------------------------------------------------------------------------------------------------------------- 158 | if __name__ == "__main__": 159 | pass 160 | -------------------------------------------------------------------------------- /opt_mintime_traj/Readme.md: -------------------------------------------------------------------------------- 1 | # Time-Optimal Trajectory Planning 2 | This python module shows the planning of time-optimal trajectories, which allows an autonomous race car to drive at 3 | the handling limits, taking into account locally changing road friction values. 4 | 5 | For this purpose, the minimum lap time problem is described as an optimal control problem, 6 | converted to a nonlinear program using direct orthogonal Gauss-Legendre collocation and then solved by the 7 | interior-point method IPOPT. Reduced computing times are achieved using a curvilinear abscissa approach for track 8 | description, algorithmic differentiation using the software framework CasADi, and a smoothing of the track input data 9 | by approximate spline regression. The vehicles behavior is approximated as a double track model with quasi-steady state 10 | tire load simplification and nonlinear tire model. 11 | 12 | The novelty of this work is the consideration of wheel-specific tire-road friction coefficients along the racetrack 13 | using a track friction map. It is shown that variable friction coefficients have a significant impact on the 14 | trajectory, and therefore significantly improve lap times on inhomogenous racetracks. 15 | 16 | ![Raceline for the Berlin FE track considering variable friction values](var_friction_berlin.png) 17 | 18 | # File and Folder Structure 19 | * `/powertrain_src`: This folder contains the powertrain models. 20 | * `approx_friction_map.py`: This function calculates an approximation of the provided friction map in a format 21 | which can be taken into accout in the formulation of time-optimal trajectory planning. 22 | * `export_mintime_solution.py`: This function exports the solution of the optimization to csvs. 23 | * `extract_friction_coeffs.py`: This function extracts a grid of friction coefficients from the friction map along the 24 | racetrack. 25 | * `friction_map_interface.py`: This class provides an interface to the friction map. 26 | * `friction_map_plot.py`: This function plots a specified friction map. 27 | * `opt_mintime.py`: This function formulates and solves the time-optimal trajectory optimization problem. 28 | * `result_plots_mintime.py`: This function visualizes the solution of the time-optimal trajectory optimization. 29 | 30 | # Optimization Options 31 | The following optimization options can be specified in the user input section of `main_globaltraj.py` in the dict 32 | `mintime_opts`: 33 | * If you set the parameter `use_warm_start = True`, the initial guess of the nonlinear program can be set with the 34 | solution of a preview optimization. This can decrease the calculation time significantly. The required files 35 | `w0.csv`, `lam_x0.csv` and `lam_g0.csv` are saved after each optimization in the outputs folder (`outputs/mintime/`). 36 | * A constant friction coefficient `mue` can be used along the racetrack (`var_friction = None`). Alternatively 37 | variable friction coefficients from a friction value map (located in `inputs/frictionmaps`) can be used. The 38 | course of these variable friction coefficients along the racetrack can be described for each wheel by linear 39 | functions (`var_friction = "linear"`) or by linear combinations of gaussian basis functions (`var_friction = "gauss"`). 40 | See below for additional parameters. 41 | 42 | The following optimization options can be specified in the parameter file in `/params` in the dict `optim_opts_mintime`: 43 | * The car width for optimization is set through `width_opt` and includes the vehicle width and a safety margin. 44 | * Penalty terms (`penalty_delta` & `penalty_F`) can be specified in order to increase the smoothness of the control 45 | trajectories. Too big values lead to a distortion of the original minimum lap time problem. 46 | * Depending on the setting of `var_friction` as described above the friction coefficient handling differs. A constant 47 | friction coefficient `mue` can be used along the racetrack (`var_friction = null`). Alternatively 48 | variable friction coefficients from a friction value map (located in `inputs/frictionmaps`) can be used. Therefore 49 | friction values are extracted on the normal vectors of the reference line with an equidistant distance (`dn`). The 50 | course of these variable friction coefficients along the racetrack can then be described for each wheel by linear 51 | functions (`var_friction = "linear"`) or by linear combinations of gaussian basis functions (`var_friction = "gauss"`). 52 | For this purpose the number of gaussian basis functions on each side (`n_gauss`) can be specified. 53 | * The energy consumption can be limited (`limit_energy = true`) with an upper bound `energy_limit` (kWh per lap). 54 | * Safe trajectories with reduced acceleration potential can be specified (`safe_traj = true`) via acceleration ellipse, 55 | defined by the acceleration limits `ax_pos_safe`, `ax_neg_safe` and `ay_safe`. 56 | * The parameters for the two-track model can be specified in the variable `vehicle`. 57 | * The parameters for Pacejka's Magic Formula tire model can be specified in in the variable `tire`. 58 | 59 | Switch on the powertrain behavior in the parameter file in `/params` using dict `pwr_params_mintime` (`pwr_behavior = 60 | true`). 61 | 62 | # Possible Failures 63 | * The trajectory planner uses the curvature of the reference line for describing the racetrack. A smooth course of the 64 | curvature and its derivative speeds up the optimization and prevents the solution from oscillations. Therefore the 65 | reference line of the racetrack is preprocessed by an approximate spline regression. It's recommended to check the 66 | curvature input before starting optimization! If the curvature is not smooth, increase the parameter `s_reg` in the 67 | `racecar.ini`. 68 | * If red coloured warnings for warm start functionality appear in the terminal (e.g. WARNING: Failed to load warm start 69 | files!), please check if the warm start functionality is used (`use_warm_start = True`) without or with the wrong 70 | warm start files. To bypass this error, just restart the optimization without warm start functionality 71 | (`use_warm_start = False`). 72 | 73 | # Outputs 74 | * After each optimization the solution for some relevant variables (controls, states, tire forces, time, acceleration 75 | etc.) are saved in the outputs folder (`outputs/mintime/`). These files include the solution of variables with respect 76 | to the independent variable s (curvi-linear distance along the reference line) and the time t. 77 | * If you set the parameter `mintime = True` in `plots_opts` in the `main_globaltraj.py` script, some useful plots appear 78 | after optimization for visualizing control variables, state variables, tire forces etc. 79 | 80 | # Required packages 81 | * `CasADi` for solving the NLP, interfacing IPOPT, calculating derviatives with AD. 82 | * `scipy` & `sklearn` for deriving the description of friction map with linear regession using gaussian basis functions. 83 | 84 | # References 85 | * Christ, Wischnewski, Heilmeier, Lohmann\ 86 | Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients\ 87 | DOI: 10.1080/00423114.2019.1704804\ 88 | Contact person: [Fabian Christ](mailto:fabian.christ@tum.de). 89 | 90 | * Powertrain Behavior\ 91 | Herrmann, Passigato, Betz, Lienkamp\ 92 | Minimum Race-Time Planning-Strategy for an Autonomous Electric Racecar\ 93 | DOI: 10.1109/ITSC45102.2020.9294681\ 94 | Preprint: https://arxiv.org/abs/2005.07127 \ 95 | Contact person: [Thomas Herrmann](mailto:thomas.herrmann@tum.de). 96 | -------------------------------------------------------------------------------- /frictionmap/src/plot_frictionmap_data.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import matplotlib.pyplot as plt 4 | from scipy.spatial import cKDTree 5 | import os.path 6 | import json 7 | import frictionmap 8 | 9 | """ 10 | Created by: 11 | Leonhard Hermansdorfer 12 | 13 | Created on: 14 | 01.02.2019 15 | 16 | Documentation: 17 | To plot friction map data from an already existing file, adjust trackname and filenames at the bottom of this file and 18 | run this file directly. 19 | """ 20 | 21 | 22 | def plot_tpamap_fromFile(track_name: str, 23 | filename_tpamap: str, 24 | filename_frictiondata: str) -> None: 25 | """ 26 | Documentation 27 | This function loads the friction map file (*_tpamap.csv') and the friction data file ('*_tpadata.json') and creates 28 | all variables necessary for plotting the friction map as a grid containing the corresponding friction data. 29 | 30 | Input 31 | :param track_name: name of the race track 32 | :param filename_tpamap: filename of the file containing the friction map ('*_tpamap.csv'') 33 | :param filename_frictiondata: filename of the file containing the friction data ('*_tpadata.json') 34 | 35 | Output 36 | --- 37 | """ 38 | 39 | # Path Management -------------------------------------------------------------------------------------------------- 40 | 41 | path2module = os.path.dirname(os.path.abspath(__file__)).split('frictionmap')[0] 42 | path2reftrack_file = os.path.join(path2module, 'inputs', 'tracks', track_name + '.csv') 43 | filepath_frictionmap = os.path.join(path2module, 'inputs', 'frictionmaps', filename_tpamap) 44 | filepath_frictiondata = os.path.join(path2module, 'inputs', 'frictionmaps', filename_frictiondata) 45 | 46 | # Read Files ------------------------------------------------------------------------------------------------------- 47 | 48 | # load reference line and calculate track boundaries 49 | reftrack = frictionmap.src.reftrack_functions.load_reftrack(path2track=path2reftrack_file) 50 | trackbound_right, trackbound_left = frictionmap.src.reftrack_functions.calc_trackboundaries(reftrack=reftrack) 51 | 52 | # load friction map 53 | with open(filepath_frictionmap, 'rb') as fh: 54 | map_coordinates = np.loadtxt(fh, comments='#', delimiter=';') 55 | tpamap_loaded = cKDTree(map_coordinates) 56 | 57 | # load friction data 58 | with open(filepath_frictiondata, 'r') as fh: 59 | tpadata_dict_string = json.load(fh) 60 | tpadata_loaded = {int(k): np.asarray(v) for k, v in tpadata_dict_string.items()} 61 | 62 | # call function to plot friction map and corresponding friction data 63 | plot_tpamap_fromVariable(tpa_map=tpamap_loaded, 64 | tpa_data=tpadata_loaded, 65 | refline=reftrack[:, :2], 66 | trackbound_right=trackbound_right, 67 | trackbound_left=trackbound_left) 68 | 69 | 70 | def plot_tpamap_fromVariable(tpa_map: cKDTree, 71 | tpa_data: dict, 72 | refline: np.array, 73 | trackbound_right: np.array, 74 | trackbound_left: np.array) -> None: 75 | """ 76 | Documentation 77 | This function plots the friction map as a grid without the corresponding friction data. 78 | 79 | Input 80 | :param tpa_map: cKDTree object containing the coordinates of the friction map 81 | :param tpa_data: dictionary containing the friction data for each grid cell of the friction map 82 | :param refline: array consisting of the x-,y-coordinates of the reference line 83 | :param trackbound_right: array consisting of the x-,y-coordinates of the right track boundary 84 | :param trackbound_left: array consisting of the x-,y-coordinates of the left track boundary 85 | 86 | Output 87 | --- 88 | """ 89 | 90 | print("INFO: Plotting friction map with data...") 91 | 92 | list_mue = [] 93 | list_coord = [] 94 | 95 | # read values from dict 96 | for index in tpa_map.indices: 97 | list_coord.append(tpa_map.data[index]) 98 | list_mue.append(tpa_data[index]) 99 | 100 | list_coord = np.array(list_coord) 101 | 102 | # recalculate width of grid cells of friction map (width is set by the user during map generation) 103 | cellwidth_m = max(abs(tpa_map.data[0] - tpa_map.data[1])) 104 | 105 | plt.figure() 106 | plt.plot(refline[:, 0], refline[:, 1], 'r') 107 | plt.plot(trackbound_left[:, 0], trackbound_left[:, 1], 'b') 108 | plt.plot(trackbound_right[:, 0], trackbound_right[:, 1], 'b') 109 | 110 | plt.axis('equal') 111 | plt.xlim(np.amin(refline[:, 0]) - 100.0, np.amax(refline[:, 0]) + 100.0) 112 | plt.ylim(np.amin(refline[:, 1]) - 100.0, np.amax(refline[:, 1]) + 100.0) 113 | 114 | # create contourf plot --------------------------------------------------------------------------------------------- 115 | 116 | x_min = math.floor(min(tpa_map.data[:, 0])) 117 | x_max = math.ceil(max(tpa_map.data[:, 0])) 118 | 119 | y_min = math.floor(min(tpa_map.data[:, 1])) 120 | y_max = math.ceil(max(tpa_map.data[:, 1])) 121 | 122 | x_vals = np.arange(x_min - 10.0, x_max + 9.5, cellwidth_m) 123 | y_vals = np.arange(y_min - 10.0, y_max + 9.5, cellwidth_m) 124 | 125 | z = np.full((y_vals.shape[0], x_vals.shape[0]), np.nan) 126 | 127 | for row, mue in zip(list_coord, list_mue): 128 | index_column = int((row[0] - min(x_vals)) / cellwidth_m) 129 | index_row = int((-1 * row[1] + max(y_vals)) / cellwidth_m) 130 | 131 | z[index_row, index_column] = mue 132 | 133 | # change colorbar settings when only a single mue value is set globally 134 | if min(list_mue) == max(list_mue): 135 | con = plt.contourf(x_vals, np.flipud(y_vals), z, 1) 136 | cbar_tickrange = np.asarray(min(list_mue)) 137 | cbar_label = 'global mue value = ' + str(min(list_mue)[0]) 138 | 139 | else: 140 | con = plt.contourf(x_vals, np.flipud(y_vals), z, 141 | np.arange(np.round(min(list_mue) - 0.05, 1), np.round(max(list_mue) + 0.06, 1) + 0.01, 0.02)) 142 | cbar_tickrange = np.arange(np.round(min(list_mue) - 0.05, 1), np.round(max(list_mue) + 0.06, 1) + 0.01, 0.05) 143 | cbar_label = 'local mue values' 144 | 145 | # create a colorbar for the ContourSet returned by the contourf call 146 | cbar = plt.colorbar(con, cmap='viridis') 147 | cbar.set_ticks(cbar_tickrange.round(2).tolist()) 148 | cbar.set_label(cbar_label) 149 | 150 | plt.title('friction map and data') 151 | plt.xlabel('x in meters') 152 | plt.ylabel('y in meters') 153 | 154 | plt.show() 155 | 156 | 157 | # ---------------------------------------------------------------------------------------------------------------------- 158 | # testing -------------------------------------------------------------------------------------------------------------- 159 | # ---------------------------------------------------------------------------------------------------------------------- 160 | if __name__ == '__main__': 161 | 162 | track_name = 'berlin_2018' 163 | filename_tpamap = 'berlin_2018_tpamap.csv' 164 | filename_tpadata = 'berlin_2018_varmue08-12_tpadata.json' 165 | 166 | plot_tpamap_fromFile(track_name, filename_tpamap, filename_tpadata) 167 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /helper_funcs_glob/src/check_traj.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import helper_funcs_glob 3 | 4 | 5 | def check_traj(reftrack: np.ndarray, 6 | reftrack_normvec_normalized: np.ndarray, 7 | trajectory: np.ndarray, 8 | ggv: np.ndarray, 9 | ax_max_machines: np.ndarray, 10 | v_max: float, 11 | length_veh: float, 12 | width_veh: float, 13 | debug: bool, 14 | dragcoeff: float, 15 | mass_veh: float, 16 | curvlim: float) -> tuple: 17 | """ 18 | Created by: 19 | Alexander Heilmeier 20 | 21 | Documentation: 22 | This function checks the generated trajectory in regards of minimum distance to the boundaries and maximum 23 | curvature and accelerations. 24 | 25 | Inputs: 26 | reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m] 27 | reftrack_normvec_normalized: normalized normal vectors on the reference line [x_m, y_m] 28 | trajectory: trajectory to be checked [s_m, x_m, y_m, psi_rad, kappa_radpm, vx_mps, ax_mps2] 29 | ggv: ggv-diagram to be applied: [vx, ax_max, ay_max]. Velocity in m/s, accelerations in m/s2. 30 | ax_max_machines: longitudinal acceleration limits by the electrical motors: [vx, ax_max_machines]. Velocity 31 | in m/s, accelerations in m/s2. They should be handed in without considering drag resistance. 32 | v_max: Maximum longitudinal speed in m/s. 33 | length_veh: vehicle length in m 34 | width_veh: vehicle width in m 35 | debug: boolean showing if debug messages should be printed 36 | dragcoeff: [m2*kg/m3] drag coefficient containing c_w_A * rho_air * 0.5 37 | mass_veh: [kg] mass 38 | curvlim: [rad/m] maximum drivable curvature 39 | 40 | Outputs: 41 | bound_r: right track boundary [x_m, y_m] 42 | bound_l: left track boundary [x_m, y_m] 43 | """ 44 | 45 | # ------------------------------------------------------------------------------------------------------------------ 46 | # CHECK VEHICLE EDGES FOR MINIMUM DISTANCE TO TRACK BOUNDARIES ----------------------------------------------------- 47 | # ------------------------------------------------------------------------------------------------------------------ 48 | 49 | # calculate boundaries and interpolate them to small stepsizes (currently linear interpolation) 50 | bound_r = reftrack[:, :2] + reftrack_normvec_normalized * np.expand_dims(reftrack[:, 2], 1) 51 | bound_l = reftrack[:, :2] - reftrack_normvec_normalized * np.expand_dims(reftrack[:, 3], 1) 52 | 53 | # check boundaries for vehicle edges 54 | bound_r_tmp = np.column_stack((bound_r, np.zeros((bound_r.shape[0], 2)))) 55 | bound_l_tmp = np.column_stack((bound_l, np.zeros((bound_l.shape[0], 2)))) 56 | 57 | bound_r_interp = helper_funcs_glob.src.interp_track.interp_track(reftrack=bound_r_tmp, 58 | stepsize_approx=1.0)[0] 59 | bound_l_interp = helper_funcs_glob.src.interp_track.interp_track(reftrack=bound_l_tmp, 60 | stepsize_approx=1.0)[0] 61 | 62 | # calculate minimum distances of every trajectory point to the boundaries 63 | min_dists = helper_funcs_glob.src.calc_min_bound_dists.calc_min_bound_dists(trajectory=trajectory, 64 | bound1=bound_r_interp, 65 | bound2=bound_l_interp, 66 | length_veh=length_veh, 67 | width_veh=width_veh) 68 | 69 | # calculate overall minimum distance 70 | min_dist = np.amin(min_dists) 71 | 72 | # warn if distance falls below a safety margin of 1.0 m 73 | if min_dist < 1.0: 74 | print("WARNING: Minimum distance to boundaries is estimated to %.2fm. Keep in mind that the distance can also" 75 | " lie on the outside of the track!" % min_dist) 76 | elif debug: 77 | print("INFO: Minimum distance to boundaries is estimated to %.2fm. Keep in mind that the distance can also lie" 78 | " on the outside of the track!" % min_dist) 79 | 80 | # ------------------------------------------------------------------------------------------------------------------ 81 | # CHECK FINAL TRAJECTORY FOR MAXIMUM CURVATURE --------------------------------------------------------------------- 82 | # ------------------------------------------------------------------------------------------------------------------ 83 | 84 | # check maximum (absolute) curvature 85 | if np.amax(np.abs(trajectory[:, 4])) > curvlim: 86 | print("WARNING: Curvature limit is exceeded: %.3frad/m" % np.amax(np.abs(trajectory[:, 4]))) 87 | 88 | # ------------------------------------------------------------------------------------------------------------------ 89 | # CHECK FINAL TRAJECTORY FOR MAXIMUM ACCELERATIONS ----------------------------------------------------------------- 90 | # ------------------------------------------------------------------------------------------------------------------ 91 | 92 | if ggv is not None: 93 | # transform curvature kappa into corresponding radii (abs because curvature has a sign in our convention) 94 | radii = np.abs(np.divide(1.0, trajectory[:, 4], 95 | out=np.full(trajectory.shape[0], np.inf), 96 | where=trajectory[:, 4] != 0)) 97 | 98 | # check max. lateral accelerations 99 | ay_profile = np.divide(np.power(trajectory[:, 5], 2), radii) 100 | 101 | if np.any(ay_profile > np.amax(ggv[:, 2]) + 0.1): 102 | print("WARNING: Lateral ggv acceleration limit is exceeded: %.2fm/s2" % np.amax(ay_profile)) 103 | 104 | # check max. longitudinal accelerations (consider that drag is included in the velocity profile!) 105 | ax_drag = -np.power(trajectory[:, 5], 2) * dragcoeff / mass_veh 106 | ax_wo_drag = trajectory[:, 6] - ax_drag 107 | 108 | if np.any(ax_wo_drag > np.amax(ggv[:, 1]) + 0.1): 109 | print("WARNING: Longitudinal ggv acceleration limit (positive) is exceeded: %.2fm/s2" % np.amax(ax_wo_drag)) 110 | 111 | if np.any(ax_wo_drag < np.amin(-ggv[:, 1]) - 0.1): 112 | print("WARNING: Longitudinal ggv acceleration limit (negative) is exceeded: %.2fm/s2" % np.amin(ax_wo_drag)) 113 | 114 | # check total acceleration 115 | a_tot = np.sqrt(np.power(ax_wo_drag, 2) + np.power(ay_profile, 2)) 116 | 117 | if np.any(a_tot > np.amax(ggv[:, 1:]) + 0.1): 118 | print("WARNING: Total ggv acceleration limit is exceeded: %.2fm/s2" % np.amax(a_tot)) 119 | 120 | else: 121 | print("WARNING: Since ggv-diagram was not given the according checks cannot be performed!") 122 | 123 | if ax_max_machines is not None: 124 | # check max. longitudinal accelerations (consider that drag is included in the velocity profile!) 125 | ax_drag = -np.power(trajectory[:, 5], 2) * dragcoeff / mass_veh 126 | ax_wo_drag = trajectory[:, 6] - ax_drag 127 | 128 | if np.any(ax_wo_drag > np.amax(ax_max_machines[:, 1]) + 0.1): 129 | print("WARNING: Longitudinal acceleration machine limits are exceeded: %.2fm/s2" % np.amax(ax_wo_drag)) 130 | 131 | # ------------------------------------------------------------------------------------------------------------------ 132 | # CHECK FINAL TRAJECTORY FOR MAXIMUM VELOCITY ---------------------------------------------------------------------- 133 | # ------------------------------------------------------------------------------------------------------------------ 134 | 135 | if np.any(trajectory[:, 5] > v_max + 0.1): 136 | print("WARNING: Maximum velocity of final trajectory exceeds the maximal velocity of the vehicle: %.2fm/s!" 137 | % np.amax(trajectory[:, 5])) 138 | 139 | return bound_r, bound_l 140 | 141 | 142 | # testing -------------------------------------------------------------------------------------------------------------- 143 | if __name__ == "__main__": 144 | pass 145 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/src/Inverter.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | 3 | 4 | class InverterModel: 5 | 6 | __slots__ = ('pars', 7 | 'temp_inv_n', 8 | 'temp_inv_s', 9 | 'temp_inv', 10 | 'dtemp', 11 | 'temp_min', 12 | 'temp_max', 13 | 'temp_guess', 14 | 'f_nlp', 15 | 'f_sol', 16 | 'p_in_inv', 17 | 'p_loss_switch', 18 | 'p_loss_cond', 19 | 'p_loss_total', 20 | 'p_loss_total_all_inverters', 21 | 'r_inv', 22 | 'p_losses_opt') 23 | 24 | def __init__(self, 25 | pwr_pars: dict): 26 | 27 | """ 28 | Python version: 3.5 29 | Created by: Thomas Herrmann (thomas.herrmann@tum.de) 30 | Created on: 01.04.2020 31 | 32 | Documentation: Inverter class for the optimization of global trajectories for electric race cars implemented in 33 | the CasADi modeling language. 34 | 35 | Inputs: 36 | pwr_pars: powertrain parameters defined in the initialization file 37 | """ 38 | 39 | self.pars = pwr_pars 40 | 41 | # -------------------------------------------------------------------------------------------------------------- 42 | # Empty inverter states 43 | # -------------------------------------------------------------------------------------------------------------- 44 | 45 | self.temp_inv_n = None 46 | self.temp_inv_s = None 47 | self.temp_inv = None 48 | self.dtemp = None 49 | self.temp_min = None 50 | self.temp_max = None 51 | self.temp_guess = None 52 | 53 | self.f_nlp = None 54 | self.f_sol = None 55 | 56 | self.p_in_inv = None 57 | self.p_loss_switch = None 58 | self.p_loss_cond = None 59 | self.p_loss_total = None 60 | self.p_loss_total_all_inverters = None 61 | 62 | self.r_inv = None 63 | 64 | # Optimized losses list: p_loss_total, p_loss_effects 65 | self.p_losses_opt = [] 66 | 67 | # Call initialization function 68 | self.initialize() 69 | 70 | def initialize(self): 71 | """ 72 | Python version: 3.5 73 | Created by: Thomas Herrmann 74 | Created on: 01.04.2020 75 | 76 | Documentation: Initialization of necessary optimization variables (symbolic CasADi expressions) 77 | and states including limits. 78 | """ 79 | 80 | self.temp_inv_n = ca.SX.sym('temp_inv_n') 81 | self.temp_inv_s = self.pars["temp_inv_max"] - 30 82 | self.temp_inv = self.temp_inv_s * self.temp_inv_n 83 | 84 | # Define limits and initial guess 85 | self.temp_min = self.pars["T_env"] / self.temp_inv_s 86 | self.temp_max = self.pars["temp_inv_max"] / self.temp_inv_s 87 | self.temp_guess = self.pars["T_env"] / self.temp_inv_s 88 | 89 | self.get_thermal_resistance() 90 | 91 | def get_increment(self, 92 | sf: ca.SX, 93 | temp_cool_mi: ca.SX, 94 | temp_cool_12: ca.SX): 95 | """ 96 | Python version: 3.5 97 | Created by: Thomas Herrmann 98 | Created on: 01.04.2020 99 | 100 | Documentation: Initializes temperature increment of inverter symbolically (sf * dx/dt = dx/ds) 101 | 102 | Inputs: 103 | sf: transformation factor dt/ds 104 | temp_cool_mi: cooling fluid temperature of machine-inverter cooling circuit [°C] 105 | temp_cool_12: intermediate temperature within motor-inverter cooling circuit (radiator-motor) [°C] 106 | """ 107 | 108 | self.dtemp = sf * ((self.p_loss_total * 1000 - (self.temp_inv - (temp_cool_mi + temp_cool_12) / 2) 109 | / self.r_inv) 110 | / (self.pars["C_therm_inv"])) 111 | 112 | def get_loss(self, 113 | i_eff: ca.SX, 114 | v_dc: ca.SX, 115 | p_out_inv: ca.SX = None): 116 | """ 117 | Python version: 3.5 118 | Created by: Thomas Herrmann 119 | Created on: 01.04.2020 120 | 121 | Documentation: Initializes total power loss of a single inverter and split into loss effects 122 | (with detailed models) or loss power of a single e-machine using a simple power fit to measured data 123 | (input -- output power). 124 | p_out_inv can be left empty in case of detailed loss model usage. 125 | 126 | Inputs: 127 | i_eff: effective current through one electric machine [A] 128 | v_dc: terminal voltage battery [V] 129 | p_out_inv: output power of single inverter [kW] 130 | """ 131 | 132 | if self.pars["simple_loss"]: 133 | 134 | # Input in single inverter [kW] = inverter output + inverter loss 135 | self.p_in_inv = \ 136 | self.pars["inverter_simple_a"] * p_out_inv ** 2 \ 137 | + self.pars["inverter_simple_b"] * p_out_inv \ 138 | + self.pars["inverter_simple_c"] 139 | 140 | # Total loss [kW] 141 | self.p_loss_total = (self.p_in_inv - p_out_inv) 142 | 143 | else: 144 | 145 | # Power loss switching [W] 146 | p_loss_switch = (v_dc / self.pars["V_ref"]) \ 147 | * ((3 * self.pars["f_sw"]) * (i_eff / self.pars["I_ref"]) 148 | * (self.pars["E_on"] + self.pars["E_off"] + self.pars["E_rr"])) 149 | 150 | # Power loss conducting [W] 151 | p_loss_cond = 3 * i_eff * (self.pars["V_ce_offset"] + (self.pars["V_ce_slope"] * i_eff)) 152 | 153 | # Loss effects [kW] 154 | self.p_loss_switch = 0.001 * p_loss_switch 155 | self.p_loss_cond = 0.001 * p_loss_cond 156 | 157 | # Total loss [kW] 158 | self.p_loss_total = (p_loss_switch + p_loss_cond) * 0.001 159 | 160 | def get_inverters_cum_losses(self): 161 | """ 162 | Python version: 3.5 163 | Created by: Thomas Herrmann 164 | Created on: 01.04.2020 165 | 166 | Documentation: Calculate total loss of all inverters in electric powertrain 167 | """ 168 | 169 | self.p_loss_total_all_inverters = self.p_loss_total * self.pars["N_machines"] 170 | 171 | def get_thermal_resistance(self): 172 | """ 173 | Python version: 3.5 174 | Created by: Thomas Herrmann 175 | Created on: 01.04.2020 176 | 177 | Documentation: Calculates thermal resistance of inverter 178 | """ 179 | 180 | # Thermal resistance inverter [K/W] 181 | self.r_inv = 1 / (self.pars["h_fluid_mi"] * self.pars["A_cool_inv"]) 182 | 183 | def ini_nlp_state(self, 184 | x: ca.SX, 185 | u: ca.SX): 186 | """ 187 | Python version: 3.5 188 | Created by: Thomas Herrmann 189 | Created on: 01.04.2020 190 | 191 | Documentation: Defines function to define inverter states in NLP 192 | 193 | Inputs: 194 | x: discrete NLP state 195 | u: discrete NLP control input 196 | """ 197 | 198 | if self.pars["simple_loss"]: 199 | self.f_nlp = \ 200 | ca.Function('f_nlp', 201 | [x, u], [self.p_loss_total, self.p_in_inv], 202 | ['x', 'u'], ['p_loss_total', 'p_inv_in']) 203 | else: 204 | self.f_nlp = \ 205 | ca.Function('f_nlp', 206 | [x, u], [self.p_loss_total, self.p_loss_switch, self.p_loss_cond], 207 | ['x', 'u'], ['p_loss_total', 'p_loss_switch', 'p_loss_cond']) 208 | 209 | def extract_sol(self, 210 | w: ca.SX, 211 | sol_states: ca.DM): 212 | """ 213 | Python version: 3.5 214 | Created by: Thomas Herrmann 215 | Created on: 01.04.2020 216 | 217 | Documentation: Defines function to retrieve values of optimized NLP inverter 218 | 219 | Inputs: 220 | w: discrete optimized NLP decision variables (x and u) 221 | sol_states: numeric values belonging to the symbolic NLP decision variables w 222 | """ 223 | 224 | if self.pars["simple_loss"]: 225 | self.f_sol = \ 226 | ca.Function('f_sol', 227 | [w], [self.p_losses_opt], 228 | ['w'], ['p_losses_opt']) 229 | 230 | # Overwrite lists with optimized numeric values 231 | p_losses_opt = self.f_sol(sol_states) 232 | 233 | self.p_loss_total = p_losses_opt[0::2] 234 | self.p_in_inv = p_losses_opt[1::2] 235 | 236 | else: 237 | self.f_sol = \ 238 | ca.Function('f_sol', 239 | [w], [self.p_losses_opt], 240 | ['w'], ['p_losses_opt']) 241 | 242 | # Overwrite lists with optimized numeric values 243 | p_losses_opt = self.f_sol(sol_states) 244 | 245 | self.p_loss_total = p_losses_opt[0::3] 246 | self.p_loss_switch = p_losses_opt[1::3] 247 | self.p_loss_cond = p_losses_opt[2::3] 248 | 249 | 250 | if __name__ == "__main__": 251 | pass 252 | -------------------------------------------------------------------------------- /opt_mintime_traj/src/approx_friction_map.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import trajectory_planning_helpers as tph 4 | from sklearn.base import BaseEstimator, TransformerMixin 5 | from sklearn.pipeline import make_pipeline 6 | from sklearn.linear_model import LinearRegression 7 | import opt_mintime_traj 8 | 9 | 10 | def approx_friction_map(reftrack: np.ndarray, 11 | normvectors: np.ndarray, 12 | tpamap_path: str, 13 | tpadata_path: str, 14 | pars: dict, 15 | dn: float, 16 | n_gauss: int, 17 | print_debug: bool, 18 | plot_debug: bool) -> tuple: 19 | """ 20 | Created by: 21 | Fabian Christ 22 | 23 | Documentation: 24 | A simplified dependency between the friction coefficients (mue) and the lateral distance to the reference line (n) 25 | is obtained for each wheel along the racetrack. For this purpose friction coefficients are determined for a fine 26 | grid on the normal vectors from the friction map. Then the dependency between the extracted friction coefficients 27 | and the decision variable n for each path coordinate s_k is described by linear equations (var_friction: "lienar") 28 | or by linear regression with gaussian basis functions (var_friction: "gauss"). 29 | 30 | Inputs: 31 | reftrack: track [x_m, y_m, w_tr_right_m, w_tr_left_m] 32 | normvectors: array containing normalized normal vectors for every traj. point [x_component, y_component] 33 | tpamap_path: file path to tpa map (required for friction map loading) 34 | tpadata_path: file path to tpa data (required for friction map loading) 35 | pars: parameters dictionary 36 | dn: distance of equidistant points on normal vectors for extracting the friction coefficients 37 | n_gauss: number of gaussian basis functions on each side (n_gauss_tot = 2 * n_gauss + 1) 38 | print_debug: determines if debug prints are shown 39 | plot_debug: determines if debug plots are shown 40 | 41 | Outputs: 42 | w_mue_fl: parameters for friction map approximation along the racetrack (left front wheel) 43 | w_mue_fr: parameters for friction map approximation along the racetrack (right front wheel) 44 | w_mue_rl: parameters for friction map approximation along the racetrack (left rear wheel) 45 | w_mue_rr: parameters for friction map approximation along the racetrack (right rear wheel) 46 | center_dist distance between two gaussian basis functions along the racetrack (only for var_friction: "gauss") 47 | """ 48 | 49 | # ------------------------------------------------------------------------------------------------------------------ 50 | # PREPARATION ------------------------------------------------------------------------------------------------------ 51 | # ------------------------------------------------------------------------------------------------------------------ 52 | 53 | # extract friction coefficients from friction map 54 | n, mue_fl, mue_fr, mue_rl, mue_rr = opt_mintime_traj.src.extract_friction_coeffs.\ 55 | extract_friction_coeffs(reftrack=reftrack, 56 | normvectors=normvectors, 57 | tpamap_path=tpamap_path, 58 | tpadata_path=tpadata_path, 59 | pars=pars, 60 | dn=dn, 61 | print_debug=print_debug, 62 | plot_debug=plot_debug) 63 | 64 | # number of steps along the reference line 65 | num_steps = len(n) 66 | 67 | # number of guassian basis functions 68 | n_gauss_tot = 2 * n_gauss + 1 69 | 70 | # initialize solution vectors 71 | if pars["optim_opts"]["var_friction"] == 'linear': 72 | w_mue_fl = np.zeros((num_steps, 2)) 73 | w_mue_fr = np.zeros((num_steps, 2)) 74 | w_mue_rl = np.zeros((num_steps, 2)) 75 | w_mue_rr = np.zeros((num_steps, 2)) 76 | center_dist = np.zeros((num_steps, 1)) 77 | 78 | elif pars["optim_opts"]["var_friction"] == "gauss": 79 | w_mue_fl = np.zeros((num_steps, n_gauss_tot + 1)) 80 | w_mue_fr = np.zeros((num_steps, n_gauss_tot + 1)) 81 | w_mue_rl = np.zeros((num_steps, n_gauss_tot + 1)) 82 | w_mue_rr = np.zeros((num_steps, n_gauss_tot + 1)) 83 | center_dist = np.zeros((num_steps, 1)) 84 | 85 | else: 86 | raise ValueError('Unknown method for friction map approximation!') 87 | 88 | if plot_debug: 89 | plt.figure(1) 90 | 91 | # ------------------------------------------------------------------------------------------------------------------ 92 | # CALCULATION ------------------------------------------------------------------------------------------------------ 93 | # ------------------------------------------------------------------------------------------------------------------ 94 | 95 | for i in range(num_steps): 96 | if pars["optim_opts"]["var_friction"] == "linear": 97 | w_mue_fl[i, :] = np.polyfit(n[i], mue_fl[i].T[0], 1) 98 | w_mue_fr[i, :] = np.polyfit(n[i], mue_fr[i].T[0], 1) 99 | w_mue_rl[i, :] = np.polyfit(n[i], mue_rl[i].T[0], 1) 100 | w_mue_rr[i, :] = np.polyfit(n[i], mue_rr[i].T[0], 1) 101 | 102 | elif pars["optim_opts"]["var_friction"] == "gauss": 103 | # get distance between center of gaussian basis functions 104 | center_dist[i, 0] = (n[i][-1] - n[i][0]) / (2 * n_gauss) 105 | 106 | # regression with gaussian basis functions (see class definition below) 107 | gauss_model = make_pipeline(GaussianFeatures(n_gauss_tot), LinearRegression()) 108 | 109 | # regression for front left wheel 110 | gauss_model.fit(n[i][:, np.newaxis], mue_fl[i]) 111 | w_mue_fl[i, :n_gauss_tot] = gauss_model._final_estimator.coef_[0] 112 | w_mue_fl[i, n_gauss_tot] = gauss_model._final_estimator.intercept_[0] 113 | 114 | # regression for front right wheel 115 | gauss_model.fit(n[i][:, np.newaxis], mue_fr[i]) 116 | w_mue_fr[i, :n_gauss_tot] = gauss_model._final_estimator.coef_[0] 117 | w_mue_fr[i, n_gauss_tot] = gauss_model._final_estimator.intercept_[0] 118 | 119 | # regression for rear left wheel 120 | gauss_model.fit(n[i][:, np.newaxis], mue_rl[i]) 121 | w_mue_rl[i, :n_gauss_tot] = gauss_model._final_estimator.coef_[0] 122 | w_mue_rl[i, n_gauss_tot] = gauss_model._final_estimator.intercept_[0] 123 | 124 | # regression for rear right wheel 125 | gauss_model.fit(n[i][:, np.newaxis], mue_rr[i]) 126 | w_mue_rr[i, :n_gauss_tot] = gauss_model._final_estimator.coef_[0] 127 | w_mue_rr[i, n_gauss_tot] = gauss_model._final_estimator.intercept_[0] 128 | 129 | if print_debug: 130 | tph.progressbar.progressbar(i, num_steps, 'Approximation of friction map') 131 | 132 | if plot_debug and pars["optim_opts"]["var_friction"] == "linear": 133 | n_fit = np.linspace(n[i][0], n[i][-1], 3) 134 | plt.scatter(n[i], mue_rr[i]) 135 | plt.plot(n_fit, w_mue_rr[i, 0] * n_fit + w_mue_rr[i, 1]) 136 | 137 | elif plot_debug and pars["optim_opts"]["var_friction"] == "gauss": 138 | n_fit = np.linspace(n[i][0], n[i][-1], 100) 139 | plt.scatter(n[i], mue_rr[i]) 140 | plt.plot(n_fit, gauss_model.predict(n_fit[:, np.newaxis])) 141 | 142 | if plot_debug: 143 | plt.xlabel('n in m') 144 | plt.ylabel(r'$\it{\mu}$') 145 | plt.title('Approximation of friction map (e.g. for tire rear right)') 146 | plt.show() 147 | 148 | return w_mue_fl, w_mue_fr, w_mue_rl, w_mue_rr, center_dist 149 | 150 | 151 | # ---------------------------------------------------------------------------------------------------------------------- 152 | # GAUSSIAN FEATURES CLASS ---------------------------------------------------------------------------------------------- 153 | # ---------------------------------------------------------------------------------------------------------------------- 154 | 155 | # uniformly spaced Gaussian features for one-dimensional input 156 | class GaussianFeatures(BaseEstimator, TransformerMixin): 157 | def __init__(self, N, width_factor=2.0): 158 | self.N = N 159 | self.width_factor = width_factor 160 | self.centers_ = None 161 | self.width_ = None 162 | 163 | @staticmethod 164 | def _gauss_basis(x, y, width, axis=None): 165 | arg = (x - y) / width 166 | return np.exp(-0.5 * np.sum(arg ** 2, axis)) 167 | 168 | def fit(self, X, y=None): 169 | # create N centers spread along the data range 170 | self.centers_ = np.linspace(X.min(), X.max(), self.N) 171 | self.width_ = self.width_factor * (self.centers_[1] - self.centers_[0]) 172 | return self 173 | 174 | def transform(self, X): 175 | return self._gauss_basis(X[:, :, np.newaxis], self.centers_, self.width_, axis=1) 176 | 177 | 178 | # testing -------------------------------------------------------------------------------------------------------------- 179 | if __name__ == "__main__": 180 | pass 181 | -------------------------------------------------------------------------------- /main_gen_frictionmap.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import json 4 | import time 5 | from datetime import datetime 6 | import os.path 7 | import matplotlib.path as mplPath 8 | from scipy.spatial import cKDTree 9 | import frictionmap 10 | 11 | """ 12 | Created by: 13 | Leonhard Hermansdorfer 14 | 15 | Created on: 16 | 01.12.2018 17 | 18 | Documentation: 19 | This script generates a grid respresenting the friction map with specified cellwidth. Additionally, it fills the 20 | corresponding cells of the friction map with a default mue value. 21 | """ 22 | 23 | # ---------------------------------------------------------------------------------------------------------------------- 24 | # USER INPUT ----------------------------------------------------------------------------------------------------------- 25 | # ---------------------------------------------------------------------------------------------------------------------- 26 | 27 | # track_name: track name for which a friction map should be generated (csv must be in the inputs folder). 28 | # initial_mue: mue value which should be used to initialize the friction map (all cells contain this value). 29 | # cellwidth_m: width of the grid cells of the friction map (cells are quadratic). 30 | # inside_trackbound: specifies which trackbound is on the inside of the racetrack ('right' or 'left'). This is 31 | # only necessary for circuits (closed racetracks). 32 | # bool_show_plots: boolean which enables plotting of the reference line, the friction map and the corresponding 33 | # mue values 34 | 35 | track_name = "modena_2019" 36 | initial_mue = 0.8 37 | cellwidth_m = 2.0 38 | inside_trackbound = 'right' 39 | bool_show_plots = True 40 | 41 | # ---------------------------------------------------------------------------------------------------------------------- 42 | # INITIALIZATION ------------------------------------------------------------------------------------------------------- 43 | # ---------------------------------------------------------------------------------------------------------------------- 44 | 45 | # determine names of output files 46 | datetime_save = datetime.now().strftime("%Y%m%d_%H%M%S") 47 | filename_tpamap = datetime_save + '_' + track_name + '_tpamap.csv' 48 | filename_tpadata = datetime_save + '_' + track_name + '_tpadata.json' 49 | 50 | # set paths 51 | path2module = os.path.dirname(os.path.abspath(__file__)) 52 | path2reftrack_file = os.path.join(path2module, 'inputs', 'tracks', track_name + '.csv') 53 | path2tpamap_file = os.path.join(path2module, 'inputs', 'frictionmaps', filename_tpamap) 54 | path2tpadata_file = os.path.join(path2module, 'inputs', 'frictionmaps', filename_tpadata) 55 | 56 | # ---------------------------------------------------------------------------------------------------------------------- 57 | # CALCULATE REFERENCE LINE --------------------------------------------------------------------------------------------- 58 | # ---------------------------------------------------------------------------------------------------------------------- 59 | 60 | # read reference track file 61 | reftrack = frictionmap.src.reftrack_functions.load_reftrack(path2track=path2reftrack_file) 62 | 63 | # check whether reference line is closed (is the race track a circuit or not) 64 | bool_isclosed_refline = frictionmap.src.reftrack_functions.check_isclosed_refline(refline=reftrack[:, :2]) 65 | 66 | # calculate coordinates of the track boundaries 67 | reftrackbound_right, reftrackbound_left = frictionmap.src.reftrack_functions.calc_trackboundaries(reftrack=reftrack) 68 | 69 | # ---------------------------------------------------------------------------------------------------------------------- 70 | # SAMPLE COORDINATES FOR FRICTION MAP ---------------------------------------------------------------------------------- 71 | # ---------------------------------------------------------------------------------------------------------------------- 72 | 73 | timer_start = time.perf_counter() 74 | 75 | # match left/right track boundary to "inside" and "outside" (only necessary for circuits) 76 | if bool_isclosed_refline and inside_trackbound == 'right': 77 | trackbound_inside = reftrackbound_right 78 | trackbound_outside = reftrackbound_left 79 | 80 | sign_trackbound = -1 81 | 82 | else: 83 | trackbound_inside = reftrackbound_left 84 | trackbound_outside = reftrackbound_right 85 | 86 | sign_trackbound = 1 87 | 88 | # set a default distance which is added / subtracted from max / min reference line coordinate to ensure that whole 89 | # racetrack is covered during coordinate point sampling 90 | default_delta = int(math.ceil(np.amax(reftrack[:, 2]) + np.amax(reftrack[:, 3]) + 5.0)) 91 | 92 | # calculate necessary range to cover the whole racetrack with grid xyRange = [x_min, x_max, y_min, y_max] 93 | xyRange = [int(math.floor(np.amin(reftrack[:, 0]) - default_delta)), 94 | int(math.ceil(np.amax(reftrack[:, 0]) + default_delta)), 95 | int(math.floor(np.amin(reftrack[:, 1]) - default_delta)), 96 | int(math.ceil(np.amax(reftrack[:, 1]) + default_delta))] 97 | 98 | # set-up 2D-grid 99 | x_grid = np.arange(xyRange[0], xyRange[1] + 0.1, cellwidth_m) 100 | y_grid = np.arange(xyRange[2], xyRange[3] + 0.1, cellwidth_m) 101 | 102 | # get number of coordinates for array initialization 103 | size_array = x_grid.shape[0] * y_grid.shape[0] 104 | coordinates = np.empty((size_array, 2)) 105 | 106 | # create coordinate array which contains all coordinate of the defined grid 107 | i_row = 0 108 | 109 | for x_coord in x_grid: 110 | coordinates[i_row:i_row + y_grid.shape[0], 0] = np.full((y_grid.shape[0]), x_coord) 111 | coordinates[i_row:i_row + y_grid.shape[0], 1] = y_grid 112 | i_row += y_grid.shape[0] 113 | 114 | # set maximum distance between grid cells outside the track and trackboundaries to determine all relevant grid cells 115 | dist_to_trackbound = cellwidth_m * 1.1 116 | 117 | # distinguish between a closed racetrack (circuit) and an "open" racetrack 118 | if bool_isclosed_refline: 119 | bool_isIn_rightBound = mplPath.Path(trackbound_outside).\ 120 | contains_points(coordinates, radius=(dist_to_trackbound * sign_trackbound)) 121 | bool_isIn_leftBound = mplPath.Path(trackbound_inside).\ 122 | contains_points(coordinates, radius=-(dist_to_trackbound * sign_trackbound)) 123 | bool_OnTrack = (bool_isIn_rightBound & ~bool_isIn_leftBound) 124 | 125 | else: 126 | trackbound = np.vstack((trackbound_inside, np.flipud(trackbound_outside))) 127 | bool_OnTrack = mplPath.Path(trackbound).contains_points(coordinates, radius=-dist_to_trackbound) 128 | 129 | # generate the friction map with coordinates which are within the trackboundaries or within the defined range outside 130 | tpa_map = cKDTree(coordinates[bool_OnTrack]) 131 | 132 | print('INFO: Time elapsed for tpa_map building: {:.3f}s\nINFO: tpa_map contains {} coordinate points'.format( 133 | (time.perf_counter() - timer_start), tpa_map.n)) 134 | 135 | # ---------------------------------------------------------------------------------------------------------------------- 136 | # SAVE FRICTION MAP ---------------------------------------------------------------------------------------------------- 137 | # ---------------------------------------------------------------------------------------------------------------------- 138 | 139 | timer_start = time.perf_counter() 140 | 141 | # create dictionary filled with default mue value (value as numpy array) 142 | tpamap_indices = tpa_map.indices 143 | tpa_data = dict(zip(tpamap_indices, np.full((tpamap_indices.shape[0], 1), initial_mue))) 144 | 145 | print('INFO: Time elapsed for tpa_data dictionary building: {:.3f}s'.format(time.perf_counter() - timer_start)) 146 | 147 | # save friction map (only grid) ('*_tpamap.csv') 148 | with open(path2tpamap_file, 'wb') as fh: 149 | np.savetxt(fh, tpa_map.data, fmt='%0.4f', delimiter=';', header='x_m;y_m') 150 | 151 | print('INFO: tpa_map saved successfully!') 152 | 153 | # get tpadata as string to save as a dictionary (as .json file) 154 | tpa_data_string = {str(k): list(v) for k, v in tpa_data.items()} 155 | 156 | # save friction data ('*_tpadata.json') 157 | with open(path2tpadata_file, 'w') as fh: 158 | json.dump(tpa_data_string, fh, separators=(',', ': ')) 159 | 160 | print('INFO: tpa_data saved successfully!') 161 | 162 | # ---------------------------------------------------------------------------------------------------------------------- 163 | # CREATE PLOTS --------------------------------------------------------------------------------------------------------- 164 | # ---------------------------------------------------------------------------------------------------------------------- 165 | 166 | if bool_show_plots: 167 | # plot reference line and normal vectors 168 | frictionmap.src.reftrack_functions.plot_refline(reftrack=reftrack) 169 | 170 | # plot spatial grid of friction map 171 | frictionmap.src.plot_frictionmap_grid.\ 172 | plot_voronoi_fromVariable(tree=tpa_map, 173 | refline=reftrack[:, :2], 174 | trackbound_left=reftrackbound_left, 175 | trackbound_right=reftrackbound_right) 176 | 177 | # plot friction data of friction map 178 | frictionmap.src.plot_frictionmap_data.\ 179 | plot_tpamap_fromVariable(tpa_map=tpa_map, 180 | tpa_data=tpa_data, 181 | refline=reftrack[:, :2], 182 | trackbound_left=reftrackbound_left, 183 | trackbound_right=reftrackbound_right) 184 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/src/Radiators.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | 3 | 4 | class RadiatorModel: 5 | 6 | __slots__ = ('pars', 7 | 'temp_cool_mi_n', 8 | 'temp_cool_mi_s', 9 | 'temp_cool_mi', 10 | 'temp_cool_b_n', 11 | 'temp_cool_b_s', 12 | 'temp_cool_b', 13 | 'temp_cool_mi_min', 14 | 'temp_cool_mi_max', 15 | 'temp_cool_mi_guess', 16 | 'temp_cool_b_min', 17 | 'temp_cool_b_max', 18 | 'temp_cool_b_guess', 19 | 'temp_cool_12', 20 | 'temp_cool_13', 21 | 'dtemp_cool_mi', 22 | 'dtemp_cool_b', 23 | 'r_rad', 24 | 'f_nlp', 25 | 'f_sol', 26 | 'temps_opt') 27 | 28 | def __init__(self, 29 | pwr_pars: dict): 30 | """ 31 | Python version: 3.5 32 | Created by: Thomas Herrmann (thomas.herrmann@tum.de) 33 | Created on: 01.04.2020 34 | 35 | Documentation: Radiators class for the optimization of global trajectories for electric race cars implemented in 36 | the CasADi modeling language. 37 | 38 | Inputs: 39 | pwr_pars: powertrain parameters defined in the initialization file 40 | """ 41 | 42 | # Store powertrain parameters 43 | self.pars = pwr_pars 44 | 45 | # -------------------------------------------------------------------------------------------------------------- 46 | # Empty radiator states 47 | # -------------------------------------------------------------------------------------------------------------- 48 | self.temp_cool_mi_n = None 49 | self.temp_cool_mi_s = None 50 | self.temp_cool_mi = None 51 | 52 | self.temp_cool_b_n = None 53 | self.temp_cool_b_s = None 54 | self.temp_cool_b = None 55 | 56 | self.temp_cool_mi_min = None 57 | self.temp_cool_mi_max = None 58 | self.temp_cool_mi_guess = None 59 | self.temp_cool_b_min = None 60 | self.temp_cool_b_max = None 61 | self.temp_cool_b_guess = None 62 | 63 | self.temp_cool_12 = None 64 | self.temp_cool_13 = None 65 | 66 | self.dtemp_cool_mi = None 67 | self.dtemp_cool_b = None 68 | 69 | self.r_rad = None 70 | 71 | self.f_nlp = None 72 | self.f_sol = None 73 | 74 | # Optimized temperatures list 75 | self.temps_opt = [] 76 | 77 | # Call initialization function 78 | self.initialize() 79 | 80 | def initialize(self): 81 | """ 82 | Python version: 3.5 83 | Created by: Thomas Herrmann 84 | Created on: 01.04.2020 85 | 86 | Documentation: Initialization of necessary optimization variables (symbolic CasADi expressions) 87 | and states including limits. 88 | """ 89 | 90 | # cooling liquid temperature for motor and inverter circuit [°C] 91 | self.temp_cool_mi_n = ca.SX.sym('temp_cool_mi_n') 92 | self.temp_cool_mi_s = self.pars["temp_inv_max"] - 30 93 | self.temp_cool_mi = self.temp_cool_mi_s * self.temp_cool_mi_n 94 | 95 | # cooling liquid temperature for battery circuit [°C] 96 | self.temp_cool_b_n = ca.SX.sym('temp_cool_b_n') 97 | self.temp_cool_b_s = self.pars["temp_batt_max"] - 10 98 | self.temp_cool_b = self.temp_cool_b_s * self.temp_cool_b_n 99 | 100 | self.temp_cool_mi_min = self.pars["T_env"] / self.temp_cool_mi_s 101 | self.temp_cool_mi_max = (self.pars["temp_inv_max"] - 10) / self.temp_cool_mi_s 102 | self.temp_cool_mi_guess = (self.pars["T_env"]) / self.temp_cool_mi_s 103 | 104 | self.temp_cool_b_min = self.pars["T_env"] / self.temp_cool_b_s 105 | self.temp_cool_b_max = self.pars["temp_batt_max"] / self.temp_cool_b_s 106 | self.temp_cool_b_guess = self.pars["T_env"] / self.temp_cool_b_s 107 | 108 | self.get_thermal_resistance() 109 | 110 | def get_thermal_resistance(self): 111 | """ 112 | Python version: 3.5 113 | Created by: Thomas Herrmann 114 | Created on: 01.04.2020 115 | 116 | Documentation: Calculates thermal resistance of single radiator to be used within a 117 | lumped thermal model description 118 | """ 119 | 120 | self.r_rad = 1 / (self.pars["h_air"] * self.pars["A_cool_rad"]) 121 | 122 | def get_intermediate_temps(self, 123 | temp_inv: ca.SX, 124 | r_inv: float): 125 | """ 126 | Python version: 3.5 127 | Created by: Thomas Herrmann 128 | Created on: 01.04.2020 129 | 130 | Documentation: Returns intermediate temps motor inverter necessary for thermodynamical modelling 131 | (motor + inverter circuit) 132 | 133 | Inputs: 134 | temp_inv: inverter temperature [°C] 135 | r_inv: inverter thermal resistance [K/W] 136 | """ 137 | 138 | self.temp_cool_12 = \ 139 | (self.temp_cool_mi * (self.pars["c_heat_fluid"] * self.pars["flow_rate_inv"] * r_inv - 1) 140 | + 2 * temp_inv) / \ 141 | (1 + self.pars["c_heat_fluid"] * self.pars["flow_rate_inv"] * r_inv) 142 | 143 | self.temp_cool_13 = \ 144 | (self.temp_cool_mi * (2 * self.pars["c_heat_fluid"] * self.pars["flow_rate_rad"] * self.r_rad + 1) - 145 | 2 * self.pars["T_env"]) / \ 146 | (-1 + 2 * self.pars["c_heat_fluid"] * self.pars["flow_rate_rad"] * self.r_rad) 147 | 148 | def get_increment_mi(self, 149 | sf: ca.SX, 150 | temp_mot: ca.SX, 151 | temp_inv: ca.SX, 152 | r_inv: float, 153 | r_machine: float): 154 | """ 155 | Python version: 3.5 156 | Created by: Thomas Herrmann 157 | Created on: 01.04.2020 158 | 159 | Documentation: Initializes temperature increment of radiator machine-inverter circuit symbolically 160 | (sf * dx/dt = dx/ds) 161 | 162 | Inputs: 163 | sf: transformation factor dt/ds 164 | temp_mot: temeprature of electric machine [°C] 165 | temp_inv: temeprature of inverter [°C] 166 | r_inv: thermal resistance of inverter [K/W] 167 | r_machine: thermal resistance of electric machine [K/W] 168 | """ 169 | 170 | self.dtemp_cool_mi = \ 171 | sf * ((self.pars["N_machines"] * ((temp_mot - (self.temp_cool_12 + self.temp_cool_13) / 2) / r_machine + 172 | (temp_inv - (self.temp_cool_mi + self.temp_cool_12) / 2) / r_inv) - 173 | ((self.temp_cool_mi + self.temp_cool_13) / 2 - self.pars["T_env"]) / self.r_rad) / 174 | (self.pars["m_therm_fluid_mi"] * self.pars["c_heat_fluid"])) 175 | 176 | def get_increment_b(self, 177 | sf: ca.SX, 178 | temp_batt: ca.SX, 179 | temp_cool_b: ca.SX, 180 | R_eq_B_inv: float): 181 | """ 182 | Python version: 3.5 183 | Created by: Thomas Herrmann 184 | Created on: 01.04.2020 185 | 186 | Documentation: Initializes temperature increment of radiator in battery circuit symbolically 187 | (sf * dx/dt = dx/ds) 188 | 189 | Inputs: 190 | sf: transformation factor dt/ds 191 | temp_batt: temeprature of battery [°C] 192 | temp_cool_b: temeprature of cooling liquid battery [°C] 193 | R_eq_B_inv: inverse of thermal resistance of battery [W/K] 194 | """ 195 | 196 | self.dtemp_cool_b = \ 197 | sf * ((R_eq_B_inv * (temp_batt - temp_cool_b) - 198 | (temp_cool_b - self.pars["T_env"]) / self.r_rad) / 199 | (self.pars["m_therm_fluid_b"] * self.pars["c_heat_fluid"])) 200 | 201 | def ini_nlp_state(self, 202 | x: ca.SX, 203 | u: ca.SX): 204 | """ 205 | Python version: 3.5 206 | Created by: Thomas Herrmann 207 | Created on: 01.04.2020 208 | 209 | Documentation: Defines function to define radiators' states in NLP 210 | 211 | Inputs: 212 | x: discrete NLP state 213 | u: discrete NLP control input 214 | """ 215 | 216 | self.f_nlp = \ 217 | ca.Function('f_nlp', 218 | [x, u], [self.temp_cool_mi, self.temp_cool_b], 219 | ['x', 'u'], ['temp_cool_mi', 'temp_cool_b']) 220 | 221 | def extract_sol(self, 222 | w: ca.SX, 223 | sol_states: ca.DM): 224 | """ 225 | Python version: 3.5 226 | Created by: Thomas Herrmann 227 | Created on: 01.04.2020 228 | 229 | Documentation: Defines function to retrieve values of optimized NLP radiators 230 | 231 | Inputs: 232 | w: discrete optimized NLP decision variables (x and u) 233 | sol_states: numeric values belonging to the symbolic NLP decision variables w 234 | """ 235 | 236 | self.f_sol = \ 237 | ca.Function('f_sol', 238 | [w], [self.temps_opt], 239 | ['w'], ['temps_opt']) 240 | 241 | # Overwrite lists with optimized numeric values 242 | temps_opt = self.f_sol(sol_states) 243 | 244 | self.temp_cool_mi = temps_opt[0::2] 245 | self.temp_cool_b = temps_opt[1::2] 246 | 247 | 248 | if __name__ == "__main__": 249 | pass 250 | -------------------------------------------------------------------------------- /params/f110.ini: -------------------------------------------------------------------------------- 1 | # As a general rule of thumb when the values are unknown, I set them to 1/10 of the `racecar.ini` parameters 2 | # ---------------------------------------------------------------------------------------------------------------------- 3 | [GENERAL_OPTIONS] 4 | 5 | ### set name of ggv diagram and ax_max_machines files to use 6 | ggv_file="ggv.csv" 7 | ax_max_machines_file="ax_max_machines.csv" 8 | 9 | ### stepsize options 10 | # stepsize_prep: [m] used for linear interpolation before spline approximation 11 | # stepsize_reg: [m] used for spline interpolation after spline approximation (stepsize during opt.) 12 | # stepsize_interp_after_opt: [m] used for spline interpolation after optimization 13 | 14 | # TODO: Modify these if solver too slow 15 | # 0.25, 0.75, 0.5 16 | #0.5, 1.5, 1.0 17 | stepsize_opts={"stepsize_prep": 0.1, 18 | "stepsize_reg": 0.3, 19 | "stepsize_interp_after_opt": 0.2} 20 | 21 | ### spline regression smooth options 22 | # k_reg: [-] order of B-Splines -> standard: 3 23 | # s_reg: [-] smoothing factor, range [1.0, 100.0] 24 | 25 | reg_smooth_opts={"k_reg": 3, 26 | "s_reg": 10} 27 | 28 | ### preview and review distances for numerical curvature calculation (used in minimum time optimization) 29 | # d_preview_curv: [m] preview distance (curvature) 30 | # d_review_curv: [m] review distance (curvature) 31 | # d_preview_head: [m] preview distance (heading) 32 | # d_review_head: [m] review distance (heading) 33 | 34 | curv_calc_opts = {"d_preview_curv": 0.2, 35 | "d_review_curv": 0.2, 36 | "d_preview_head": 0.1, 37 | "d_review_head": 0.1} 38 | 39 | ### general vehicle parameters required in several functions 40 | # v_max: [m/s] maximal vehicle speed 41 | # length: [m] vehicle length 42 | # width: [m] vehicle width 43 | # mass: [kg] vehicle mass 44 | # dragcoeff: [kg*m2/m3] drag coefficient calculated by 0.5 * rho_air * c_w * A_front TODO: 45 | # curvlim: [rad/m] curvature limit of the vehicle 46 | # g: [N/kg] gravity acceleration 47 | 48 | # See https://github.com/jainachin/bayesrace/blob/master/bayes_race/params/f110.py 49 | veh_params = {"v_max": 15.0, 50 | "length": 0.568, 51 | "width": 0.296, 52 | "mass": 3.74, 53 | "dragcoeff": 0.075, 54 | "curvlim": 3.0, 55 | "g": 9.81} 56 | 57 | ### velocity profile calculation options 58 | # dyn_model_exp: [-] exponent used in the vehicle dynamics model (range [1.0, 2.0]) 59 | # vel_profile_conv_filt_window: [-] moving average filter window size for velocity profile (set null if not used) 60 | 61 | vel_calc_opts = {"dyn_model_exp": 1.0, 62 | "vel_profile_conv_filt_window": null} 63 | 64 | # ---------------------------------------------------------------------------------------------------------------------- 65 | [OPTIMIZATION_OPTIONS] 66 | 67 | ### optimization problem options (shortest path optimization) ---------------------------------------------------------- 68 | # width_opt: [m] vehicle width for optimization including safety distance 69 | 70 | optim_opts_shortest_path={"width_opt": 0.34} 71 | 72 | ### optimization problem options (minimum curvature optimization) ------------------------------------------------------ 73 | # width_opt: [m] vehicle width for optimization including safety distance 74 | # iqp_iters_min: [-] minimum number of iterations for the IQP 75 | # iqp_curverror_allowed: [rad/m] maximum allowed curvature error for the IQP 76 | 77 | optim_opts_mincurv={"width_opt": 0.4, 78 | "iqp_iters_min": 3, 79 | "iqp_curverror_allowed": 0.01} 80 | 81 | ### optimization problem options (minimum lap time optimization) ------------------------------------------------------- 82 | # width_opt: [m] vehicle width for optimization including safety distance 83 | # penalty_delta: [-] penalty of delta derivative for improved smoothness of controls (range [0.0, 50.0]) 84 | # penalty_F: [-] penalty of F derivative for improved smoothness of controls (range [0.0, 2.0]) 85 | # mue: [-] constant friction coefficient (determines tire's D parameter of MF by D = F_z * mue) 86 | # n_gauss [-] number of gaussian basis functions on each side (var_friction: "gauss") 87 | # dn [m] distance of equidistant points on normal vectors for extracting friction 88 | # coefficients (var_friction: "linear" or "gauss") 89 | # limit_energy: [true/false] limit energy consumption 90 | # energy_limit: [kWh/lap] energy consumption limit (limit_energy: true) 91 | # safe_traj: [true/false] safe trajectories -> limit accelerations 92 | # ax_pos_safe: [m/s2] a_x+ limit for safe trajectories (safe_traj: true) -> null if ggv should be used 93 | # ax_neg_safe: [m/s2] a_x- limit for safe trajectories (safe_traj: true) -> null if ggv should be used 94 | # ay_safe: [m/s2] a_y limit for safe trajectories (safe_traj: true) -> null if ggv should be used 95 | # w_tr_reopt: [m] total track width in case of reoptimization using the IQP 96 | # w_veh_reopt: [m] vehicle width in case of reoptimization using the IQP 97 | # w_add_spl_regr: [m] width added in case of reoptimization to compensate second spline regression 98 | # step_non_reg: [-] defines how many points to be skipped per step during non-regular point sampling 99 | # (dependent on curvature) 100 | # eps_kappa: [rad/m] curvature threshold to skip discretization points on straights (if 101 | # step_non_reg > 0) 102 | 103 | optim_opts_mintime={"width_opt": 0.9, 104 | "penalty_delta": 10.0, 105 | "penalty_F": 0.01, 106 | "mue": 0.3, 107 | "n_gauss": 5, 108 | "dn": 0.25, 109 | "limit_energy": false, 110 | "energy_limit": 2.0, 111 | "safe_traj": false, 112 | "ax_pos_safe": null, 113 | "ax_neg_safe": null, 114 | "ay_safe": null, 115 | "w_tr_reopt": 1.0, 116 | "w_veh_reopt": 0.4, 117 | "w_add_spl_regr": 0.2, 118 | "step_non_reg": 0, 119 | "eps_kappa": 1e-3} 120 | 121 | ### vehicle parameters (minimum lap time optimization) 122 | # wheelbase_front: [m] wheelbase front 123 | # wheelbase_rear: [m] wheelbase rear 124 | # track_width_front: [m] track width front 125 | # track_width_rear: [m] track width rear 126 | # cog_z: [m] center of gravity 127 | # I_z: [kgm^2] yaw inertia 128 | # liftcoeff_front: [kg*m2/m3] lift coefficient front axle calculated by 0.5 * rho_air * c_l_f * A_spoiler_f 129 | # liftcoeff_rear: [kg*m2/m3] lift coefficient rear axle calculated by 0.5 * rho_air * c_l_r * A_spoiler_r 130 | # k_brake_front: [-] portion of braking force at the front axle of the total braking force 131 | # k_drive_front: [-] portion of driving force at the front axle of the total driving force 132 | # k_roll: [-] portion of roll moment at the front axle of the total roll moment 133 | # t_delta: [s] time constant for steering dynamic 134 | # t_drive: [s] time constant for acceleration dynamic 135 | # t_brake: [s] time constant for braking dynamic 136 | # power_max: [W] maximal engine power #TODO: 137 | # f_drive_max: [N] maximal drive force #TODO: 138 | # f_brake_max: [N] maximal brake force (only needed for actor dynamics) TODO: 139 | # delta_max: [rad] maximal steer angle 140 | 141 | vehicle_params_mintime = {"wheelbase_front": 0.275, 142 | "wheelbase_rear": 0.275, 143 | "track_width_front": 0.296, 144 | "track_width_rear": 0.296, 145 | "cog_z": 0.074, 146 | "I_z": 0.04712, 147 | "liftcoeff_front": 0.045, 148 | "liftcoeff_rear": 0.075, 149 | "k_brake_front": 0.6, 150 | "k_drive_front": 0.5, 151 | "k_roll": 0.5, 152 | "t_delta": 0.2, 153 | "t_drive": 0.1, 154 | "t_brake": 0.1, 155 | "power_max": 23000.0, 156 | "f_drive_max": 700.0, 157 | "f_brake_max": 2000.0, 158 | "delta_max": 0.4189} 159 | 160 | ### tire parameters (minimum lap time optimization) 161 | # c_roll: [-] rolling resistance coefficient 162 | # f_z0: [N] nominal normal force 163 | # B_front: [-] Coefficient B for front tire 164 | # C_front: [-] Coefficient C for front tire 165 | # eps_front: [-] load dependence of Coefficient D for front tire 166 | # E_front: [-] Coefficient E for front tire 167 | # B_rear: [-] Coefficient B for rear tire 168 | # C_rear: [-] Coefficient C for rear tire 169 | # eps_rear: [-] load dependence of Coefficient D for rear tire 170 | # E_rear: [-] Coefficient E for rear tire 171 | # Hint: The D parameter of the Magic Formula is determined by D = F_z * mue. mue can be set above in optim_opts_mintime! 172 | 173 | tire_params_mintime = {"c_roll": 0.013, 174 | "f_z0": 300.0, 175 | "B_front": 10.0, 176 | "C_front": 2.5, 177 | "eps_front": -0.1, 178 | "E_front": 1.0, 179 | "B_rear": 10.0, 180 | "C_rear": 2.5, 181 | "eps_rear": -0.1, 182 | "E_rear": 1.0} 183 | 184 | ### powertrain behavior (minimum lap time optimization) 185 | pwr_params_mintime = {"pwr_behavior": false} 186 | -------------------------------------------------------------------------------- /inputs/tracks/handling_track.csv: -------------------------------------------------------------------------------- 1 | # x_m,y_m,w_tr_right_m,w_tr_left_m 2 | 0,0,5,5 3 | 0,1.2489,5,5 4 | 0,2.4978,5,5 5 | 0,3.7467,5,5 6 | 0,4.9956,5,5 7 | 0,6.2445,5,5 8 | 0,7.4934,5,5 9 | 0,8.7423,5,5 10 | 0,9.9912,5,5 11 | 0,11.24,5,5 12 | 0,12.489,5,5 13 | 0,13.738,5,5 14 | 0,14.987,5,5 15 | 0,16.236,5,5 16 | 0,17.485,5,5 17 | 0,18.733,5,5 18 | 0,19.982,5,5 19 | 0,21.231,5,5 20 | 0,22.48,5,5 21 | 0,23.729,5,5 22 | 0,24.978,5,5 23 | 0,26.227,5,5 24 | 0,27.476,5,5 25 | 0,28.725,5,5 26 | 0,29.974,5,5 27 | 0,31.222,5,5 28 | 0,32.471,5,5 29 | 0,33.72,5,5 30 | 0,34.969,5,5 31 | 0,36.218,5,5 32 | 0,37.467,5,5 33 | 0,38.716,5,5 34 | 0,39.965,5,5 35 | 0,41.214,5,5 36 | 0,42.463,5,5 37 | 0,43.711,5,5 38 | 0,44.96,5,5 39 | 0,46.209,5,5 40 | 0,47.458,5,5 41 | 0,48.707,5,5 42 | 0,49.956,5,5 43 | 0,51.205,5,5 44 | 0,52.454,5,5 45 | 0,53.703,5,5 46 | 0,54.952,5,5 47 | 0,56.2,5,5 48 | 0,57.449,5,5 49 | 0,58.698,5,5 50 | 0,59.947,5,5 51 | 0,61.196,5,5 52 | 0,62.445,5,5 53 | 0,63.694,5,5 54 | 0,64.943,5,5 55 | 0,66.192,5,5 56 | 0,67.44,5,5 57 | 0,68.689,5,5 58 | 0,69.938,5,5 59 | 0,71.187,5,5 60 | 0,72.436,5,5 61 | 0,73.685,5,5 62 | 0,74.934,5,5 63 | 0,76.183,5,5 64 | 0,77.432,5,5 65 | 0,78.681,5,5 66 | 0,79.929,5,5 67 | -0.0014451,81.178,5,5 68 | -0.0090217,82.427,5,5 69 | -0.029217,83.676,5,5 70 | -0.068518,84.924,5,5 71 | -0.1334,86.171,5,5 72 | -0.23031,87.416,5,5 73 | -0.36563,88.658,5,5 74 | -0.54566,89.893,5,5 75 | -0.77654,91.12,5,5 76 | -1.0642,92.335,5,5 77 | -1.4142,93.534,5,5 78 | -1.8318,94.71,5,5 79 | -2.3215,95.858,5,5 80 | -2.8873,96.971,5,5 81 | -3.5321,98.039,5,5 82 | -4.2579,99.054,5,5 83 | -5.0651,100.01,5,5 84 | -5.9527,100.88,5,5 85 | -6.9181,101.67,5,5 86 | -7.9564,102.36,5,5 87 | -9.0604,102.94,5,5 88 | -10.221,103.4,5,5 89 | -11.425,103.72,5,5 90 | -12.66,103.9,5,5 91 | -13.906,103.92,5,5 92 | -15.147,103.8,5,5 93 | -16.367,103.54,5,5 94 | -17.555,103.16,5,5 95 | -18.702,102.67,5,5 96 | -19.803,102.08,5,5 97 | -20.854,101.41,5,5 98 | -21.855,100.66,5,5 99 | -22.807,99.857,5,5 100 | -23.713,98.998,5,5 101 | -24.577,98.097,5,5 102 | -25.405,97.162,5,5 103 | -26.202,96.201,5,5 104 | -26.977,95.221,5,5 105 | -27.735,94.229,5,5 106 | -28.485,93.23,5,5 107 | -29.232,92.23,5,5 108 | -29.98,91.229,5,5 109 | -30.727,90.229,5,5 110 | -31.474,89.228,5,5 111 | -32.224,88.229,5,5 112 | -32.982,87.236,5,5 113 | -33.757,86.257,5,5 114 | -34.558,85.3,5,5 115 | -35.394,84.372,5,5 116 | -36.272,83.484,5,5 117 | -37.197,82.646,5,5 118 | -38.174,81.87,5,5 119 | -39.205,81.166,5,5 120 | -40.285,80.541,5,5 121 | -41.409,79.998,5,5 122 | -42.571,79.542,5,5 123 | -43.763,79.173,5,5 124 | -44.98,78.895,5,5 125 | -46.214,78.709,5,5 126 | -47.458,78.615,5,5 127 | -48.706,78.615,5,5 128 | -49.951,78.709,5,5 129 | -51.184,78.895,5,5 130 | -52.401,79.174,5,5 131 | -53.594,79.542,5,5 132 | -54.755,79.999,5,5 133 | -55.879,80.542,5,5 134 | -56.959,81.167,5,5 135 | -57.99,81.871,5,5 136 | -58.964,82.65,5,5 137 | -59.878,83.5,5,5 138 | -60.725,84.417,5,5 139 | -61.503,85.394,5,5 140 | -62.203,86.426,5,5 141 | -62.825,87.508,5,5 142 | -63.364,88.634,5,5 143 | -63.82,89.796,5,5 144 | -64.195,90.986,5,5 145 | -64.501,92.197,5,5 146 | -64.742,93.422,5,5 147 | -64.932,94.656,5,5 148 | -65.081,95.896,5,5 149 | -65.195,97.139,5,5 150 | -65.288,98.385,5,5 151 | -65.369,99.631,5,5 152 | -65.449,100.88,5,5 153 | -65.538,102.12,5,5 154 | -65.647,103.37,5,5 155 | -65.785,104.61,5,5 156 | -65.962,105.84,5,5 157 | -66.19,107.07,5,5 158 | -66.478,108.29,5,5 159 | -66.834,109.48,5,5 160 | -67.268,110.65,5,5 161 | -67.785,111.79,5,5 162 | -68.397,112.88,5,5 163 | -69.102,113.91,5,5 164 | -69.905,114.86,5,5 165 | -70.805,115.72,5,5 166 | -71.799,116.48,5,5 167 | -72.877,117.1,5,5 168 | -74.024,117.59,5,5 169 | -75.223,117.93,5,5 170 | -76.455,118.12,5,5 171 | -77.701,118.15,5,5 172 | -78.942,118.03,5,5 173 | -80.157,117.75,5,5 174 | -81.329,117.33,5,5 175 | -82.439,116.76,5,5 176 | -83.475,116.07,5,5 177 | -84.419,115.25,5,5 178 | -85.268,114.34,5,5 179 | -86.014,113.34,5,5 180 | -86.653,112.27,5,5 181 | -87.184,111.14,5,5 182 | -87.605,109.97,5,5 183 | -87.92,108.76,5,5 184 | -88.13,107.53,5,5 185 | -88.24,106.29,5,5 186 | -88.255,105.04,5,5 187 | -88.18,103.79,5,5 188 | -88.021,102.55,5,5 189 | -87.784,101.33,5,5 190 | -87.475,100.12,5,5 191 | -87.101,98.927,5,5 192 | -86.669,97.756,5,5 193 | -86.183,96.606,5,5 194 | -85.651,95.476,5,5 195 | -85.078,94.367,5,5 196 | -84.471,93.276,5,5 197 | -83.833,92.202,5,5 198 | -83.17,91.143,5,5 199 | -82.488,90.097,5,5 200 | -81.791,89.061,5,5 201 | -81.085,88.031,5,5 202 | -80.372,87.005,5,5 203 | -79.659,85.981,5,5 204 | -78.945,84.956,5,5 205 | -78.231,83.931,5,5 206 | -77.517,82.906,5,5 207 | -76.803,81.881,5,5 208 | -76.09,80.857,5,5 209 | -75.376,79.832,5,5 210 | -74.662,78.807,5,5 211 | -73.948,77.782,5,5 212 | -73.234,76.757,5,5 213 | -72.52,75.733,5,5 214 | -71.807,74.708,5,5 215 | -71.093,73.684,5,5 216 | -70.379,72.658,5,5 217 | -69.665,71.633,5,5 218 | -68.951,70.609,5,5 219 | -68.237,69.584,5,5 220 | -67.524,68.559,5,5 221 | -66.81,67.535,5,5 222 | -66.096,66.509,5,5 223 | -65.382,65.485,5,5 224 | -64.668,64.46,5,5 225 | -63.955,63.435,5,5 226 | -63.241,62.41,5,5 227 | -62.527,61.385,5,5 228 | -61.813,60.361,5,5 229 | -61.099,59.336,5,5 230 | -60.385,58.311,5,5 231 | -59.672,57.286,5,5 232 | -58.958,56.261,5,5 233 | -58.244,55.237,5,5 234 | -57.53,54.212,5,5 235 | -56.816,53.187,5,5 236 | -56.103,52.162,5,5 237 | -55.389,51.138,5,5 238 | -54.675,50.113,5,5 239 | -53.961,49.088,5,5 240 | -53.247,48.063,5,5 241 | -52.533,47.038,5,5 242 | -51.82,46.014,5,5 243 | -51.106,44.989,5,5 244 | -50.392,43.964,5,5 245 | -49.678,42.939,5,5 246 | -48.964,41.914,5,5 247 | -48.25,40.89,5,5 248 | -47.537,39.865,5,5 249 | -46.823,38.84,5,5 250 | -46.109,37.815,5,5 251 | -45.395,36.79,5,5 252 | -44.684,35.764,5,5 253 | -43.975,34.735,5,5 254 | -43.273,33.703,5,5 255 | -42.577,32.666,5,5 256 | -41.892,31.622,5,5 257 | -41.218,30.57,5,5 258 | -40.559,29.51,5,5 259 | -39.915,28.439,5,5 260 | -39.29,27.358,5,5 261 | -38.686,26.265,5,5 262 | -38.104,25.16,5,5 263 | -37.549,24.041,5,5 264 | -37.021,22.91,5,5 265 | -36.524,21.764,5,5 266 | -36.06,20.605,5,5 267 | -35.632,19.432,5,5 268 | -35.241,18.246,5,5 269 | -34.886,17.048,5,5 270 | -34.57,15.84,5,5 271 | -34.291,14.623,5,5 272 | -34.05,13.398,5,5 273 | -33.848,12.166,5,5 274 | -33.684,10.928,5,5 275 | -33.559,9.6852,5,5 276 | -33.473,8.4394,5,5 277 | -33.426,7.1915,5,5 278 | -33.417,5.9428,5,5 279 | -33.448,4.6945,5,5 280 | -33.518,3.4476,5,5 281 | -33.626,2.2036,5,5 282 | -33.774,0.96361,5,5 283 | -33.96,-0.27121,5,5 284 | -34.184,-1.4996,5,5 285 | -34.447,-2.7204,5,5 286 | -34.747,-3.9324,5,5 287 | -35.086,-5.1345,5,5 288 | -35.462,-6.3254,5,5 289 | -35.874,-7.5039,5,5 290 | -36.324,-8.6691,5,5 291 | -36.809,-9.8196,5,5 292 | -37.33,-10.954,5,5 293 | -37.886,-12.072,5,5 294 | -38.477,-13.172,5,5 295 | -39.102,-14.254,5,5 296 | -39.761,-15.315,5,5 297 | -40.452,-16.355,5,5 298 | -41.175,-17.373,5,5 299 | -41.931,-18.367,5,5 300 | -42.721,-19.333,5,5 301 | -43.551,-20.266,5,5 302 | -44.424,-21.159,5,5 303 | -45.343,-22.004,5,5 304 | -46.31,-22.794,5,5 305 | -47.326,-23.519,5,5 306 | -48.39,-24.17,5,5 307 | -49.501,-24.738,5,5 308 | -50.655,-25.214,5,5 309 | -51.846,-25.584,5,5 310 | -53.067,-25.842,5,5 311 | -54.307,-25.977,5,5 312 | -55.554,-25.98,5,5 313 | -56.793,-25.845,5,5 314 | -58.008,-25.565,5,5 315 | -59.18,-25.14,5,5 316 | -60.293,-24.578,5,5 317 | -61.337,-23.897,5,5 318 | -62.306,-23.111,5,5 319 | -63.195,-22.235,5,5 320 | -64.003,-21.285,5,5 321 | -64.733,-20.272,5,5 322 | -65.388,-19.21,5,5 323 | -65.973,-18.107,5,5 324 | -66.495,-16.972,5,5 325 | -66.959,-15.814,5,5 326 | -67.374,-14.637,5,5 327 | -67.753,-13.446,5,5 328 | -68.1,-12.246,5,5 329 | -68.425,-11.041,5,5 330 | -68.738,-9.8317,5,5 331 | -69.049,-8.622,5,5 332 | -69.366,-7.414,5,5 333 | -69.697,-6.2104,5,5 334 | -70.057,-5.0142,5,5 335 | -70.45,-3.8289,5,5 336 | -70.886,-2.6589,5,5 337 | -71.374,-1.5096,5,5 338 | -71.921,-0.38748,5,5 339 | -72.535,0.69958,5,5 340 | -73.221,1.7423,5,5 341 | -73.984,2.7301,5,5 342 | -74.825,3.6506,5,5 343 | -75.748,4.4903,5,5 344 | -76.749,5.2345,5,5 345 | -77.824,5.8675,5,5 346 | -78.963,6.3732,5,5 347 | -80.155,6.7373,5,5 348 | -81.383,6.9531,5,5 349 | -82.628,7.0209,5,5 350 | -83.872,6.9436,5,5 351 | -85.1,6.7259,5,5 352 | -86.297,6.3745,5,5 353 | -87.449,5.8971,5,5 354 | -88.545,5.3027,5,5 355 | -89.576,4.6008,5,5 356 | -90.534,3.8015,5,5 357 | -91.412,2.9152,5,5 358 | -92.205,1.9523,5,5 359 | -92.911,0.9231,5,5 360 | -93.526,-0.16255,5,5 361 | -94.05,-1.2951,5,5 362 | -94.483,-2.4655,5,5 363 | -94.826,-3.6655,5,5 364 | -95.08,-4.8873,5,5 365 | -95.25,-6.124,5,5 366 | -95.336,-7.3693,5,5 367 | -95.343,-8.6175,5,5 368 | -95.276,-9.864,5,5 369 | -95.137,-11.105,5,5 370 | -94.93,-12.336,5,5 371 | -94.662,-13.555,5,5 372 | -94.336,-14.76,5,5 373 | -93.955,-15.95,5,5 374 | -93.526,-17.122,5,5 375 | -93.052,-18.278,5,5 376 | -92.538,-19.415,5,5 377 | -91.987,-20.536,5,5 378 | -91.403,-21.64,5,5 379 | -90.791,-22.729,5,5 380 | -90.154,-23.803,5,5 381 | -89.496,-24.864,5,5 382 | -88.821,-25.915,5,5 383 | -88.131,-26.956,5,5 384 | -87.431,-27.99,5,5 385 | -86.723,-29.019,5,5 386 | -86.01,-30.045,5,5 387 | -85.297,-31.069,5,5 388 | -84.583,-32.094,5,5 389 | -83.869,-33.119,5,5 390 | -83.155,-34.144,5,5 391 | -82.442,-35.169,5,5 392 | -81.727,-36.193,5,5 393 | -81.014,-37.218,5,5 394 | -80.3,-38.243,5,5 395 | -79.586,-39.268,5,5 396 | -78.872,-40.293,5,5 397 | -78.158,-41.317,5,5 398 | -77.445,-42.342,5,5 399 | -76.731,-43.367,5,5 400 | -76.017,-44.392,5,5 401 | -75.303,-45.417,5,5 402 | -74.589,-46.441,5,5 403 | -73.876,-47.466,5,5 404 | -73.161,-48.491,5,5 405 | -72.448,-49.516,5,5 406 | -71.734,-50.541,5,5 407 | -71.02,-51.565,5,5 408 | -70.306,-52.59,5,5 409 | -69.593,-53.615,5,5 410 | -68.879,-54.64,5,5 411 | -68.165,-55.664,5,5 412 | -67.451,-56.689,5,5 413 | -66.737,-57.714,5,5 414 | -66.023,-58.739,5,5 415 | -65.309,-59.763,5,5 416 | -64.588,-60.783,5,5 417 | -63.855,-61.793,5,5 418 | -63.095,-62.785,5,5 419 | -62.307,-63.752,5,5 420 | -61.48,-64.689,5,5 421 | -60.608,-65.583,5,5 422 | -59.687,-66.425,5,5 423 | -58.712,-67.205,5,5 424 | -57.682,-67.909,5,5 425 | -56.597,-68.525,5,5 426 | -55.46,-69.038,5,5 427 | -54.277,-69.434,5,5 428 | -53.059,-69.698,5,5 429 | -51.818,-69.817,5,5 430 | -50.572,-69.786,5,5 431 | -49.337,-69.612,5,5 432 | -48.128,-69.306,5,5 433 | -46.955,-68.881,5,5 434 | -45.826,-68.35,5,5 435 | -44.748,-67.726,5,5 436 | -43.715,-67.021,5,5 437 | -42.736,-66.247,5,5 438 | -41.805,-65.415,5,5 439 | -40.919,-64.535,5,5 440 | -40.074,-63.616,5,5 441 | -39.264,-62.666,5,5 442 | -38.483,-61.692,5,5 443 | -37.722,-60.701,5,5 444 | -36.976,-59.7,5,5 445 | -36.235,-58.694,5,5 446 | -35.496,-57.687,5,5 447 | -34.758,-56.68,5,5 448 | -34.019,-55.673,5,5 449 | -33.28,-54.667,5,5 450 | -32.541,-53.66,5,5 451 | -31.802,-52.653,5,5 452 | -31.063,-51.646,5,5 453 | -30.324,-50.64,5,5 454 | -29.585,-49.633,5,5 455 | -28.846,-48.626,5,5 456 | -28.107,-47.619,5,5 457 | -27.368,-46.612,5,5 458 | -26.629,-45.606,5,5 459 | -25.89,-44.599,5,5 460 | -25.151,-43.592,5,5 461 | -24.412,-42.585,5,5 462 | -23.673,-41.578,5,5 463 | -22.934,-40.572,5,5 464 | -22.195,-39.565,5,5 465 | -21.456,-38.558,5,5 466 | -20.717,-37.551,5,5 467 | -19.978,-36.544,5,5 468 | -19.239,-35.538,5,5 469 | -18.5,-34.531,5,5 470 | -17.761,-33.524,5,5 471 | -17.022,-32.517,5,5 472 | -16.283,-31.51,5,5 473 | -15.544,-30.504,5,5 474 | -14.805,-29.497,5,5 475 | -14.066,-28.49,5,5 476 | -13.327,-27.483,5,5 477 | -12.588,-26.476,5,5 478 | -11.849,-25.47,5,5 479 | -11.11,-24.463,5,5 480 | -10.371,-23.456,5,5 481 | -9.6318,-22.449,5,5 482 | -8.8928,-21.442,5,5 483 | -8.1538,-20.436,5,5 484 | -7.4148,-19.429,5,5 485 | -6.6758,-18.422,5,5 486 | -5.9368,-17.415,5,5 487 | -5.1996,-16.407,5,5 488 | -4.4706,-15.393,5,5 489 | -3.7625,-14.365,5,5 490 | -3.0848,-13.316,5,5 491 | -2.4501,-12.24,5,5 492 | -1.8713,-11.134,5,5 493 | -1.3617,-9.9949,5,5 494 | -0.93587,-8.8219,5,5 495 | -0.60407,-7.6189,5,5 496 | -0.3628,-6.3943,5,5 497 | -0.19832,-5.1568,5,5 498 | -0.095828,-3.9124,5,5 499 | -0.039979,-2.6649,5,5 500 | -0.015275,-1.4163,5,5 501 | -0.0061524,-0.16743,5,5 502 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # Introduction 2 | This repository contains algorithms that allow us to determine an optimal racing line on a race track. You can chose 3 | between several objectives: 4 | * Shortest path 5 | * Minimum curvature (with or without iterative call) 6 | * Minimum time 7 | * Minimum time with powertrain behavior consideration 8 | 9 | The minimum curvature line is quite near to a minimum time line in corners but will differ as soon as the car's 10 | acceleration limits are not exploited. However, the minimum time optimization requires a lot more parameters and takes 11 | more computation time. Please look into the `main_globaltraj.py` for all possible options. 12 | 13 | # List of components 14 | * `frictionmap`: This package contains the functions related to the creation and handling of friction maps along the 15 | race track. 16 | * `helper_funcs_glob`: This package contains some helper functions used in several other functions when 17 | calculating the global race trajectory. 18 | * `inputs`: This folder contains the vehicle dynamics information, the reference track csvs and friction maps. 19 | * `opt_mintime_traj`: This package contains the functions required to find the time-optimal trajectory. 20 | 21 | It also includes the powertrain components in `opt_mintime_traj/powertrain_src` used to calculate power losses and 22 | thermal behavior of the powertrain and to consider the state of charge of the battery. 23 | * `params`: This folder contains a parameter file with optimization and vehicle parameters. 24 | 25 | # Getting Started 26 | 27 | First, clone the repository 28 | ```bash 29 | git clone https://github.com/CL2-UWaterloo/Raceline-Optimization.git 30 | cd Raceline-Optimization 31 | ``` 32 | 33 | Then, set up your virtual environment. Conda is the recommended method. 34 | 35 | ```bash 36 | conda create --name raceline python=3.8 37 | conda activate raceline 38 | ``` 39 | 40 | Lots of the required functions for trajectory planning are cumulated in the trajectory planning helpers repository. 41 | 42 | **Note**: A fork of the trajectory planning helper repository is used in the `requirements.txt` file instead of the original repository due to issues with outdated `quadprog` library. 43 | 44 | Install the required python packages 45 | ```bash 46 | pip install -r requirements.txt 47 | ``` 48 | 49 | # Steps 50 | 51 | ### For `slam_toolbox` 52 | If you generated the map through `slam_toolbox`, consult https://stevengong.co/notes/Raceline-Optimization. 53 | You might need to photoshop the map first to remove any artifacts and have clear track boundaries. 54 | 55 | First, run `map_converter.ipynb`, and then run `sanity_check.ipynb` to make sure the line generated is correct. 56 | 57 | This will export a `.csv` file of the map to `inputs/tracks`. 58 | 59 | ### For Waypoint Generator 60 | Alternatively, if you generated the map by storing a set of waypoints, you can directly store it inside `inputs/tracks`. 61 | 62 | ### Common steps 63 | Then, run `main_globaltraj_f110.py` with the map name to generate the trajectory (expects the map to be inside `inputs/tracks`). By default, the generated raceline will be stored inside `outputs/`. 64 | 65 | ```bash 66 | python3 main_globaltraj_f110.py --map_name e7_floor5_square 67 | ``` 68 | 69 | Optionally, you can also specify a `--map_path` and `--export_path`: 70 | 71 | ```bash 72 | python3 main_globaltraj_f110.py --map_name e7_floor5_square --map_path ~/workspaces/racetracks/e7_floor5_square.csv --export_path /tmp/traj_race_cl.csv 73 | ``` 74 | 75 | The code is developed with Ubuntu 20.04 LTS and Python 3.8. 76 | 77 | ### Solutions for possible installation problems (Windows) 78 | * `cvxpy`, `cython` or any other package requires a `Visual C++ compiler` -> Download the build tools for Visual Studio 79 | 2019 (https://visualstudio.microsoft.com/de/downloads/ -> tools for Visual Studio 2019 -> build tools), install them and 80 | chose the `C++ build tools` option to install the required C++ compiler and its dependencies 81 | * Problems with quadprog -> reason currently not clear, test using quadprog in version 0.1.6 instead 0.1.7 82 | 83 | ### Solutions for possible installation problems (Ubuntu) 84 | * `matplotlib` requires `tkinter` -> can be solved by `sudo apt install python3-tk` 85 | * `Python.h` required `quadprog` -> can be solved by `sudo apt install python3-dev` 86 | 87 | # Creating your own friction map 88 | The script `main_gen_frictionmap.py` can be used to create your own friction map for any race track file supplied in the 89 | input folder. The resulting maps are stored in the `inputs/frictionmaps` folder. These friction maps can be used within 90 | the minimum time optimization. In principle, they can also be considered within the velocity profile calculation of the 91 | minimum curvature planner. However, this is currently not supported from our side. 92 | 93 | # Running the code 94 | * `Step 1:` (optional) Adjust the parameter file that can be found in the `params` folder (required file). 95 | * `Step 2:` (optional) Adjust the ggv diagram and ax_max_machines file in `inputs/veh_dyn_info` (if used). This 96 | acceleration should be calculated without drag resistance, i.e. simply by F_x_drivetrain / m_veh! 97 | * `Step 3:` (optional) Add your own reference track file in `inputs/tracks` (required file). 98 | * `Step 4:` (optional) Add your own friction map files in `inputs/frictionmaps` (if used). 99 | * `Step 5:` (optional) If you want to consider the powertrain behavior (thermal behavior, power loss, state of charge), 100 | enable the powertrain-option in the parameter file (`/params`) and adjust the powertrain parameters as needed. Set the 101 | number of race laps in the dict `imp_opts` in `main_globaltraj.py` and specify a non-regular discretization step length 102 | for faster optimization in the parameter file (`/params`) (if used). You can select a simple approximation of the 103 | powertrain components by setting `/params/racecar.ini:simple_loss = True` or consider more detailed models by 104 | specifying `/params/racecar.ini:simple_loss = False`. 105 | * `Step 6:` Adjust the parameters in the upper part of `main_globaltraj.py` and execute it to start the trajectory 106 | generation process. The calculated race trajectory is stored in `outputs/traj_race_cl.csv`. 107 | 108 | IMPORTANT: For further information on the minimum time optimization have a look into the according `Readme.md` which can 109 | be found in the `opt_mintime_traj` folder! 110 | 111 | ![Resulting raceline for the Berlin FE track](opt_raceline_berlin.png) 112 | 113 | # Wording and conventions 114 | We tried to keep a consistant wording for the variable names: 115 | * path -> [x, y] Describes any array containing x,y coordinates of points (i.e. point coordinates).\ 116 | * refline -> [x, y] A path that is used as reference line during our calculations.\ 117 | * reftrack -> [x, y, w_tr_right, w_tr_left] An array that contains not only the reference line information but also 118 | right and left track widths. In our case it contains the race track that is used as a basis for the raceline 119 | optimization. 120 | 121 | Our normal vectors usually point to the right in the direction of driving. Therefore, we get the track boundaries by 122 | multiplication as follows: norm_vector * w_tr_right, -norm_vector * w_tr_left. 123 | 124 | # Trajectory definition 125 | The global racetrajectory optimization currently supports two output formats: 126 | 127 | * *Race Trajectory* - (default) holds detailed information about the race trajectory. 128 | * *LTPL Trajectory* - holds source information about the race trajectory as well as information about the track bounds 129 | and tue reference line (the actual race trajectory has to be calculated based on the stored information). 130 | 131 | In order to en-/disable the export of any of these files, add the respective entry to the 'file_paths'-dict in the 132 | 'main_globtraj.py'-script (search for `# assemble export paths`). By the default, the file path for the 133 | 'LTPL Trajectory' is commented out. 134 | 135 | Details about the individual formats are given in the following. 136 | 137 | ### Race Trajectory 138 | The output csv contains the global race trajectory. The array is of size [no_points x 7] where no_points depends on 139 | stepsize and track length. The seven columns are structured as follows: 140 | 141 | * `s_m`: float32, meter. Curvi-linear distance along the raceline. 142 | * `x_m`: float32, meter. X-coordinate of raceline point. 143 | * `y_m`: float32, meter. Y-coordinate of raceline point. 144 | * `psi_rad`: float32, rad. Heading of raceline in current point from -pi to +pi rad. Zero is north (along y-axis). 145 | * `kappa_radpm`: float32, rad/meter. Curvature of raceline in current point. 146 | * `vx_mps`: float32, meter/second. Target velocity in current point. 147 | * `ax_mps2`: float32, meter/second². Target acceleration in current point. We assume this acceleration to be constant 148 | from current point until next point. 149 | 150 | ### LTPL Trajectory 151 | The output csv contains the source information of the global race trajectory and map information via the normal vectors. 152 | The array is of size [no_points x 12] where no_points depends on step size and track length. The seven columns are 153 | structured as follows: 154 | 155 | * `x_ref_m`: float32, meter. X-coordinate of reference line point (e.g. center line of the track). 156 | * `y_ref_m`: float32, meter. Y-coordinate of reference line point (e.g. center line of the track). 157 | * `width_right_m`: float32, meter. Distance between reference line point and right track bound (along normal vector). 158 | * `width_left_m`: float32, meter. Distance between reference line point and left track bound (along normal vector). 159 | * `x_normvec_m`: float32, meter. X-coordinate of the normalized normal vector based on the reference line point. 160 | * `y_normvec_m`: float32, meter. Y-coordinate of the normalized normal vector based on the reference line point. 161 | * `alpha_m`: float32, meter. Solution of the opt. problem holding the lateral shift in m for the reference line point. 162 | * `s_racetraj_m`: float32, meter. Curvi-linear distance along the race line. 163 | * `psi_racetraj_rad`: float32, rad. Heading of raceline in current point from -pi to +pi rad. Zero is north. 164 | * `kappa_racetraj_radpm`: float32, rad/meter. Curvature of raceline in current point. 165 | * `vx_racetraj_mps`: float32, meter/second. Target velocity in current point. 166 | * `ax_racetraj_mps2`: float32, meter/second². Target acceleration in current point. We assume this acceleration to be 167 | constant from current point until next point. 168 | 169 | The generated file can be directly imported by the 170 | [graph-based local trajectory planner](https://github.com/TUMFTM/GraphBasedLocalTrajectoryPlanner). 171 | 172 | 173 | # References 174 | * Minimum Curvature Trajectory Planning\ 175 | Heilmeier, Wischnewski, Hermansdorfer, Betz, Lienkamp, Lohmann\ 176 | Minimum Curvature Trajectory Planning and Control for an Autonomous Racecar\ 177 | DOI: 10.1080/00423114.2019.1631455\ 178 | Contact person: [Alexander Heilmeier](mailto:alexander.heilmeier@tum.de). 179 | 180 | * Time-Optimal Trajectory Planning\ 181 | Christ, Wischnewski, Heilmeier, Lohmann\ 182 | Time-Optimal Trajectory Planning for a Race Car Considering Variable Tire-Road Friction Coefficients\ 183 | DOI: 10.1080/00423114.2019.1704804\ 184 | Contact person: [Fabian Christ](mailto:fabian.christ@tum.de). 185 | 186 | * Friction Map Generation\ 187 | Hermansdorfer, Betz, Lienkamp\ 188 | A Concept for Estimation and Prediction of the Tire-Road Friction Potential for an Autonomous Racecar\ 189 | DOI: 10.1109/ITSC.2019.8917024\ 190 | Contact person: [Leonhard Hermansdorfer](mailto:leo.hermansdorfer@tum.de). 191 | 192 | * Powertrain Behavior\ 193 | Herrmann, Passigato, Betz, Lienkamp\ 194 | Minimum Race-Time Planning-Strategy for an Autonomous Electric Racecar\ 195 | DOI: 10.1109/ITSC45102.2020.9294681\ 196 | Preprint: https://arxiv.org/abs/2005.07127 \ 197 | Contact person: [Thomas Herrmann](mailto:thomas.herrmann@tum.de). 198 | -------------------------------------------------------------------------------- /opt_mintime_traj/powertrain_src/src/EMachine.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import numpy as np 3 | 4 | 5 | class EMachineModel: 6 | 7 | __slots__ = ('pars', 8 | 'temp_mot_n', 9 | 'temp_mot_s', 10 | 'temp_mot', 11 | 'dtemp', 12 | 'temp_min', 13 | 'temp_max', 14 | 'temp_guess', 15 | 'f_nlp', 16 | 'f_sol', 17 | 'i_eff', 18 | 'omega_machine', 19 | 'p_input', 20 | 'p_loss_copper', 21 | 'p_loss_stator_iron', 22 | 'p_loss_rotor', 23 | 'p_loss_total', 24 | 'p_loss_total_all_machines', 25 | 'r_machine', 26 | 'p_losses_opt') 27 | 28 | def __init__(self, 29 | pwr_pars: dict): 30 | """ 31 | Python version: 3.5 32 | Created by: Thomas Herrmann (thomas.herrmann@tum.de) 33 | Created on: 01.04.2020 34 | 35 | Documentation: E-Machine class for the optimization of global trajectories for electric race cars implemented in 36 | the CasADi modeling language. 37 | 38 | Inputs: 39 | pwr_pars: powertrain parameters defined in the initialization file 40 | """ 41 | 42 | # Store powertrain parameters 43 | self.pars = pwr_pars 44 | 45 | # -------------------------------------------------------------------------------------------------------------- 46 | # Empty machine states 47 | # -------------------------------------------------------------------------------------------------------------- 48 | self.temp_mot_n = None 49 | self.temp_mot_s = None 50 | self.temp_mot = None 51 | self.dtemp = None 52 | self.temp_min = None 53 | self.temp_max = None 54 | self.temp_guess = None 55 | 56 | self.f_nlp = None 57 | self.f_sol = None 58 | 59 | self.i_eff = None 60 | self.omega_machine = None 61 | 62 | self.p_input = None 63 | self.p_loss_copper = None 64 | self.p_loss_stator_iron = None 65 | self.p_loss_rotor = None 66 | self.p_loss_total = None 67 | self.p_loss_total_all_machines = None 68 | 69 | self.r_machine = None 70 | 71 | # Optimized losses list: p_loss_total, p_loss_effects 72 | self.p_losses_opt = [] 73 | 74 | # Call initialization function 75 | self.initialize() 76 | 77 | def initialize(self): 78 | """ 79 | Python version: 3.5 80 | Created by: Thomas Herrmann 81 | Created on: 01.04.2020 82 | 83 | Documentation: Initialization of necessary optimization variables (symbolic CasADi expressions) 84 | and states including limits. 85 | """ 86 | 87 | self.temp_mot_n = ca.SX.sym('temp_mot_n') 88 | self.temp_mot_s = self.pars["temp_mot_max"] - 50 89 | self.temp_mot = self.temp_mot_s * self.temp_mot_n 90 | 91 | # Define limits and initial guess 92 | self.temp_min = self.pars["T_env"] / self.temp_mot_s 93 | self.temp_max = self.pars["temp_mot_max"] / self.temp_mot_s 94 | self.temp_guess = self.pars["T_env"] / self.temp_mot_s 95 | 96 | self.get_thermal_resistance() 97 | 98 | def get_states(self, 99 | f_drive: ca.SX, 100 | v: ca.SX): 101 | """ 102 | Python version: 3.5 103 | Created by: Thomas Herrmann 104 | Created on: 01.04.2020 105 | 106 | Documentation: Initializes states of the electric machine symbolically 107 | 108 | Inputs: 109 | f_drive: drive force at wheel [N] 110 | v: velocity of vehicle [m/s] 111 | """ 112 | 113 | # Effective current through single electric machine [A] 114 | self.i_eff = (f_drive * self.pars["r_wheel"] / self.pars["MotorConstant"] / self.pars["transmission"]) / \ 115 | self.pars["N_machines"] 116 | 117 | # Rotational speed electric machine > speed wheel; v = 2 pi f r [rpm] 118 | self.omega_machine = v / (2 * np.pi * self.pars["r_wheel"]) * self.pars["transmission"] * 60 119 | 120 | def get_increment(self, 121 | sf: ca.SX, 122 | temp_cool_12: ca.SX, 123 | temp_cool_13: ca.SX): 124 | """ 125 | Python version: 3.5 126 | Created by: Thomas Herrmann 127 | Created on: 01.04.2020 128 | 129 | Documentation: Initializes temperature increment of electric machine symbolically (sf * dx/dt = dx/ds) 130 | 131 | Inputs: 132 | sf: transformation factor dt/ds 133 | temp_cool_12: intermediate temperature within motor-inverter cooling circuit (radiator-motor) [°C] 134 | temp_cool_13: intermediate temperature within motor-inverter cooling circuit (motor-inverter) [°C] 135 | """ 136 | 137 | self.dtemp = sf * ((self.p_loss_total * 1000 - (self.temp_mot - (temp_cool_12 + temp_cool_13) / 2) 138 | / self.r_machine) 139 | / (self.pars["C_therm_machine"])) 140 | 141 | def get_loss(self, 142 | p_wheel: ca.SX): 143 | """ 144 | Python version: 3.5 145 | Created by: Thomas Herrmann 146 | Created on: 01.04.2020 147 | 148 | Documentation: Initializes total power loss of a single electric machine and split into loss effects 149 | (with detailed models) or loss power of a single e-machine using a simple power fit to measured data 150 | (input -- output power) 151 | 152 | Inputs: 153 | p_wheel: on wheels desired power [kW] 154 | """ 155 | 156 | if self.pars["simple_loss"]: 157 | 158 | # Input in machine [kW] = P_wheel + machine loss 159 | p_machine_in = \ 160 | self.pars["machine_simple_a"] * (p_wheel / self.pars["N_machines"]) ** 2 \ 161 | + self.pars["machine_simple_b"] * (p_wheel / self.pars["N_machines"]) \ 162 | + self.pars["machine_simple_c"] 163 | 164 | self.p_input = p_machine_in 165 | 166 | # Machine loss [kW] 167 | self.p_loss_total = p_machine_in - p_wheel / self.pars["N_machines"] 168 | 169 | else: 170 | 171 | temp_mot = self.temp_mot 172 | omega_machine = self.omega_machine 173 | i_eff = self.i_eff 174 | 175 | # Copper loss [W] 176 | p_loss_copper = \ 177 | ( 178 | (((temp_mot - 20) * self.pars["C_TempCopper"]) + 1) * self.pars["R_Phase"] 179 | ) * (i_eff ** 2) * (3 / 2) 180 | 181 | # Stator iron loss [W] 182 | p_loss_stator_iron = \ 183 | 2.885e-13 * omega_machine ** 4 \ 184 | - 1.114e-08 * omega_machine ** 3 \ 185 | + 0.0001123 * omega_machine ** 2 \ 186 | + 0.1657 * omega_machine \ 187 | + 272 188 | 189 | # Rotor loss [W] 190 | p_loss_rotor = \ 191 | 8.143e-14 * omega_machine ** 4 \ 192 | - 2.338e-09 * omega_machine ** 3 \ 193 | + 1.673e-05 * omega_machine ** 2 \ 194 | + 0.112 * omega_machine \ 195 | - 113.6 196 | 197 | # Total loss [kW] 198 | p_loss_total = (p_loss_copper 199 | + p_loss_stator_iron 200 | + p_loss_rotor) * 0.001 201 | 202 | # Store losses [kW] 203 | self.p_loss_copper = 0.001 * p_loss_copper 204 | self.p_loss_stator_iron = 0.001 * p_loss_stator_iron 205 | self.p_loss_rotor = 0.001 * p_loss_rotor 206 | self.p_loss_total = p_loss_total 207 | 208 | def get_machines_cum_losses(self): 209 | """ 210 | Python version: 3.5 211 | Created by: Thomas Herrmann 212 | Created on: 01.04.2020 213 | 214 | Documentation: Calculate total loss of all e-machines in electric powertrain 215 | """ 216 | 217 | self.p_loss_total_all_machines = self.p_loss_total * self.pars["N_machines"] 218 | 219 | def get_thermal_resistance(self): 220 | """ 221 | Python version: 3.5 222 | Created by: Thomas Herrmann 223 | Created on: 01.04.2020 224 | 225 | Documentation: Calculates thermal resistance of electric machine to be used within a 226 | lumped thermal model description 227 | """ 228 | 229 | A_cool_machine = 2 * np.pi * self.pars["r_stator_ext"] * self.pars["l_machine"] * \ 230 | self.pars["A_cool_inflate_machine"] 231 | 232 | # Thermal conduction stator [K/W] 233 | r_cond_stator = np.log(self.pars["r_stator_ext"] / 234 | self.pars["r_stator_int"]) / (2 * np.pi * self.pars["k_iro"] * self.pars["l_machine"]) 235 | 236 | # Thermal conduction rotor [K/W] 237 | r_cond_rotor = np.log(self.pars["r_rotor_ext"] / self.pars["r_rotor_int"]) / \ 238 | (2 * np.pi * self.pars["k_iro"] * self.pars["l_machine"]) 239 | 240 | # Thermal conduction shaft [K/W] 241 | r_cond_shaft = 1 / (4 * np.pi * self.pars["k_iro"] * self.pars["l_machine"]) 242 | 243 | # Thermal convection stator -- cooling liquid [K/W] 244 | r_conv_fluid = 1 / (self.pars["h_fluid_mi"] * A_cool_machine) 245 | 246 | # Thermal resistance by convection in the machine air gap rotor -- stator [K/W] 247 | r_conv_airgap = 1 / (2 * np.pi * self.pars["h_air_gap"] * self.pars["r_stator_int"] * self.pars["l_machine"]) 248 | 249 | # Thermal resistance out [K/W] 250 | r1 = r_cond_stator + r_conv_fluid 251 | # Thermal resistance in [K/W] 252 | r2 = r_cond_shaft + r_cond_rotor + r_conv_airgap 253 | 254 | # Thermal resistance machine [K/W] 255 | self.r_machine = (r1 * r2) / (r1 + r2) 256 | 257 | def ini_nlp_state(self, 258 | x: ca.SX, 259 | u: ca.SX): 260 | """ 261 | Python version: 3.5 262 | Created by: Thomas Herrmann 263 | Created on: 01.04.2020 264 | 265 | Documentation: Defines function to define e-machine states in NLP 266 | 267 | Inputs: 268 | x: discrete NLP state 269 | u: discrete NLP control input 270 | """ 271 | 272 | if self.pars["simple_loss"]: 273 | self.f_nlp = \ 274 | ca.Function('f_nlp', 275 | [x, u], [self.p_loss_total, self.p_input], 276 | ['x', 'u'], ['p_loss_total', 'p_input']) 277 | else: 278 | self.f_nlp = \ 279 | ca.Function('f_nlp', 280 | [x, u], [self.p_loss_total, self.p_loss_copper, self.p_loss_stator_iron, self.p_loss_rotor, 281 | self.i_eff], 282 | ['x', 'u'], ['p_loss_total', 'p_loss_copper', 'p_loss_stator_iron', 'p_loss_rotor', 283 | 'i_eff']) 284 | 285 | def extract_sol(self, 286 | w: ca.SX, 287 | sol_states: ca.DM): 288 | """ 289 | Python version: 3.5 290 | Created by: Thomas Herrmann 291 | Created on: 01.04.2020 292 | 293 | Documentation: Defines function to retrieve values of optimized NLP e-machine 294 | 295 | Inputs: 296 | w: discrete optimized NLP decision variables (x and u) 297 | sol_states: numeric values belonging to the symbolic NLP decision variables w 298 | """ 299 | 300 | if self.pars["simple_loss"]: 301 | self.f_sol = \ 302 | ca.Function('f_sol', 303 | [w], [self.p_losses_opt], 304 | ['w'], ['p_losses_opt']) 305 | 306 | # Overwrite lists with optimized numeric values 307 | p_losses_opt = self.f_sol(sol_states) 308 | 309 | self.p_loss_total = p_losses_opt[0::2] 310 | self.p_input = p_losses_opt[1::2] 311 | 312 | else: 313 | self.f_sol = \ 314 | ca.Function('f_sol', 315 | [w], [self.p_losses_opt], 316 | ['w'], ['p_losses_opt']) 317 | 318 | # Overwrite lists with optimized numeric values 319 | p_losses_opt = self.f_sol(sol_states) 320 | 321 | self.p_loss_total = p_losses_opt[0::5] 322 | self.p_loss_copper = p_losses_opt[1::5] 323 | self.p_loss_stator_iron = p_losses_opt[2::5] 324 | self.p_loss_rotor = p_losses_opt[3::5] 325 | self.i_eff = p_losses_opt[4::5] 326 | 327 | 328 | if __name__ == "__main__": 329 | pass 330 | -------------------------------------------------------------------------------- /params/racecar.ini: -------------------------------------------------------------------------------- 1 | # ---------------------------------------------------------------------------------------------------------------------- 2 | [GENERAL_OPTIONS] 3 | 4 | ### set name of ggv diagram and ax_max_machines files to use 5 | ggv_file="ggv.csv" 6 | ax_max_machines_file="ax_max_machines.csv" 7 | 8 | ### stepsize options 9 | # stepsize_prep: [m] used for linear interpolation before spline approximation 10 | # stepsize_reg: [m] used for spline interpolation after spline approximation (stepsize during opt.) 11 | # stepsize_interp_after_opt: [m] used for spline interpolation after optimization 12 | 13 | stepsize_opts={"stepsize_prep": 1.0, 14 | "stepsize_reg": 3.0, 15 | "stepsize_interp_after_opt": 2.0} 16 | 17 | ### spline regression smooth options 18 | # k_reg: [-] order of B-Splines -> standard: 3 19 | # s_reg: [-] smoothing factor, range [1.0, 100.0] 20 | 21 | reg_smooth_opts={"k_reg": 3, 22 | "s_reg": 10} 23 | 24 | ### preview and review distances for numerical curvature calculation (used in minimum time optimization) 25 | # d_preview_curv: [m] preview distance (curvature) 26 | # d_review_curv: [m] review distance (curvature) 27 | # d_preview_head: [m] preview distance (heading) 28 | # d_review_head: [m] review distance (heading) 29 | 30 | curv_calc_opts = {"d_preview_curv": 2.0, 31 | "d_review_curv": 2.0, 32 | "d_preview_head": 1.0, 33 | "d_review_head": 1.0} 34 | 35 | ### general vehicle parameters required in several functions 36 | # v_max: [m/s] maximal vehicle speed 37 | # length: [m] vehicle length 38 | # width: [m] vehicle width 39 | # mass: [kg] vehicle mass 40 | # dragcoeff: [kg*m2/m3] drag coefficient calculated by 0.5 * rho_air * c_w * A_front 41 | # curvlim: [rad/m] curvature limit of the vehicle 42 | # g: [N/kg] gravity acceleration 43 | 44 | veh_params = {"v_max": 70.0, 45 | "length": 4.7, 46 | "width": 2.0, 47 | "mass": 1200.0, 48 | "dragcoeff": 0.75, 49 | "curvlim": 0.12, 50 | "g": 9.81} 51 | 52 | ### velocity profile calculation options 53 | # dyn_model_exp: [-] exponent used in the vehicle dynamics model (range [1.0, 2.0]) 54 | # vel_profile_conv_filt_window: [-] moving average filter window size for velocity profile (set null if not used) 55 | 56 | vel_calc_opts = {"dyn_model_exp": 1.0, 57 | "vel_profile_conv_filt_window": null} 58 | 59 | # ---------------------------------------------------------------------------------------------------------------------- 60 | [OPTIMIZATION_OPTIONS] 61 | 62 | ### optimization problem options (shortest path optimization) ---------------------------------------------------------- 63 | # width_opt: [m] vehicle width for optimization including safety distance 64 | 65 | optim_opts_shortest_path={"width_opt": 3.4} 66 | 67 | ### optimization problem options (minimum curvature optimization) ------------------------------------------------------ 68 | # width_opt: [m] vehicle width for optimization including safety distance 69 | # iqp_iters_min: [-] minimum number of iterations for the IQP 70 | # iqp_curverror_allowed: [rad/m] maximum allowed curvature error for the IQP 71 | 72 | optim_opts_mincurv={"width_opt": 3.4, 73 | "iqp_iters_min": 3, 74 | "iqp_curverror_allowed": 0.01} 75 | 76 | ### optimization problem options (minimum lap time optimization) ------------------------------------------------------- 77 | # width_opt: [m] vehicle width for optimization including safety distance 78 | # penalty_delta: [-] penalty of delta derivative for improved smoothness of controls (range [0.0, 50.0]) 79 | # penalty_F: [-] penalty of F derivative for improved smoothness of controls (range [0.0, 2.0]) 80 | # mue: [-] constant friction coefficient (determines tire's D parameter of MF by D = F_z * mue) 81 | # n_gauss [-] number of gaussian basis functions on each side (var_friction: "gauss") 82 | # dn [m] distance of equidistant points on normal vectors for extracting friction 83 | # coefficients (var_friction: "linear" or "gauss") 84 | # limit_energy: [true/false] limit energy consumption 85 | # energy_limit: [kWh/lap] energy consumption limit (limit_energy: true) 86 | # safe_traj: [true/false] safe trajectories -> limit accelerations 87 | # ax_pos_safe: [m/s2] a_x+ limit for safe trajectories (safe_traj: true) -> null if ggv should be used 88 | # ax_neg_safe: [m/s2] a_x- limit for safe trajectories (safe_traj: true) -> null if ggv should be used 89 | # ay_safe: [m/s2] a_y limit for safe trajectories (safe_traj: true) -> null if ggv should be used 90 | # w_tr_reopt: [m] total track width in case of reoptimization using the IQP 91 | # w_veh_reopt: [m] vehicle width in case of reoptimization using the IQP 92 | # w_add_spl_regr: [m] width added in case of reoptimization to compensate second spline regression 93 | # step_non_reg: [-] defines how many points to be skipped per step during non-regular point sampling 94 | # (dependent on curvature) 95 | # eps_kappa: [rad/m] curvature threshold to skip discretization points on straights (if 96 | # step_non_reg > 0) 97 | 98 | optim_opts_mintime={"width_opt": 3.4, 99 | "penalty_delta": 10.0, 100 | "penalty_F": 0.01, 101 | "mue": 1.0, 102 | "n_gauss": 5, 103 | "dn": 0.25, 104 | "limit_energy": false, 105 | "energy_limit": 2.0, 106 | "safe_traj": false, 107 | "ax_pos_safe": null, 108 | "ax_neg_safe": null, 109 | "ay_safe": null, 110 | "w_tr_reopt": 2.0, 111 | "w_veh_reopt": 1.6, 112 | "w_add_spl_regr": 0.2, 113 | "step_non_reg": 0, 114 | "eps_kappa": 1e-3} 115 | 116 | ### vehicle parameters (minimum lap time optimization) 117 | # wheelbase_front: [m] wheelbase front 118 | # wheelbase_rear: [m] wheelbase rear 119 | # track_width_front: [m] track width front 120 | # track_width_rear: [m] track width rear 121 | # cog_z: [m] center of gravity 122 | # I_z: [kgm^2] yaw inertia 123 | # liftcoeff_front: [kg*m2/m3] lift coefficient front axle calculated by 0.5 * rho_air * c_l_f * A_spoiler_f 124 | # liftcoeff_rear: [kg*m2/m3] lift coefficient rear axle calculated by 0.5 * rho_air * c_l_r * A_spoiler_r 125 | # k_brake_front: [-] portion of braking force at the front axle of the total braking force 126 | # k_drive_front: [-] portion of driving force at the front axle of the total driving force 127 | # k_roll: [-] portion of roll moment at the front axle of the total roll moment 128 | # t_delta: [s] time constant for steering dynamic 129 | # t_drive: [s] time constant for acceleration dynamic 130 | # t_brake: [s] time constant for braking dynamic 131 | # power_max: [W] maximal engine power 132 | # f_drive_max: [N] maximal drive force 133 | # f_brake_max: [N] maximal brake force (only needed for actor dynamics) 134 | # delta_max: [rad] maximal steer angle 135 | 136 | vehicle_params_mintime = {"wheelbase_front": 1.6, 137 | "wheelbase_rear": 1.4, 138 | "track_width_front": 1.6, 139 | "track_width_rear": 1.6, 140 | "cog_z": 0.38, 141 | "I_z": 1200.0, 142 | "liftcoeff_front": 0.45, 143 | "liftcoeff_rear": 0.75, 144 | "k_brake_front": 0.6, 145 | "k_drive_front": 0.0, 146 | "k_roll": 0.5, 147 | "t_delta": 0.2, 148 | "t_drive": 0.05, 149 | "t_brake": 0.05, 150 | "power_max": 230000.0, 151 | "f_drive_max": 7000.0, 152 | "f_brake_max": 20000.0, 153 | "delta_max": 0.35} 154 | 155 | ### tire parameters (minimum lap time optimization) 156 | # c_roll: [-] rolling resistance coefficient 157 | # f_z0: [N] nominal normal force 158 | # B_front: [-] Coefficient B for front tire 159 | # C_front: [-] Coefficient C for front tire 160 | # eps_front: [-] load dependence of Coefficient D for front tire 161 | # E_front: [-] Coefficient E for front tire 162 | # B_rear: [-] Coefficient B for rear tire 163 | # C_rear: [-] Coefficient C for rear tire 164 | # eps_rear: [-] load dependence of Coefficient D for rear tire 165 | # E_rear: [-] Coefficient E for rear tire 166 | # Hint: The D parameter of the Magic Formula is determined by D = F_z * mue. mue can be set above in optim_opts_mintime! 167 | 168 | tire_params_mintime = {"c_roll": 0.013, 169 | "f_z0": 3000.0, 170 | "B_front": 10.0, 171 | "C_front": 2.5, 172 | "eps_front": -0.1, 173 | "E_front": 1.0, 174 | "B_rear": 10.0, 175 | "C_rear": 2.5, 176 | "eps_rear": -0.1, 177 | "E_rear": 1.0} 178 | 179 | ### powertrain behavior (minimum lap time optimization) 180 | # [1] Prof. Dr.-Ing. Markus Lienkamp, „Auslegung von Elektrofahrzeugen: Lecture slides,“ Unpublished manuscript, 2018 181 | # [2] F. P. Incropera, Fundamentals of heat and mass transfer, 6th ed., Hoboken NJ, John Wiley, 2007, ISBN: 9780471457282 182 | # [3] M. Grabowski, K. Urbaniec, J. Wernik and K. J. Wołosz, „Numerical simulation and experimental verification of heat transfer from a finned housing of an electric motor,“Energy Conversion and Management, vol. 125, pp. 91–96, 2016 183 | # [4] K. Li, S. Wang and J. P. Sullivan, „A novel thermal network for the maximum temperaturerise of hollow cylinder,“ Applied Thermal Engineering, vol. 52, no. 1, pp. 198–208, 2013 184 | 185 | # pwr_behavior: [-] consider powertrain behavior 186 | # simple_loss: [-] use simple loss models (fitted to measured data, input -- ouput power) 187 | # T_env: [°C] temperature of environment 188 | # T_mot_ini: [°C] initial temperature electric machines 189 | # T_batt_ini: [°C] initial temperature battery 190 | # T_inv_ini: [°C] initial temperature inverter 191 | # T_cool_mi_ini: [°C] initial temperature cooling fluid machine and inverter 192 | # T_cool_b_ini: [°C] initial temperature battery fluid 193 | # r_wheel: [m] Wheel radius 194 | # R_i_sumo: [Ohm] internal resistance of battery (SUMO model fit) 195 | # R_i_simple: [Ohm] internal resistance of simple battery model 196 | # R_i_offset: [Ohm] single cell resistance, offset in temperature dependency 197 | # R_i_slope : [Ohm/°C] single cell resistance, slope in temperature dependency 198 | # V_OC_simple: [V] Open Circuit Voltage of simple battery model 199 | # SOC_ini: [-] initial SOC of battery 200 | # C_batt: [kWh] Capacity of battery (spreadsheet) 201 | # N_cells_serial: [-] Number of battery cells in series in battery pack 202 | # N_cells_parallel: [-] Number of battery cells in parallel in battery pack 203 | # temp_mot_max: [°C] max. allowed temperature of electric machines (spreadsheet) 204 | # temp_batt_max: [°C] max. allowed temperature of battery (spreadsheet) 205 | # temp_inv_max: [°C] max. allowed temperature of inverters [1, p 7.19] 206 | # N_machines: [-] number of electric machines (spreadsheet) 207 | # transmission: [-] gear transmission 208 | # MotorConstant: [Nm/A] motor constant (linear dependency current and torque) 209 | # C_therm_machine: [J/K] Absolute thermal capacity electric machine 210 | # c_iro = 420 J/kgK; c_magnet = 460 J/kgK; m_machine = 18 kg (spreadsheet) 211 | # -> C_therm_machine = 99 % * m_machine * c_iro + 1 % * m_machine * c_magnet 212 | # C_therm_inv: [J/K] Absolute thermal capacity inverter (equal to that of motor) 213 | # C_therm_cell: [J/K] Absolute thermal capacity battery pouch cell 214 | # C_TempCopper: [1/°C] temperature coefficient copper in electric machine 215 | # m_therm_fluid_mi [kg] mass of cooling fluid (machine inverter circuit) 216 | # m_therm_fluid_b [kg] mass of cooling fluid (battery circuit) 217 | # R_Phase: [kOhm] phase resistance electric machine 218 | # r_rotor_int: [m] radius rotor internal (spreadsheet) 219 | # r_rotor_ext: [m] radius rotor external (spreadsheet) 220 | # r_stator_int: [m] radius stator internal (spreadsheet) 221 | # r_stator_ext: [m] radius stator external (spreadsheet) 222 | # l_machine: [m] length electric machine (spreadsheet) 223 | # A_cool_inflate_machine: [-] factor of which cooling surface of machine is bigger than raw outer surface 224 | # A_cool_inv: [m^2] cooling area of inverter: length * width 225 | # A_cool_rad: [m^2] cooling area of radiator [2, p 704] 226 | # k_iro: [W/m^2K] thermal conductivity of iron [3] 227 | # h_air: [W/m^2K] convective heat flux coefficient of radiator to air [1, p 9.95] 228 | # h_air_gap: [W/m^2K] convective heat flux coefficient of machine air gap [4] 229 | # h_fluid_mi: [W/m^2K] convective heat flux coefficient of cooling fluid machine + inverter [1, p 9.95] 230 | # c_heat_fluid [J/kgK] specific heat capacity cooling fluid (similar to water) [2] 231 | # flow_rate_inv [kg/s] flow rate of cooling fluid through inverters 232 | # flow_rate_rad [kg/s] flow rate of cooling fluid through radiator (estimated from spreadsheet) 233 | # machine_simple_a,b,c: [-] fitting coefficients (ax^2 + bx + c) of machine efficiency (input -- output power) 234 | # V_ref: [V] reference voltage inverter 235 | # I_ref: [A] reference current inverter 236 | # V_ce_offset: [V] current dependent Collector-Emitter voltage IGBT 'SKim459GD12E4' (linear dependency) 237 | # V_ce_slope: [V] current dependent Collector-Emitter voltage IGBT 'SKim459GD12E4' (linear dependency) 238 | # E_on: [J] loss energy switching ON IGBT-bridge 'SKim459GD12E4', value for bridge (2 IGBTS) 239 | # E_off: [J] loss energy switching OFF IGBT-bridge 'SKim459GD12E4', value for bridge (2 IGBTS) 240 | # E_rr: [J] loss energy reverse recovery IGBT-bridge 'SKim459GD12E4', value for bridge (2 IGBTS) 241 | # f_sw: [Hz] constantly assumed inverter frequency 242 | # inverter_fit_a,b,c [-] fitting coefficients (ax^2 + bx + c) of inverter efficiency (input -- output power) 243 | 244 | pwr_params_mintime = {"pwr_behavior": false, 245 | "simple_loss": true, 246 | "T_env": 30, 247 | "T_mot_ini": 30, 248 | "T_batt_ini": 30, 249 | "T_inv_ini": 30, 250 | "T_cool_mi_ini": 30, 251 | "T_cool_b_ini": 30, 252 | "r_wheel": 0.3, 253 | "R_i_sumo": 0.001, 254 | "R_i_simple": 0.125, 255 | "R_i_offset": 0.0013871, 256 | "R_i_slope": 7.5133e-6, 257 | "V_OC_simple": 700, 258 | "SOC_ini": 0.4, 259 | "C_batt": 41.0, 260 | "N_cells_serial": 176, 261 | "N_cells_parallel": 3, 262 | "temp_mot_max": 170.0, 263 | "temp_batt_max": 50.0, 264 | "temp_inv_max": 100.0, 265 | "N_machines": 2, 266 | "transmission": 6.25, 267 | "MotorConstant": 0.465, 268 | "C_therm_machine": 7567.2, 269 | "C_therm_inv": 7567.2, 270 | "C_therm_cell": 824.0, 271 | "C_TempCopper": 0.004041, 272 | "m_therm_fluid_mi": 5, 273 | "m_therm_fluid_b": 5, 274 | "R_Phase": 0.0105, 275 | "r_rotor_int": 0.03, 276 | "r_rotor_ext": 0.087, 277 | "r_stator_int": 0.088, 278 | "r_stator_ext": 0.121, 279 | "l_machine": 0.055, 280 | "A_cool_inflate_machine": 3.0, 281 | "A_cool_inv": 0.3969, 282 | "A_cool_rad": 5.0, 283 | "k_iro": 45.0, 284 | "h_air": 50.0, 285 | "h_air_gap": 60.0, 286 | "h_fluid_mi": 5000.0, 287 | "c_heat_fluid": 4181.0, 288 | "flow_rate_inv": 0.2, 289 | "flow_rate_rad": 0.2, 290 | "machine_simple_a": -0.000027510784764, 291 | "machine_simple_b": 1.046187222759047, 292 | "machine_simple_c": 1.001964003837042, 293 | "V_ref": 600.0, 294 | "I_ref": 450.0, 295 | "V_ce_offset": 0.8, 296 | "V_ce_slope": 0.0036, 297 | "E_on": 0.022, 298 | "E_off": 0.057, 299 | "E_rr": 0.04, 300 | "f_sw": 12000.0, 301 | "inverter_simple_a": -0.000707138661579, 302 | "inverter_simple_b": 1.139958410466637, 303 | "inverter_simple_c": 1.004970807882952} 304 | --------------------------------------------------------------------------------