├── vehicle_dynamics ├── utils │ ├── __init__.py │ ├── import_data_CM.py │ ├── LocalLogger.py │ ├── StaticParameters.py │ ├── CurrentStates.py │ └── plot_function.py ├── modules │ ├── __init__.py │ ├── LocalLogger.py │ ├── wheels.py │ ├── powertrain.py │ └── body.py ├── __init__.py ├── structures │ ├── __init__.py │ ├── WheelHubForce.py │ ├── AngularWheelPosition.py │ ├── TireForces.py │ ├── StrutForce.py │ ├── Displacement.py │ ├── Manoeuvre.py │ ├── StateVector.py │ ├── StaticParameters.py │ ├── OutputStates.py │ └── CurrentStates.py ├── VehicleDynamics.py └── main.py ├── example_data └── Braking.pickle ├── pyproject.toml ├── setup.py ├── LICENSE ├── README.md ├── .gitignore ├── main.py └── bmw_m8.yaml /vehicle_dynamics/utils/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["plot_function"] 2 | -------------------------------------------------------------------------------- /vehicle_dynamics/modules/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["powertrain", "wheels", "body","LocalLogger"] 2 | -------------------------------------------------------------------------------- /example_data/Braking.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/roadview-project/vehicle-dynamics/HEAD/example_data/Braking.pickle -------------------------------------------------------------------------------- /vehicle_dynamics/__init__.py: -------------------------------------------------------------------------------- 1 | from vehicle_dynamics.modules import * 2 | from vehicle_dynamics.structures import * 3 | from vehicle_dynamics.utils import * 4 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ["AngularWheelPosition", "Displacement", "StateVector", "StrutForce", "TireForces", "WheelHubForce","CurrentStates","OutputStates"] 2 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/WheelHubForce.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class WheelHubForce(object): 5 | def __init__(self, f_zr_dot=np.zeros(4), wheel_load_z =np.array([2000., 3000., 2000., 3000.])): 6 | self.f_zr_dot = f_zr_dot 7 | self.wheel_load_z = wheel_load_z 8 | 9 | def __array__(self) -> np.ndarray: 10 | return np.array([self.f_zr_dot, 11 | self.wheel_load_z]) 12 | -------------------------------------------------------------------------------- /vehicle_dynamics/utils/import_data_CM.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri Jul 7 11:34:11 2023 4 | 5 | @author: albertonbloemer 6 | """ 7 | 8 | 9 | def import_data_CM(path): 10 | import pickle 11 | with open(path, "rb") as handle: 12 | data = pickle.load(handle) 13 | return data 14 | 15 | 16 | def main(): 17 | path = "../../exampledata/acc_brake/SimulationData.pickle" 18 | data = import_data_CM(path) 19 | print(data) 20 | 21 | if __name__ == '__main__': 22 | main() 23 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/AngularWheelPosition.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class AngularWheelPosition(object): 5 | def __init__(self, pho_r=np.zeros(4), pho_r_dot = np.zeros(4), pho_r_2dot = np.zeros(4)): 6 | 7 | # pag 272 eq.52 8 | # Angular position/speed of each wheel 9 | self.pho_r = pho_r 10 | self.pho_r_dot = pho_r_dot 11 | self.pho_r_2dot = pho_r_2dot 12 | 13 | def __array__(self) -> np.ndarray: 14 | return np.array([self.pho_r, self.pho_r_dot, self.pho_r_2dot]) 15 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/TireForces.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class TireForces(object): 5 | def __init__(self, fx =np.zeros(4), fy =np.zeros(4), wheel_forces_transformed_force2vehicle_sys = np.zeros((3, 4), dtype=float)): 6 | # Dynamic forces on the tires 7 | # pag 272 eq.54 8 | self.fx = fx 9 | self.fy = fy 10 | self.wheel_forces_transformed_force2vehicle_sys = wheel_forces_transformed_force2vehicle_sys 11 | 12 | def __array__(self) -> np.ndarray: 13 | return np.array([*self.fx, 14 | *self.fy, 15 | *self.wheel_forces_transformed_force2vehicle_sys.flatten()]) 16 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/StrutForce.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class StrutForce(object): 5 | def __init__(self, f_za=np.zeros(4), f_za_dot=np.zeros(4), spring_force = np.zeros(4), dumper_force = np.zeros(4)): # TODO calculate forces on the strut 6 | self.f_za = f_za 7 | self.f_za_dot = f_za_dot 8 | self.dumper_force = dumper_force 9 | self.spring_force = spring_force 10 | 11 | def __array__(self) -> np.ndarray: 12 | return np.array([*self.f_za.flatten(), 13 | *self.f_za_dot.flatten(), 14 | *self.dumper_force.flatten(), 15 | *self.spring_force.flatten()]) 16 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [project] 6 | name = "vehicle_dynamics" 7 | version = "1.0.6" 8 | description = "Python Package that makes Vehicle Dynamics Calculation for cars. " 9 | authors = [{name = "Yuri Poledna", email = "yuri.poledna@carissma.eu"}] 10 | license = {text = "MIT"} 11 | readme = "README.md" 12 | requires-python = ">=3.7" 13 | dependencies = [ 14 | 'numpy', 15 | 'matplotlib', 16 | 'scipy', 17 | "pyyaml", 18 | "tqdm", 19 | "pandas", 20 | "munch", 21 | ] 22 | 23 | [project.urls] 24 | Homepage = "https://github.com/roadview-project/vehicle-dynamics" 25 | Repository = "https://github.com/roadview-project/vehicle-dynamics" 26 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/Displacement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Displacement(object): 5 | def __init__(self, static_suspension, suspension=np.zeros(4), suspension_dot=np.zeros(4), zr_dot=np.zeros(4), zr_2dot = np.zeros(4), road =np.zeros(4)): 6 | self.static_suspension = static_suspension 7 | self.suspension = suspension 8 | self.suspension_dot = suspension_dot 9 | self.zr_2dot = zr_2dot 10 | self.zr_dot = zr_dot 11 | self.road = road 12 | 13 | def __array__(self) -> np.ndarray: 14 | return np.array([self.static_suspension, 15 | self.suspension, 16 | self.suspension_dot, 17 | self.zr_2dot, 18 | self.zr_dot, 19 | self.road]) 20 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | VERSION = '1.0.6' 4 | DESCRIPTION = 'vehicle_dynamics' 5 | LONG_DESCRIPTION = 'package that calculates the vehicle dynamics' 6 | 7 | # Setting up 8 | setup( 9 | name="vehicle_dynamics", 10 | version=VERSION, 11 | author="Maikol Funk Dreschler, Yuri Poledna", 12 | author_email="", 13 | description=DESCRIPTION, 14 | long_description=LONG_DESCRIPTION, 15 | packages=find_packages(), 16 | install_requires=[ 17 | 'numpy', 18 | 'matplotlib', 19 | 'scipy', 20 | "pyyaml", 21 | "tqdm", 22 | "pandas", 23 | "munch", 24 | ], 25 | 26 | keywords=['python', 'vehicle dynamics'], 27 | classifiers= [ 28 | "Programming Language :: Python :: 3", 29 | ] 30 | ) 31 | -------------------------------------------------------------------------------- /vehicle_dynamics/utils/LocalLogger.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import sys 3 | 4 | 5 | class LocalLogger(logging.Logger): 6 | """Logger util for the package""" 7 | loggers = set() 8 | 9 | def __init__(self, name, format="%(asctime)s | %(levelname)s | %(funcName)s %(lineno)d | %(message)s", level=logging.INFO): 10 | # Initial construct. 11 | self.format = format 12 | self.level = level 13 | self.name = name 14 | 15 | # Logger configuration. 16 | self.console_formatter = logging.Formatter(self.format) 17 | self.console_logger = logging.StreamHandler(sys.stdout) 18 | self.console_logger.setFormatter(self.console_formatter) 19 | 20 | # Complete logging config. 21 | self.logger = logging.getLogger(name) 22 | if name not in self.loggers: 23 | self.loggers.add(name) 24 | self.logger.setLevel(self.level) 25 | self.logger.addHandler(self.console_logger) 26 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/Manoeuvre.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple 2 | 3 | import numpy as np 4 | ManoeuvreState = namedtuple('ManoeuvreState', 'throttle brake steering') 5 | 6 | 7 | class Manoeuvre(): 8 | """docstring for Manoeuvre""" 9 | 10 | def __init__(self, steering, throttle, brake, time): 11 | super(Manoeuvre, self).__init__() 12 | self.steering = steering 13 | self.throttle = throttle 14 | self.brake = brake 15 | self.time = time 16 | 17 | def __getitem__(self, items): 18 | """ 19 | receives items to be returned 20 | returns ManoeuvreState(throttle, brake, steering) 21 | 22 | """ 23 | return ManoeuvreState(self.throttle[items], self.brake[items], self.steering[items]) 24 | 25 | def __array__(self) -> np.ndarray: 26 | return np.array([self.steering, 27 | self.throttle, 28 | self.brake, 29 | self.time]) 30 | 31 | def __len__(self): 32 | return len(self.time) 33 | pass 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 ROADVIEW Project 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /vehicle_dynamics/modules/LocalLogger.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model - Logger Class 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | import logging 9 | import sys 10 | 11 | 12 | class LocalLogger(logging.Logger): 13 | """Logger util for the package""" 14 | loggers = set() 15 | 16 | def __init__(self, name, format="%(asctime)s | %(levelname)s | %(funcName)s %(lineno)d | %(message)s", level=logging.INFO): 17 | # Initial construct. 18 | self.format = format 19 | self.level = level 20 | self.name = name 21 | 22 | # Logger configuration. 23 | self.console_formatter = logging.Formatter(self.format) 24 | self.console_logger = logging.StreamHandler(sys.stdout) 25 | self.console_logger.setFormatter(self.console_formatter) 26 | 27 | # Complete logging config. 28 | self.logger = logging.getLogger(name) 29 | if name not in self.loggers: 30 | self.loggers.add(name) 31 | self.logger.setLevel(self.level) 32 | self.logger.addHandler(self.console_logger) 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # vehicle-dynamics 2 | 3 | Developed by Maikol Funk Drechsler & Yuri Poledna(Technische Hochschule Ingolstadt - CARISSMA Institute of Automated Driving) with assistance of Mattias Hjort and Sogol Kharrazi (VTI - Statens Vag- och Transportforskningsinstitut) 4 | 5 | This work also has utils that allow for optimization of your vehicle. 6 | 7 | Under the [ROADVIEW Consortium](https://roadview-project.eu/). 8 | 9 | # Instalation 10 | ``` 11 | python setup.py sdist bdist_wheel 12 | pip install --editable . 13 | ``` 14 | 15 | # Usage 16 | For first use you can simply run 17 | ``` 18 | python main.py 19 | ``` 20 | there is a default vehicle with its parameters in the [YAML](bmw_m8.yaml) file. 21 | 22 | 23 | # Acknoledgment 24 | Co-funded by the European Union. Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. Project grant no. 101069576. 25 | UK participants in this project are co-funded by Innovate UK under contract no.10045139. 26 | Swiss participants in this project are co-funded by the Swiss State Secretariat for Education, Research and Innovation (SERI) under contract no. 22.00123. 27 | 28 | 29 | # Citation 30 | Please cite the following work when publishing: 31 | ``` 32 | @INPROCEEDINGS{10786416, 33 | author={Drechsler, Maikol Funk and Poledna, Yuri and Hjort, Mattias and Kharrazi, Sogol and Huber, Werner}, 34 | booktitle={2024 IEEE International Automated Vehicle Validation Conference (IAVVC)}, 35 | title={Vehicle Dynamics Parameter Estimation Methodology for Virtual Automated Driving Testing}, 36 | year={2024}, 37 | volume={}, 38 | number={}, 39 | pages={1-7}, 40 | keywords={Weight measurement;Parameter estimation;Heuristic algorithms;Torque converters;Process control;Optimization methods;Position measurement;Tires;Vehicle dynamics;Testing;Adverse Weather;Automated Driving Vehicles;Vehicle Dynamics Model}, 41 | doi={10.1109/IAVVC63304.2024.10786416}} 42 | 43 | } 44 | ``` 45 | # Other ROADVIEW-Project Works: 46 | 47 | [REHEARSE Dataset](https://s3.ice.ri.se/roadview-WP3-Warwick/T3.2%20-%20Create%20Dataset/rehearse/index.html) 48 | - The REHEARSE dataset is a dataset made for sensor models creation and validation, made under extreme weather in controllable conditions. 49 | 50 | [Dataset Creator OSI](https://github.com/roadview-project/dataset_creator_OSI) 51 | - Creates Dataset in ASAM OSI format from rosbags. 52 | 53 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | build/ 7 | dist/ 8 | 9 | *.dat 10 | *.zip 11 | *.png 12 | vehicle_dynamics/utils/reading_data/ 13 | # C extensions 14 | *.so 15 | 16 | # Distribution / packaging 17 | .Python 18 | build/ 19 | develop-eggs/ 20 | dist/ 21 | downloads/ 22 | eggs/ 23 | .eggs/ 24 | lib/ 25 | lib64/ 26 | parts/ 27 | sdist/ 28 | var/ 29 | wheels/ 30 | pip-wheel-metadata/ 31 | share/python-wheels/ 32 | *.egg-info/ 33 | .installed.cfg 34 | *.egg 35 | MANIFEST 36 | 37 | # PyInstaller 38 | # Usually these files are written by a python script from a template 39 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 40 | *.manifest 41 | *.spec 42 | 43 | # Installer logs 44 | pip-log.txt 45 | pip-delete-this-directory.txt 46 | 47 | # Unit test / coverage reports 48 | htmlcov/ 49 | .tox/ 50 | .nox/ 51 | .coverage 52 | .coverage.* 53 | .cache 54 | nosetests.xml 55 | coverage.xml 56 | *.cover 57 | *.py,cover 58 | .hypothesis/ 59 | .pytest_cache/ 60 | 61 | # Translations 62 | *.mo 63 | *.pot 64 | 65 | # Django stuff: 66 | *.log 67 | local_settings.py 68 | db.sqlite3 69 | db.sqlite3-journal 70 | 71 | # Flask stuff: 72 | instance/ 73 | .webassets-cache 74 | 75 | # Scrapy stuff: 76 | .scrapy 77 | 78 | # Sphinx documentation 79 | docs/_build/ 80 | 81 | # PyBuilder 82 | target/ 83 | 84 | # Jupyter Notebook 85 | .ipynb_checkpoints 86 | 87 | # IPython 88 | profile_default/ 89 | ipython_config.py 90 | 91 | # pyenv 92 | .python-version 93 | 94 | # pipenv 95 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 96 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 97 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 98 | # install all needed dependencies. 99 | #Pipfile.lock 100 | 101 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 102 | __pypackages__/ 103 | 104 | # Celery stuff 105 | celerybeat-schedule 106 | celerybeat.pid 107 | 108 | # SageMath parsed files 109 | *.sage.py 110 | 111 | # Environments 112 | .env 113 | .venv 114 | env/ 115 | venv/ 116 | ENV/ 117 | env.bak/ 118 | venv.bak/ 119 | 120 | # Spyder project settings 121 | .spyderproject 122 | .spyproject 123 | 124 | # Rope project settings 125 | .ropeproject 126 | 127 | # mkdocs documentation 128 | /site 129 | 130 | # mypy 131 | .mypy_cache/ 132 | .dmypy.json 133 | dmypy.json 134 | 135 | # Pyre type checker 136 | .pyre/ 137 | 138 | # Debug material 139 | 140 | pacejka.py 141 | testecopia.py 142 | Tire.py 143 | torque_interpolation.py 144 | __pycache__/ 145 | 146 | .vscode/ 147 | 148 | 149 | numpy_multiplication.py 150 | -------------------------------------------------------------------------------- /vehicle_dynamics/VehicleDynamics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model Main Class 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | 9 | from vehicle_dynamics.structures.TireForces import TireForces 10 | from vehicle_dynamics.structures.StateVector import StateVector 11 | from vehicle_dynamics.structures.Displacement import Displacement 12 | 13 | from vehicle_dynamics.structures.WheelHubForce import WheelHubForce 14 | from vehicle_dynamics.structures.AngularWheelPosition import AngularWheelPosition 15 | from vehicle_dynamics.structures.OutputStates import OutputStates 16 | from vehicle_dynamics.structures.CurrentStates import CurrentStates 17 | from vehicle_dynamics.structures.StaticParameters import StaticParameters 18 | 19 | from vehicle_dynamics.modules.LocalLogger import LocalLogger 20 | 21 | from vehicle_dynamics.modules.powertrain import Powertrain 22 | from vehicle_dynamics.modules.wheels import Wheels 23 | from vehicle_dynamics.modules.body import Body 24 | 25 | 26 | from scipy.interpolate import interp1d 27 | from numpy.linalg import inv 28 | import numpy as np 29 | import yaml 30 | import math 31 | 32 | 33 | class VehicleDynamics(object): 34 | """ This Class does the grunt of calculating VehicleDynamics!! 35 | Calculate longitudinal and lateral dynamics with desired values 36 | of brake, steer and thorttle positons. 37 | """ 38 | 39 | def __init__(self, initial_state = np.zeros(15), initial_gear = 1, frequency=1000, car_parameters_path = ""): 40 | self.logger = LocalLogger("MainLogger").logger 41 | self.logger.setLevel("INFO") 42 | 43 | self.static_parameters = StaticParameters(car_parameters_path, frequency) 44 | 45 | self.logger.info("Imported YAML car self") 46 | self.current_states = CurrentStates(self.static_parameters, frequency, initial_state, initial_gear, self.logger) 47 | self.powertrain = Powertrain(self.static_parameters, self.logger) 48 | self.wheels = Wheels(self.static_parameters, self.logger) 49 | self.body = Body(self.static_parameters, self.logger) 50 | self.CUT_VALUE = 0.2 51 | 52 | def tick(self, throttle, brake, steering_angle): 53 | if self.CUT_VALUE > np.any(self.current_states.slip_x): 54 | throttle=throttle/2 55 | self.current_states = self.powertrain.powertrain(self.current_states, throttle, brake) 56 | self.current_states = self.wheels.wheels(self.current_states, steering_angle) 57 | self.current_states = self.body.body(self.current_states) 58 | 59 | return self.current_states 60 | -------------------------------------------------------------------------------- /vehicle_dynamics/utils/StaticParameters.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import numpy as np 3 | from munch import DefaultMunch 4 | from copy import deepcopy 5 | import munch 6 | 7 | 8 | class StaticParameters(object): 9 | 10 | _initialized=False 11 | 12 | def __init__(self, car_parameters_path, freq=1000, optimization=False): 13 | super(StaticParameters, self).__init__() 14 | with open(car_parameters_path, 'r') as file: 15 | param = yaml.safe_load(file) 16 | 17 | self.convert_YAML2Struct(param) 18 | 19 | self.powertrain.gearbox.gear_max_rpm *= np.pi / 30 20 | self.powertrain.gearbox.gear_min_rpm *= np.pi / 30 21 | self.powertrain.engine.w_table *= np.pi / 30 22 | self.powertrain.engine.minimum_rpm *= np.pi / 30 23 | self.powertrain.engine.maximum_rpm *= np.pi / 30 24 | self.powertrain.engine.idle_rpm *= np.pi / 30 25 | 26 | self.wheel_base = self.body.lr + self.body.lf 27 | self.track_width = self.body.wl + self.body.wr 28 | 29 | wd1 = (self.body.lr / (self.wheel_base)) * (self.body.wr / self.track_width) 30 | wd2 = (self.body.lf / (self.wheel_base)) * (self.body.wr / self.track_width) 31 | wd3 = (self.body.lr / (self.wheel_base)) * (self.body.wl / self.track_width) 32 | wd4 = (self.body.lf / (self.wheel_base)) * (self.body.wl / self.track_width) 33 | 34 | self.wd = np.array([wd1, wd2, wd3, wd4]) 35 | self.tire.dynamic_radius = np.array(self.tire.dynamic_radius) 36 | self.sprung_mass = self.body.mass * self.wd - self.suspension.unsprung_mass 37 | 38 | self.time_step = 1. / freq 39 | 40 | self.aerodynamics_front = self.aerodynamics.air_drag_coefficient * self.aerodynamics.front_area * self.aerodynamics.air_density 41 | 42 | self.gravity = 9.81 43 | 44 | if not optimization: 45 | self._initialized = True 46 | 47 | 48 | def __setattr__(self, name, value): 49 | if self._initialized: 50 | raise AttributeError("We are not in the saving attributes phase of this code, Do Better") 51 | else: 52 | object.__setattr__(self, name, value) 53 | 54 | def convert_YAML2Struct(self, yaml): 55 | data = DefaultMunch.fromDict(yaml) 56 | for name in dir(data): 57 | current_key = getattr(data, name) 58 | setattr(data, name, self.check_munch_key(current_key)) 59 | setattr(self, name, getattr(data, name)) 60 | 61 | def check_munch_key(self, current_key): 62 | for i in dir(current_key): 63 | if isinstance(getattr(current_key, i), list): 64 | setattr(current_key, i, np.array(getattr(current_key, i))) 65 | elif isinstance(getattr(current_key, i), munch.DefaultMunch): 66 | setattr(current_key, i, self.check_munch_key(getattr(current_key, i))) 67 | return current_key 68 | 69 | 70 | def main(): 71 | car_parameters_path = "../../Audi_r8.yaml" 72 | 73 | car_parameters = StaticParameters(car_parameters_path,optimization=True) 74 | 75 | print(car_parameters.aerodynamics.air_drag_coefficient) 76 | print(car_parameters.powertrain.torque_converter.efficiency_function) 77 | print(car_parameters.powertrain.torque_converter.efficiency_function) 78 | car_parameters.gravity = 1 79 | print(car_parameters.gravity) 80 | 81 | car_parameters._initialized = True 82 | 83 | car_parameters.gravity = 9.8 84 | print(car_parameters.gravity) 85 | 86 | if __name__ == '__main__': 87 | main() 88 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Main Function for the vehicle dynamics module. 3 | @author: Maikol Funk Drechsler, Yuri Poledna 4 | 5 | ROADVIEW Project 6 | Co-Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. UK and Swiss participants in this project are supported by Innovate UK (contract no. 10045139) and the Swiss State Secretariat for Education, Research and Innovation (Contract no.22.00123) respectevely. 7 | """ 8 | from vehicle_dynamics.structures.OutputStates import OutputStates 9 | from vehicle_dynamics.utils.plot_function import plot_function 10 | from vehicle_dynamics.structures.Manoeuvre import Manoeuvre 11 | from vehicle_dynamics.structures.StateVector import StateVector 12 | 13 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 14 | 15 | from vehicle_dynamics.VehicleDynamics import VehicleDynamics 16 | 17 | import pandas 18 | import numpy as np 19 | import tqdm 20 | 21 | from collections import namedtuple 22 | 23 | import pickle 24 | import os 25 | import urllib.request 26 | file_path = "example_data/Braking.pickle" 27 | url = "https://github.com/roadview-project/vehicle-dynamics/raw/refs/heads/main/example_data/Braking.pickle" 28 | urllib.request.urlretrieve(url, "Braking.pickle") 29 | os.makedirs(os.path.dirname(file_path), exist_ok=True) 30 | 31 | if not os.path.exists(file_path): 32 | print(f"{file_path} not found. Downloading from {url}...") 33 | urllib.request.urlretrieve(url, file_path) 34 | print("Download complete!") 35 | else: 36 | print(f"{file_path} already exists. Skipping download.") 37 | 38 | with open(file_path,"rb") as handle: 39 | data=pickle.load(handle) 40 | 41 | frequency = 1000 42 | 43 | steering = data["steering"] 44 | throttle = data["throttle"] 45 | brake = data["brake"] 46 | 47 | points = len(brake) 48 | end_time = points/frequency 49 | 50 | time = data["time"] 51 | 52 | manoeuvre_M8 = Manoeuvre(steering, throttle, brake, time) 53 | 54 | output_states = OutputStates() 55 | initial_state = StateVector(x = np.array(data["Rel_pos_x"][0]), 56 | y = np.array(data["Rel_pos_y"][0]), 57 | vx = np.array(data["Velocity_X"][0]), 58 | yaw = np.array(data["Yaw"][0])) 59 | 60 | car_parameters_path = "bmw_m8.yaml" 61 | 62 | vehicle_dynamics = VehicleDynamics(initial_state = initial_state, 63 | initial_gear = 1, 64 | frequency = frequency, 65 | car_parameters_path = car_parameters_path) 66 | simulation_range = range(0, len(time)) 67 | 68 | for i in tqdm.tqdm(simulation_range): 69 | try: 70 | output_states.set_states(vehicle_dynamics.tick(*manoeuvre_M8[i])) 71 | except ValueError as e: 72 | print(e) 73 | output_states.padding(len(manoeuvre_M8) - len(output_states)) 74 | plot_function(output_states, manoeuvre_M8,data, 75 | xlim=(-.25,time[-1] + 0.25), 76 | plot_type = "powertrain") 77 | exit() 78 | 79 | plot_function(output_states, manoeuvre_M8, 80 | xlim=(-.25,time[-1] + 0.25), 81 | data=data, 82 | savefig=False, 83 | fig_save_dir="figures/", 84 | static_parameters = StaticParameters(car_parameters_path, frequency), 85 | initial_iteraction = 0) 86 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/StateVector.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class StateVector(object): 5 | def __init__(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0., vx=0., vy=0., vz=0., wx=0., wy=0., wz=0., acc_x = 0, acc_y=0, acc_z=0): 6 | 7 | # Position and Euler angles 8 | self._x = x 9 | self._y = y 10 | self._z = z 11 | self.roll = roll 12 | self.pitch = pitch 13 | self.yaw = yaw 14 | self.vx = vx 15 | self.vy = vy 16 | self.vz = vz 17 | self.wx = wx 18 | self.wy = wy 19 | self.wz = wz 20 | self.acc_x = acc_x 21 | self.acc_y = acc_y 22 | self.acc_z = acc_z 23 | self.wx_dot = 0 24 | self.wy_dot = 0 25 | self.wz_dot = 0 26 | 27 | def __array__(self) -> np.ndarray: 28 | return np.array([self._x, 29 | self._y, 30 | self._z, 31 | self.roll, 32 | self.pitch, 33 | self.yaw, 34 | self.vx, 35 | self.vy, 36 | self.vz, 37 | self.wx, 38 | self.wy, 39 | self.wz, 40 | self.acc_x, 41 | self.acc_y, 42 | self.acc_z, 43 | self.wx_dot, 44 | self.wy_dot, 45 | self.wz_dot]) 46 | 47 | def __repr__(self): 48 | return f"StateVector with x {self.x} y {self.y} z {self.z} roll {self.roll} pitch {self.pitch} yaw {self.yaw} vx {self.vx} vy {self.vy} vz {self.vz} wx {self.wx} wy {self.wy} wz {self.wz} acc_x {self.acc_x} acc_y {self.acc_y} acc_z {self.acc_z} wx_dot {self.wx_dot} wy_dot {self.wy_dot} wz_dot {self.wz_dot}" 49 | pass 50 | 51 | @property 52 | def x(self): 53 | return self._x 54 | 55 | @property 56 | def y(self): 57 | return self._y 58 | 59 | @property 60 | def z(self): 61 | return self._z 62 | 63 | @x.setter 64 | def x(self, value): 65 | if np.isnan(value).any(): 66 | raise ValueError("x_a was set as NaN") 67 | self._x = value 68 | 69 | @y.setter 70 | def y(self, value): 71 | if np.isnan(value).any(): 72 | raise ValueError("x_a was set as NaN") 73 | self._y = value 74 | 75 | @z.setter 76 | def z(self, value): 77 | if np.isnan(value).any(): 78 | raise ValueError("x_a was set as NaN") 79 | self._z = value 80 | 81 | @property 82 | def roll(self): 83 | return self._roll 84 | 85 | @property 86 | def pitch(self): 87 | return self._pitch 88 | 89 | @property 90 | def yaw(self): 91 | return self._yaw 92 | 93 | @roll.setter 94 | def roll(self, value): 95 | if np.isnan(value).any(): 96 | raise ValueError("x_a was set as NaN") 97 | self._roll = value 98 | self.sin_roll = np.sin(value) 99 | self.cos_roll = np.cos(value) 100 | 101 | @pitch.setter 102 | def pitch(self, value): 103 | if np.isnan(value).any(): 104 | raise ValueError("x_a was set as NaN") 105 | self._pitch = value 106 | self.sin_pitch = np.sin(value) 107 | self.cos_pitch = np.cos(value) 108 | 109 | @yaw.setter 110 | def yaw(self, value): 111 | if np.isnan(value).any(): 112 | raise ValueError("x_a was set as NaN") 113 | self._yaw = value 114 | self.sin_yaw = np.sin(value) 115 | self.cos_yaw = np.cos(value) 116 | -------------------------------------------------------------------------------- /vehicle_dynamics/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Main Function for the vehicle dynamics module. 3 | @author: Maikol Funk Drechsler, Yuri Poledna 4 | 5 | ROADVIEW Project 6 | Co-Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. UK and Swiss participants in this project are supported by Innovate UK (contract no. 10045139) and the Swiss State Secretariat for Education, Research and Innovation (Contract no.22.00123) respectevely. 7 | """ 8 | from vehicle_dynamics.structures.OutputStates import OutputStates 9 | from vehicle_dynamics.utils.plot_function import plot_function 10 | from vehicle_dynamics.structures.Manoeuvre import Manoeuvre 11 | from vehicle_dynamics.structures.StateVector import StateVector 12 | 13 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 14 | 15 | from vehicle_dynamics.VehicleDynamics import VehicleDynamics 16 | 17 | import pandas 18 | import numpy as np 19 | import tqdm 20 | 21 | from collections import namedtuple 22 | 23 | import pickle 24 | import os 25 | import urllib.request 26 | 27 | def check_file_or_download(file_path,url): 28 | if not os.path.exists(file_path): 29 | print(f"{file_path} not found. Downloading from {url}...") 30 | urllib.request.urlretrieve(url, file_path) 31 | print("Download complete!") 32 | else: 33 | print(f"{file_path} already exists. Skipping download.") 34 | 35 | file_path_example_data = "example_data/Braking.pickle" 36 | url = "https://github.com/roadview-project/vehicle-dynamics/raw/refs/heads/main/example_data/Braking.pickle" 37 | 38 | os.makedirs(os.path.dirname(file_path_example_data), exist_ok=True) 39 | check_file_or_download(file_path_example_data,url) 40 | 41 | url = "https://raw.githubusercontent.com/roadview-project/vehicle-dynamics/refs/heads/main/bmw_m8.yaml" 42 | file_path = "bmw_m8.yaml" 43 | check_file_or_download(file_path,url) 44 | 45 | 46 | with open(file_path_example_data,"rb") as handle: 47 | data=pickle.load(handle) 48 | 49 | frequency = 1000 50 | 51 | steering = data["steering"] 52 | throttle = data["throttle"] 53 | brake = data["brake"] 54 | 55 | points = len(brake) 56 | end_time = points/frequency 57 | 58 | time = data["time"] 59 | 60 | manoeuvre_M8 = Manoeuvre(steering, throttle, brake, time) 61 | 62 | output_states = OutputStates() 63 | initial_state = StateVector(x = np.array(data["Rel_pos_x"][0]), 64 | y = np.array(data["Rel_pos_y"][0]), 65 | vx = np.array(data["Velocity_X"][0]), 66 | yaw = np.array(data["Yaw"][0])) 67 | 68 | car_parameters_path = "bmw_m8.yaml" 69 | 70 | vehicle_dynamics = VehicleDynamics(initial_state = initial_state, 71 | initial_gear = 1, 72 | frequency = frequency, 73 | car_parameters_path = car_parameters_path) 74 | simulation_range = range(0, len(time)) 75 | 76 | for i in tqdm.tqdm(simulation_range): 77 | try: 78 | output_states.set_states(vehicle_dynamics.tick(*manoeuvre_M8[i])) 79 | except ValueError as e: 80 | print(e) 81 | output_states.padding(len(manoeuvre_M8) - len(output_states)) 82 | plot_function(output_states, manoeuvre_M8,data, 83 | xlim=(-.25,time[-1] + 0.25), 84 | plot_type = "powertrain") 85 | exit() 86 | 87 | plot_function(output_states, manoeuvre_M8, 88 | xlim=(-.25,time[-1] + 0.25), 89 | data=data, 90 | savefig=False, 91 | fig_save_dir="figures/", 92 | static_parameters = StaticParameters(car_parameters_path, frequency), 93 | initial_iteraction = 0) 94 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/StaticParameters.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model - StaticParameters Structure 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | import yaml 9 | import numpy as np 10 | from munch import DefaultMunch 11 | from copy import deepcopy 12 | import munch 13 | 14 | 15 | class StaticParameters(object): 16 | 17 | _initialized=False 18 | 19 | def __init__(self, car_parameters_path, freq=1000, optimization=False): 20 | super(StaticParameters, self).__init__() 21 | with open(car_parameters_path, 'r') as file: 22 | param = yaml.safe_load(file) 23 | 24 | self.convert_YAML2Struct(param) 25 | 26 | self.powertrain.gearbox.gear_max_rpm *= np.pi / 30 27 | self.powertrain.gearbox.gear_min_rpm *= np.pi / 30 28 | self.powertrain.engine.w_table *= np.pi / 30 29 | self.powertrain.engine.minimum_rpm *= np.pi / 30 30 | self.powertrain.engine.maximum_rpm *= np.pi / 30 31 | self.powertrain.engine.idle_rpm *= np.pi / 30 32 | 33 | self.wheel_base = self.body.lr + self.body.lf 34 | self.track_width = self.body.wl + self.body.wr 35 | 36 | wd1 = (self.body.lr / (self.wheel_base)) * (self.body.wr / self.track_width) 37 | wd2 = (self.body.lf / (self.wheel_base)) * (self.body.wr / self.track_width) 38 | wd3 = (self.body.lr / (self.wheel_base)) * (self.body.wl / self.track_width) 39 | wd4 = (self.body.lf / (self.wheel_base)) * (self.body.wl / self.track_width) 40 | 41 | self.wd = np.array([wd1, wd2, wd3, wd4]) 42 | self.tire.dynamic_radius = np.array(self.tire.dynamic_radius) 43 | self.sprung_mass = self.body.mass * self.wd - self.suspension.unsprung_mass 44 | 45 | self.time_step = 1. / freq 46 | 47 | self.aerodynamics_front = self.aerodynamics.air_drag_coefficient * self.aerodynamics.front_area * self.aerodynamics.air_density 48 | 49 | self.gravity = 9.81 50 | 51 | if not optimization: 52 | self._initialized = True 53 | 54 | 55 | def __setattr__(self, name, value): 56 | if self._initialized: 57 | raise AttributeError("We are not in the saving attributes phase of this code, Do Better") 58 | else: 59 | object.__setattr__(self, name, value) 60 | 61 | def convert_YAML2Struct(self, yaml): 62 | data = DefaultMunch.fromDict(yaml) 63 | for name in dir(data): 64 | current_key = getattr(data, name) 65 | setattr(data, name, self.check_munch_key(current_key)) 66 | setattr(self, name, getattr(data, name)) 67 | 68 | def check_munch_key(self, current_key): 69 | for i in dir(current_key): 70 | if isinstance(getattr(current_key, i), list): 71 | setattr(current_key, i, np.array(getattr(current_key, i))) 72 | elif isinstance(getattr(current_key, i), munch.DefaultMunch): 73 | setattr(current_key, i, self.check_munch_key(getattr(current_key, i))) 74 | return current_key 75 | 76 | 77 | def main(): 78 | car_parameters_path = "../../Audi_r8.yaml" 79 | 80 | car_parameters = StaticParameters(car_parameters_path,optimization=True) 81 | 82 | print(car_parameters.aerodynamics.air_drag_coefficient) 83 | print(car_parameters.powertrain.torque_converter.efficiency_function) 84 | print(car_parameters.powertrain.torque_converter.efficiency_function) 85 | car_parameters.gravity = 1 86 | print(car_parameters.gravity) 87 | 88 | car_parameters._initialized = True 89 | 90 | car_parameters.gravity = 9.8 91 | print(car_parameters.gravity) 92 | 93 | if __name__ == '__main__': 94 | main() 95 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/OutputStates.py: -------------------------------------------------------------------------------- 1 | from vehicle_dynamics.structures.TireForces import TireForces 2 | from vehicle_dynamics.structures.StateVector import StateVector 3 | from vehicle_dynamics.structures.Displacement import Displacement 4 | from vehicle_dynamics.structures.StrutForce import StrutForce 5 | from vehicle_dynamics.structures.WheelHubForce import WheelHubForce 6 | from vehicle_dynamics.structures.AngularWheelPosition import AngularWheelPosition 7 | from collections import namedtuple 8 | from copy import deepcopy, copy 9 | import numpy as np 10 | OutputState = namedtuple('OutputState', 'compiled_wheel_forces delta displacement engine_w f_za f_zr gear powertrain_net_torque slip_x slip_y sum_f_wheel wheel_w_vel x_a x_rf x_rr suspension_force') 11 | 12 | 13 | class OutputStates(object): 14 | """OutputStates Class 15 | makes a deep copy of the simulation parameters, and stores them in a list, any parameter is available using the .attribute[] operator""" 16 | 17 | def __init__(self): 18 | super(OutputStates, self).__init__() 19 | self.compiled_wheel_forces = [] 20 | self.delta = [] 21 | self.displacement = [] 22 | self.engine_w = [] 23 | self.f_za = [] 24 | self.f_zr = [] 25 | self.gear = [] 26 | self.get_data = [] 27 | self.powertrain_net_torque = [] 28 | self.slip_x = [] 29 | self.slip_y = [] 30 | self.sum_f_wheel = [] 31 | self.wheel_w_vel = [] 32 | self.x_a = [] 33 | self.x_rf = [] 34 | self.x_rr = [] 35 | self.suspension_force = [] 36 | 37 | def __len__(self): 38 | return len(self.compiled_wheel_forces) 39 | 40 | def set_states(self, current_states): 41 | self.compiled_wheel_forces.append(copy(current_states.compiled_wheel_forces)) 42 | self.delta.append(copy(current_states.delta)) 43 | self.displacement.append(copy(current_states.displacement)) 44 | self.engine_w.append(copy(current_states.engine_w)) 45 | self.f_za.append(deepcopy(current_states.f_za)) 46 | self.f_zr.append(deepcopy(current_states.f_zr)) 47 | self.gear.append(copy(current_states.gear)) 48 | self.powertrain_net_torque.append(copy(current_states.powertrain_net_torque)) 49 | self.slip_x.append(copy(current_states.slip_x)) 50 | self.slip_y.append(copy(current_states.slip_y)) 51 | self.sum_f_wheel.append(copy(current_states.sum_f_wheel)) 52 | self.wheel_w_vel.append(copy(current_states.wheel_w_vel)) 53 | self.x_a.append(deepcopy(current_states.x_a)) 54 | self.x_rf.append(deepcopy(current_states.x_rf)) 55 | self.x_rr.append(deepcopy(current_states.x_rr)) 56 | self.suspension_force.append(deepcopy(current_states.suspension_force)) 57 | 58 | def __getitem__(self, items): 59 | return OutputState(self.compiled_wheel_forces, 60 | self.delta, 61 | self.displacement, 62 | self.engine_w, 63 | self.f_za, 64 | self.f_zr, 65 | self.gear, 66 | self.powertrain_net_torque, 67 | self.slip_x, 68 | self.slip_y, 69 | self.sum_f_wheel, 70 | self.wheel_w_vel, 71 | self.x_a, 72 | self.x_rf, 73 | self.x_rr, 74 | self.suspension_force) 75 | 76 | def padding(self, value): 77 | self.compiled_wheel_forces.extend([self.compiled_wheel_forces[-1]] * value) 78 | self.delta.extend([self.delta[-1]] * value) 79 | self.displacement.extend([self.displacement[-1]] * value) 80 | self.engine_w.extend([self.engine_w[-1]] * value) 81 | self.f_za.extend([self.f_za[-1]] * value) 82 | self.f_zr.extend([self.f_zr[-1]] * value) 83 | self.gear.extend([self.gear[-1]] * value) 84 | self.powertrain_net_torque.extend([self.powertrain_net_torque[-1]] * value) 85 | self.slip_x.extend([self.slip_x[-1]] * value) 86 | self.slip_y.extend([self.slip_y[-1]] * value) 87 | self.sum_f_wheel.extend([self.sum_f_wheel[-1]] * value) 88 | self.wheel_w_vel.extend([self.wheel_w_vel[-1]] * value) 89 | self.x_a.extend([self.x_a[-1]] * value) 90 | self.x_rf.extend([self.x_rf[-1]] * value) 91 | self.x_rr.extend([self.x_rr[-1]] * value) 92 | self.suspension_force.extend([self.suspension_force[-1]] * value) 93 | pass 94 | -------------------------------------------------------------------------------- /bmw_m8.yaml: -------------------------------------------------------------------------------- 1 | powertrain: 2 | bias: [0.25,0.25,0.25,0.25] #OPTIMIZE 3 | engine: 4 | idle_rpm : 710 #Measured from the real data 5 | minimum_rpm: 600 #rpm # Minimum in all tests 6 | maximum_rpm: 7324 #rpm # Maximum in all tests 7 | w_table: [600.0,1821,5837,6835,7324] # [rpm] # automobile-catalog.com 8 | torque_max: [290,750,750,640,516] # [Nm] # Torque extrapolated from 1000 to 600 rpm 9 | inertia: 1.2 #OPTIMIZE # engine_inertia [Kgm^2] 0.8 10 | 11 | torque_converter: 12 | speed_ratio: [0,0.23,0.92, 1] #Manually adapted from the real data 13 | ratio: [2.5, 1.3, 0.95, 0.95] #Manually adapted from the real data 14 | factor: [0.04582197, 0.02773522, 0.01706852, 0.01279646, 0.01051292, 0.00982103, 0.00913212, 0.00769946, 0.00682872, 0.00627394, 0.00306653] 15 | CONVERTER_SYNC_TIME: 10 #Manually adapted on the real data 16 | lock_up_ratio: 0.92 #Manually adapted on the real data 17 | 18 | gearbox: 19 | MIN_GEAR_CHANGE_INTERVAL: 200 #Stimated from Double lane change_Dry_Automatic_Light_70_Repetition 1 20 | #N, 1st, 2nd, 3rd, 4th, 5th, 6th, 7th, 8th 21 | gear_ratio: [0, 5.0, 3.2, 2.143, 1.720, 1.313, 1.0, 0.823, 0.64] #BMW Technical Data 22 | #N, 1st, 2nd, 3rd, 4th, 5th, 6th, 7th, 8th 23 | gear_max_rpm: [[0.0, 6500, 6200, 6200, 6200, 6200, 6200, 6200, 6200], #0% [0.0, 1750, 2350, 2000, 2000, 2000, 2000, 2000, 2000], #0% 24 | [0.0, 1750, 2350, 2000, 2000, 2000, 2000, 2000, 2000], #10% 25 | [0.0, 1750, 2350, 2000, 2000, 2000, 2000, 2000, 2000], #20% 26 | [0.0, 1750, 2350, 2000, 2000, 2000, 2000, 2000, 2000], #30% 27 | [0.0, 2250, 2350, 2000, 2000, 2000, 2000, 2000, 2000], #40% 28 | [0.0, 2250, 2550, 2750, 2750, 2750, 2750, 2750, 2750], #50% 29 | [0.0, 2250, 2850, 3650, 3650, 3650, 3650, 3650, 3650], #60% 30 | [0.0, 2500, 3050, 3650, 3650, 3650, 3650, 3650, 3650], #70% 31 | [0.0, 2850, 4300, 4300, 4300, 4300, 4300, 4300, 4300], #80% 32 | [0.0, 4700, 4700, 4700, 4700, 4700, 4700, 4700, 4700], #90% 33 | [0.0, 6500, 6200, 6200, 6200, 6200, 6200, 6200, 6200]] #100% # Calculated 34 | 35 | gear_min_rpm: [[0.0, 750, 800, 1300, 1400, 1200, 1000, 1000, 1000], #0% 36 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #10% 37 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #20% 38 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #30% 39 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #40% 40 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #50% 41 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #60% 42 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #70% 43 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #80% 44 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600], #90% 45 | [0.0, 1000, 1500, 1750, 2000, 2375, 2600, 2600, 2600]] #100% # Calculated 46 | 47 | efficiency: 0.95 #0.95 # Efficiency of the whole powertrain system #OPTIMIZE 48 | inertia: 0.1 # Gear Box Inertia [Kgm^2] #OPTIMIZE 0.05 49 | 50 | differential: 51 | ratio: 3.154 # Differential ratio #BMW Technical Data 52 | driveshaft_inertia: 0.9 #7.8596 # drive shaft Inertia [Kgm^2] #OPTIMIZE 53 | 54 | brake: 55 | max_braking_torque: 17500. # [N/m] 56 | brake_bias: [0.30,0.20,0.30,0.20] # [%] 57 | 58 | steering: 59 | maximum_steering_angle: 20. # maximum steering angle [rad] # Need to be checked! 25.1 60 | ratio: 14.3 #BMW Technical Data 61 | 62 | tire: 63 | relaxation: 0.1 64 | inertia: [1.5, 2.0, 1.5, 2.0] # Wheel Inertia on each wheel [Kgm^2] #OPTIMIZE 65 | rolling_resistance_coefficient: 0.08 66 | longitudinal: 67 | peak_friction : 1.1 68 | shape_factor : 1.3 69 | slip_stiffness : 22 70 | lateral: 71 | peak_friction : 1.1 72 | shape_factor : 1.3 73 | cornering_coefficient : 30. 74 | dynamic_radius: [0.35025, # FL - 275/35 ZR20 75 | 0.35375, # RL - 285/35 ZR20 76 | 0.35025, # FR - 275/35 ZR20 77 | 0.35375] # RR - 285/35 ZR20 # Tire Radius [m] 78 | 79 | aerodynamics: 80 | air_drag_coefficient: 0.33 # BMW Technical Data 81 | front_area: 2.25 # BMW Technical Data 82 | air_density: 1.2 # Air Density 83 | 84 | suspension: 85 | roll_bar_stiffness: 2000 #5546.58 #OPTIMIZE 86 | pitch_centre_height: 0.05 #OPTIMIZE distance from the CG to the pitch center 87 | roll_centre_height: 0.05 #OPTIMIZE distance from the CG to the roll center 88 | unsprung_mass: [100.0, 99.0, 100.0, 99.0] 89 | 90 | 91 | spring_rate: [18795.0, 14184.0, 18795.0, 14184.0] 92 | #damping_rate: [5026.0, 5249.0, 5026.0, 5249.0] 93 | damping_rate: [1200.0, 1200.0, 1200.0, 1200.0] 94 | 95 | body: 96 | lf: 1.331 # x-distance from Vehicle CoG to the front axle [m] # Measured 97 | lr: 1.496 # x-distance from Vehicle Cog to the rear axle [m] # Measured 98 | wl: 0.804 # y-distance from Vehicle CoG to the left chassis point [m] # Measured 99 | wr: 0.826 # y-distance from Vehicle CoG to the right chassis point [m] # Measured 100 | sz: -0.078 # z-distance from Vehicle CoG to the chassis point [m] # Measured NEED TO BE CHECKED - POSSIBLE OPTIMIZATION 101 | mass: 2074.4 # Vehicle Mass # Measured 102 | 103 | # Body inertias 104 | i_x_s: 800.0 # moment of inertia for sprung mass in roll [kg m^2] 818.05 #OPTIMIZE 105 | i_y_s: 2638.0 # moment of inertia for sprung mass in pitch [kg m^2] 2277. #OPTIMIZE 106 | i_z: 4983.0 # moment of inertia for sprung mass in yaw [kg m^2] 1921.3 #OPTIMIZE 107 | -------------------------------------------------------------------------------- /vehicle_dynamics/modules/wheels.py: -------------------------------------------------------------------------------- 1 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 2 | from vehicle_dynamics.structures.StateVector import StateVector 3 | from vehicle_dynamics.utils.CurrentStates import CurrentStates 4 | from vehicle_dynamics.utils.import_data_CM import import_data_CM 5 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 6 | from vehicle_dynamics.utils.plot_function import plot_function 7 | from copy import copy 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import logging 11 | import yaml 12 | 13 | 14 | class Wheels: 15 | def __init__(self, static_parameters, logger: logging.Logger): 16 | self.static_parameters = static_parameters 17 | # x-position from Vehicle CoG to the front axle [m] 18 | self.long_f = static_parameters.body.lf 19 | # x-position from Vehicle Cog to the rear axle [m] 20 | self.long_r = - static_parameters.body.lr 21 | # y-position from Vehicle CoG to the left chassis point [m] 22 | self.lat_l = static_parameters.body.wl 23 | # y-position from Vehicle CoG to the right chassis point [m] 24 | self.lat_r = -static_parameters.body.wr 25 | pass 26 | 27 | def wheels(self, current_state: CurrentStates, steering_input: float): 28 | """ 29 | wheels is a function that calculates the current angle 30 | of the frontal wheel based on the vehicle coordinate system, 31 | where the output delta is steering angle of the frontal wheels. 32 | 33 | This method calculate the wheel slip on each wheel. This is done using the relative 34 | velocity between the wheel angular speed and the vehicle speed. 35 | 36 | Tire model calculates the wheel forces fx and fy using the Magic Formula 37 | 38 | Required current_state from static_parameters: 39 | 1. steering_lock 40 | 2. steering_ratio 41 | 3. dynamic_radius 42 | Required Arguments: 43 | 1.steering_inputtire. 44 | 2. x_a 45 | 2.01 vx 46 | 2.02 vy 47 | 3. wheel_w_vel 48 | Returns: 49 | 1. delta 50 | 2. slip_x 51 | 3. slip_y 52 | 53 | """ 54 | # Convert Steering input [-1,1] to wheel steering (delta) 55 | steering_angle = steering_input * self.static_parameters.steering.maximum_steering_angle 56 | current_state.delta = steering_angle / self.static_parameters.steering.ratio 57 | 58 | # Slip Calculation 59 | MINIMUM_SPEED_VALUE = 15 60 | if (abs(self.static_parameters.tire.dynamic_radius * current_state.wheel_w_vel).all() <= MINIMUM_SPEED_VALUE) and (abs(current_state.x_a.vx) <= MINIMUM_SPEED_VALUE): 61 | current_state.slip_x = ((((self.static_parameters.tire.dynamic_radius * current_state.wheel_w_vel) - current_state.x_a.vx) / (MINIMUM_SPEED_VALUE))) 62 | else: 63 | # equation 11.30 Bardini 64 | current_state.slip_x = ((((self.static_parameters.tire.dynamic_radius * current_state.wheel_w_vel) - current_state.x_a.vx) / np.maximum(np.absolute(self.static_parameters.tire.dynamic_radius * current_state.wheel_w_vel), np.absolute(current_state.x_a.vx)))) 65 | 66 | 67 | previous_slip_y = copy(current_state.slip_y) 68 | # Refenrence from VTI Y Slip angle 69 | if (current_state.x_a.vx <= MINIMUM_SPEED_VALUE and current_state.x_a.wz * self.lat_r <= MINIMUM_SPEED_VALUE): 70 | current_state.slip_y[0] = (current_state.delta - np.arctan((current_state.x_a.vy + self.long_f * current_state.x_a.wz) / MINIMUM_SPEED_VALUE)) # Front Left 71 | current_state.slip_y[1] = (- np.arctan((current_state.x_a.vy + self.long_r * current_state.x_a.wz) / MINIMUM_SPEED_VALUE)) # Rear Left 72 | current_state.slip_y[2] = (current_state.delta - np.arctan((current_state.x_a.vy + self.long_f * current_state.x_a.wz) / MINIMUM_SPEED_VALUE)) # Front Right 73 | current_state.slip_y[3] = (- np.arctan((current_state.x_a.vy + self.long_r * current_state.x_a.wz) / MINIMUM_SPEED_VALUE)) # Rear Right 74 | else: 75 | current_state.slip_y[0] = (current_state.delta - np.arctan((current_state.x_a.vy + self.long_f * current_state.x_a.wz) / (current_state.x_a.vx - self.lat_l * current_state.x_a.wz))) #- self.static_parameters.tire.relaxation*current_state.slip_y_rate[0]/current_state.x_a.vx # Front Left 76 | current_state.slip_y[1] = (- np.arctan((current_state.x_a.vy + self.long_r * current_state.x_a.wz) / (current_state.x_a.vx - self.lat_l * current_state.x_a.wz))) #- self.static_parameters.tire.relaxation*current_state.slip_y_rate[1]/current_state.x_a.vx # Rear Left 77 | current_state.slip_y[2] = (current_state.delta - np.arctan((current_state.x_a.vy + self.long_f * current_state.x_a.wz) / (current_state.x_a.vx - self.lat_r * current_state.x_a.wz))) #- self.static_parameters.tire.relaxation*current_state.slip_y_rate[2]/current_state.x_a.vx # Front Right 78 | current_state.slip_y[3] = (- np.arctan((current_state.x_a.vy + self.long_r * current_state.x_a.wz) / (current_state.x_a.vx - self.lat_r * current_state.x_a.wz))) #- self.static_parameters.tire.relaxation*current_state.slip_y_rate[3]/current_state.x_a.vx # Rear Right 79 | 80 | current_state.slip_y_rate = (previous_slip_y - current_state.slip_y)/self.static_parameters.time_step 81 | 82 | # Tire Model 83 | 84 | current_state.x_rf.fx = current_state.f_zr.wheel_load_z * self.static_parameters.tire.longitudinal.peak_friction * np.sin(self.static_parameters.tire.longitudinal.shape_factor * np.arctan((self.static_parameters.tire.longitudinal.slip_stiffness / (self.static_parameters.tire.longitudinal.shape_factor * self.static_parameters.tire.longitudinal.peak_friction)) * current_state.slip_x)) 85 | current_state.x_rf.fy = current_state.f_zr.wheel_load_z * self.static_parameters.tire.lateral.peak_friction * np.sin(self.static_parameters.tire.lateral.shape_factor * np.arctan((self.static_parameters.tire.lateral.cornering_coefficient / (self.static_parameters.tire.lateral.shape_factor * self.static_parameters.tire.lateral.peak_friction)) * current_state.slip_y)) 86 | 87 | 88 | 89 | current_state.compiled_wheel_forces = np.array([current_state.x_rf.fx, current_state.x_rf.fy, current_state.f_zr.wheel_load_z]) 90 | 91 | delta = np.array([current_state.delta, 0.0, current_state.delta, 0.0]) # FL, RL, FR, RR 92 | for i in range(4): 93 | current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, i] = current_state.x_rf.fx[i] * np.cos(delta[i]) - current_state.x_rf.fy[i] * np.sin(delta[i]) 94 | current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, i] = current_state.x_rf.fy[i] * np.cos(delta[i]) + current_state.x_rf.fx[i] * np.sin(delta[i]) 95 | 96 | final_ratio = self.static_parameters.powertrain.gearbox.gear_ratio[current_state.gear] * self.static_parameters.powertrain.differential.ratio 97 | current_state.x_rr.pho_r_2dot = (current_state.powertrain_net_torque - current_state.x_rf.fx * self.static_parameters.tire.dynamic_radius - self.static_parameters.tire.rolling_resistance_coefficient*current_state.f_zr.wheel_load_z) / (self.static_parameters.tire.inertia + self.static_parameters.powertrain.gearbox.inertia * final_ratio ** 2 + self.static_parameters.powertrain.differential.driveshaft_inertia * self.static_parameters.powertrain.differential.ratio ** 2) 98 | current_state.wheel_w_vel = current_state.wheel_w_vel + (current_state.x_rr.pho_r_2dot * self.static_parameters.time_step) # rad/s 99 | for i in range(len(current_state.wheel_w_vel)): 100 | if current_state.wheel_w_vel[i] <= 0.0: 101 | current_state.wheel_w_vel[i] = 0.0 102 | current_state.x_rr.pho_r_2dot[i] = 0.0 103 | 104 | return current_state 105 | -------------------------------------------------------------------------------- /vehicle_dynamics/modules/powertrain.py: -------------------------------------------------------------------------------- 1 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 2 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 3 | from vehicle_dynamics.utils.CurrentStates import CurrentStates 4 | from vehicle_dynamics.utils.import_data_CM import import_data_CM 5 | from vehicle_dynamics.utils.plot_function import plot_function 6 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 7 | 8 | from vehicle_dynamics.structures.OutputStates import OutputStates 9 | from vehicle_dynamics.structures.Manoeuvre import Manoeuvre 10 | 11 | import logging 12 | from scipy.interpolate import interp1d 13 | import numpy as np 14 | 15 | 16 | class Powertrain(object): 17 | """ 18 | Powertrain is a class calculates the current Torque delivered by the engine to the wheels. 19 | For that is necessary select the proper gear and find the torque 20 | converter multiplicator. 21 | gear_change() function uses engine angular velocity and car linear speed (vx) 22 | torque_converter() multiplication is based on input and output rotation ratio. 23 | 24 | Required current_state from static_parameters: 25 | 1.engine_w_table, 26 | 2.torque_max_table, 27 | 3.gear_ratio, 28 | 4.diff, 29 | 5.diff_ni, 30 | 6.transmition_ni, 31 | 7.gear_selection, 32 | 8.engine_inertia, 33 | 9.axel_inertia, 34 | 10.gearbox_inertia, 35 | 11.shaft_inertia, 36 | 12.wheel_inertia, 37 | 13.max_brake_torque, 38 | 14.brake_bias, 39 | 40 | Required Arguments: 41 | 1. throttle 42 | 2. brake 43 | 3. acc_x 44 | 4.gear[t-1] 45 | 5. vx 46 | 47 | Returns: (current_state) 48 | 1. Engine_w 49 | 2. Gear 50 | 3. Torque of the engine on the wheels 51 | 4. prev_gear 52 | 5. current_gear 53 | 54 | """ 55 | 56 | def __init__(self, static_parameters: StaticParameters, logger: LocalLogger): 57 | super(Powertrain, self).__init__() 58 | self.logger = logger 59 | self.torque_interpolation = interp1d(static_parameters.powertrain.engine.w_table, static_parameters.powertrain.engine.torque_max) 60 | self.MIN_GEAR_CHANGE_INTERVAL = static_parameters.powertrain.gearbox.MIN_GEAR_CHANGE_INTERVAL 61 | self.CONVERTER_SYNC_TIME = static_parameters.powertrain.torque_converter.CONVERTER_SYNC_TIME 62 | self.current_sync = 0 63 | self.current_grace_period = 0 64 | self.static_parameters = static_parameters 65 | self.μ_funct = interp1d(static_parameters.powertrain.torque_converter.speed_ratio, 66 | static_parameters.powertrain.torque_converter.ratio) 67 | self.k_in_funct = interp1d(np.array(np.linspace(0., 1., len(static_parameters.powertrain.torque_converter.factor))), np.array(static_parameters.powertrain.torque_converter.factor)) 68 | self.previous_throtle = 0 69 | 70 | def gear_change_rpm(self, current_state: CurrentStates, throttle: float): 71 | if self.current_grace_period > 0: 72 | self.current_grace_period -= 1 73 | return False 74 | 75 | # Gearbox up or down shifting 76 | if current_state.engine_w > self.static_parameters.powertrain.gearbox.gear_max_rpm[round(throttle * 10)][current_state.gear]: 77 | if current_state.gear + 1 >= self.static_parameters.powertrain.gearbox.gear_ratio.size: 78 | return False 79 | current_state.gear = current_state.gear + 1 80 | self.current_grace_period = self.MIN_GEAR_CHANGE_INTERVAL 81 | return True 82 | 83 | elif current_state.engine_w < self.static_parameters.powertrain.gearbox.gear_min_rpm[round(throttle * 10)][current_state.gear]: 84 | if current_state.gear - 1 < 1: 85 | return False 86 | self.current_grace_period = self.MIN_GEAR_CHANGE_INTERVAL 87 | current_state.gear = current_state.gear - 1 88 | return True 89 | 90 | def powertrain(self, current_state: CurrentStates, throttle: float, brake: float): 91 | # Update Converter engine_w 92 | # Based on page 3 from Generating Proper Integrated Dynamic Models for Vehicle Mobility (Loucas S. Louca) 93 | # Based on the whell velocity and engine engine_w the torque on the 94 | 95 | if self.gear_change_rpm(current_state, throttle): 96 | self.current_sync = self.CONVERTER_SYNC_TIME 97 | 98 | if self.current_sync > 0: 99 | torque_converter_out = 0 100 | self.current_sync -= 1 101 | throttle = 0.2 102 | 103 | # Calculate torque provided by the engine based on the engine engine_w 104 | torque_available = self.torque_interpolation(current_state.engine_w) 105 | 106 | if self.previous_throtle - throttle != 0 : 107 | self.current_grace_period += 2 108 | self.previous_throtle = throttle 109 | else: 110 | self.previous_throtle = throttle 111 | 112 | gas_pedal = throttle 113 | 114 | # checking for idle state 115 | if current_state.engine_w < self.static_parameters.powertrain.engine.idle_rpm: 116 | #throttle = 1 117 | current_state.engine_w = self.static_parameters.powertrain.engine.idle_rpm 118 | 119 | # find the torque delivered by the engine 120 | engine_torque = (throttle * torque_available) 121 | 122 | final_ratio = self.static_parameters.powertrain.gearbox.gear_ratio[current_state.gear] * self.static_parameters.powertrain.differential.ratio 123 | 124 | turbine_w = final_ratio * np.mean(current_state.wheel_w_vel) 125 | 126 | torque_converter_ratio = turbine_w / current_state.engine_w 127 | 128 | 129 | if torque_converter_ratio >= self.static_parameters.powertrain.torque_converter.lock_up_ratio: 130 | # Clutch Mode 131 | current_state.engine_w = turbine_w 132 | torque_converter_out = engine_torque 133 | 134 | else: 135 | torque_converter_ratio = 0 if torque_converter_ratio < 0 else torque_converter_ratio 136 | # Torque Converter 137 | k_in = self.k_in_funct(torque_converter_ratio) 138 | μ = self.μ_funct(torque_converter_ratio) 139 | 140 | torque_converter_in = k_in * (current_state.engine_w ** 2) 141 | torque_converter_out = μ * torque_converter_in 142 | 143 | engine_wdot = (engine_torque - torque_converter_in) / self.static_parameters.powertrain.engine.inertia 144 | current_state.engine_w = (current_state.engine_w + engine_wdot * self.static_parameters.time_step) 145 | 146 | # Gillespie equation 2-7 147 | traction_torque = torque_converter_out * final_ratio * self.static_parameters.powertrain.gearbox.efficiency 148 | 149 | #if traction_torque > 5000.0: 150 | # traction_torque = 5000.0 151 | 152 | wheel_torque = traction_torque * self.static_parameters.powertrain.bias 153 | 154 | #if brake > 0: 155 | # wheel_torque = np.zeros(4) 156 | 157 | # --------------------Break Torque ------------------------- 158 | brake_torque = brake * self.static_parameters.brake.max_braking_torque * self.static_parameters.brake.brake_bias 159 | 160 | # -------------------- Total Torque ------------------------- 161 | if np.mean((wheel_torque - brake_torque)) <= 0 and current_state.x_a.vx <= 1e-6: 162 | current_state.powertrain_net_torque = np.zeros(4) 163 | self.logger.debug(f"case 1") 164 | 165 | elif current_state.x_a.vx <= 1e-6 and brake > 0: 166 | # IDLE state 167 | current_state.powertrain_net_torque = np.zeros(4) 168 | self.logger.debug(f"case 2") 169 | 170 | else: 171 | current_state.powertrain_net_torque = wheel_torque - brake_torque 172 | 173 | # Unecessary but for code safety 174 | if current_state.engine_w > self.static_parameters.powertrain.engine.maximum_rpm: 175 | current_state.engine_w = self.static_parameters.powertrain.engine.maximum_rpm 176 | 177 | return current_state 178 | 179 | 180 | def main(static_parameters, current_state, data, logger, savefig=False): 181 | import yaml 182 | from tqdm import tqdm 183 | 184 | steering = data["steering"] 185 | throttle = data["throttle"] 186 | brake = data["brake"] 187 | 188 | time = data["time"] 189 | 190 | points = len(brake) 191 | 192 | manoeuvre = Manoeuvre(steering, throttle, brake, time) 193 | logger.info("loaded SimulationData") 194 | 195 | 196 | fl,rl,fr,rr = (data["v_wheel_fl"], 197 | data["v_wheel_fr"], 198 | data["v_wheel_rl"], 199 | data["v_wheel_rr"]) 200 | 201 | simulation_range = range(0, len(time)) 202 | 203 | wheel_w_vel = np.array([[fl[i],rl[i],fr[i],rr[i]] for i in range(len(fl))]) 204 | vx = data ["Velocity_X"] 205 | vy = data ["Velocity_Y"] 206 | 207 | yawrate = data["Yaw_Velocity"] 208 | 209 | powertrain = Powertrain(static_parameters, logger) 210 | test_function = powertrain.powertrain 211 | output_states = OutputStates() 212 | 213 | for i in tqdm(simulation_range): 214 | current_state.x_a.vx = vx[i] 215 | current_state.wheel_w_vel = wheel_w_vel[i] 216 | try: 217 | current_state = test_function(current_state, 218 | throttle = manoeuvre[i].throttle, 219 | brake = manoeuvre[i].brake) 220 | output_states.set_states(current_state) 221 | except ValueError as e: 222 | print(e) 223 | output_states.padding(len(manoeuvre) - len(output_states)) 224 | plot_function(output_states, manoeuvre, sync_adma, sync_zgw, savefig, plot_type="powertrain", xlim=(-.25,sync_zgw[-1].timestamp-sync_zgw[0].timestamp + 0.25)) 225 | exit() 226 | 227 | 228 | plot_function(output_states, manoeuvre, data, savefig, plot_type="powertrain", xlim=(-.25,time[-1]-time[0] + 0.25)) 229 | 230 | 231 | if __name__ == '__main__': 232 | import pickle 233 | 234 | PATH_TO_DATA = "../../example_data/Braking.pickle" 235 | with open(PATH_TO_DATA,"rb")as handle: 236 | data=pickle.load(handle) 237 | 238 | from vehicle_dynamics.structures.StateVector import StateVector 239 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 240 | static_parameters = StaticParameters("../../bmw_m8.yaml", freq = 1000) 241 | 242 | function_name = "Powertrain" 243 | logger = LocalLogger(function_name).logger 244 | logger.info("loaded current_state") 245 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 246 | static_parameters = StaticParameters("../../BMW_M8.yaml") 247 | from vehicle_dynamics.structures.StateVector import StateVector 248 | 249 | initial_state = StateVector(x = np.array( data["Rel_pos_x"][0]), 250 | y = np.array(data["Rel_pos_y"][0]), 251 | vx = np.array( data["Velocity_X"][0]), 252 | yaw = np.array( data["Yaw"][0])) 253 | 254 | current_state = CurrentStates(static_parameters, logger=logger,initial_state=initial_state) 255 | main(static_parameters, current_state, data, logger, False) 256 | -------------------------------------------------------------------------------- /vehicle_dynamics/utils/CurrentStates.py: -------------------------------------------------------------------------------- 1 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 2 | from vehicle_dynamics.structures.TireForces import TireForces 3 | from vehicle_dynamics.structures.StateVector import StateVector 4 | from vehicle_dynamics.structures.Displacement import Displacement 5 | from vehicle_dynamics.structures.StrutForce import StrutForce 6 | from vehicle_dynamics.structures.WheelHubForce import WheelHubForce 7 | from vehicle_dynamics.structures.AngularWheelPosition import AngularWheelPosition 8 | 9 | from vehicle_dynamics.utils.import_data_CM import import_data_CM 10 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 11 | 12 | 13 | from scipy.interpolate import interp1d 14 | from numpy.linalg import inv 15 | import numpy as np 16 | import yaml 17 | import math 18 | 19 | 20 | class CurrentStates(object): 21 | """This class initialize the values of a vehicular dynamic model. """ 22 | 23 | def __init__(self, static_parameters, freq = 1000, initial_state = np.zeros(15), initial_gear = 1, logger = 1): 24 | super(CurrentStates, self).__init__() 25 | self.delta = 0 26 | 27 | self.engine_w = static_parameters.powertrain.engine.idle_rpm 28 | self.gear = initial_gear # gear selector 29 | self.wheel_w_vel = np.zeros(4) 30 | 31 | # State initiate with the position, orientation and speed provided by the arguments, acc = 0; 32 | if isinstance(initial_state, np.ndarray): 33 | self.x_a = StateVector(*initial_state) # initial_state 34 | elif isinstance(initial_state, StateVector): 35 | self.x_a = initial_state 36 | 37 | self.reference_zCG = self.x_a.z 38 | 39 | Ф = self.x_a.roll 40 | θ = self.x_a.pitch 41 | Ψ = self.x_a.yaw 42 | cos = np.cos 43 | sin = np.sin 44 | sin_Ф = self.x_a.sin_roll 45 | cos_Ф = self.x_a.cos_roll 46 | sin_θ = self.x_a.sin_pitch 47 | cos_θ = self.x_a.cos_pitch 48 | sin_Ψ = self.x_a.sin_yaw 49 | cos_Ψ = self.x_a.cos_yaw 50 | 51 | # Wheel initiate stoped 52 | self.x_rr = AngularWheelPosition(pho_r=np.zeros(4), pho_r_dot = np.zeros(4), pho_r_2dot =np.zeros(4)) 53 | self.x_rf = TireForces(fx =np.zeros(4), fy = np.zeros(4), wheel_forces_transformed_force2vehicle_sys = np.ones((3, 4), dtype=float)) 54 | self.displacement = Displacement(static_suspension=(static_parameters.body.mass * static_parameters.wd * static_parameters.gravity) / static_parameters.suspension.spring_rate, 55 | suspension = np.zeros(4), 56 | suspension_dot=np.zeros(4), 57 | zr_dot=np.zeros(4), 58 | zr_2dot=np.zeros(4)) 59 | zCG = self.x_a.z 60 | static_cg_height = static_parameters.body.sz 61 | long_f = static_parameters.body.lf 62 | # x-position from Vehicle Cog to the rear axle [m] 63 | long_r = - static_parameters.body.lr 64 | # y-position from Vehicle CoG to the left chassis point [m] 65 | lat_l = static_parameters.body.wl 66 | # y-position from Vehicle CoG to the right chassis point [m] 67 | lat_r = - static_parameters.body.wr 68 | 69 | zCG = self.x_a.z 70 | static_cg_height = self.reference_zCG 71 | 72 | displacement_FL = zCG - static_cg_height - (sin_θ * long_f) + (sin_Ф * lat_l) - self.displacement.road[0] 73 | displacement_RL = zCG - static_cg_height - (sin_θ * long_r) + (sin_Ф * lat_l) - self.displacement.road[1] 74 | displacement_FR = zCG - static_cg_height - (sin_θ * long_f) + (sin_Ф * lat_r) - self.displacement.road[2] 75 | displacement_RR = zCG - static_cg_height - (sin_θ * long_r) + (sin_Ф * lat_r) - self.displacement.road[3] 76 | 77 | displacement_suspension = np.array([displacement_FL, displacement_RL, displacement_FR, displacement_RR]) 78 | 79 | self.displacement.suspension_dot = (displacement_suspension - self.displacement.suspension) / static_parameters.time_step 80 | self.displacement.suspension = displacement_suspension 81 | 82 | sprung_mass = static_parameters.body.mass * static_parameters.wd - static_parameters.suspension.unsprung_mass 83 | 84 | damping_force = (static_parameters.suspension.damping_rate * self.displacement.suspension_dot) 85 | spring_force = (static_parameters.suspension.spring_rate * displacement_suspension) 86 | 87 | anti_roll_bar_force = static_parameters.suspension.roll_bar_stiffness * sin_Ф / (2 * np.array([lat_l, lat_l, lat_r, lat_r])) 88 | 89 | suspension_force = spring_force + damping_force - anti_roll_bar_force 90 | 91 | self.f_zr = WheelHubForce(f_zr_dot=np.array([0., 0., 0., 0.]), 92 | wheel_load_z = static_parameters.body.mass * static_parameters.gravity * static_parameters.wd) 93 | 94 | static_force = static_parameters.wd * static_parameters.body.mass 95 | 96 | lateral_force_transfer = np.array([-1, -1, 1, 1]) * (np.sum(sprung_mass) * self.x_a.acc_y * static_parameters.suspension.roll_centre_height / static_parameters.track_width) 97 | 98 | longitudinal_force_transfer = np.array([-1, 1, -1, 1]) * (np.sum(sprung_mass) * self.x_a.acc_x * static_parameters.suspension.pitch_centre_height / (2 * static_parameters.wheel_base)) 99 | 100 | self.f_zr.wheel_load_z = sprung_mass * static_parameters.gravity + suspension_force + longitudinal_force_transfer + lateral_force_transfer 101 | 102 | self.f_za = StrutForce(f_za = static_parameters.body.mass * static_parameters.gravity * static_parameters.wd, 103 | f_za_dot = np.zeros(4), 104 | spring_force = (static_parameters.body.mass * np.array([static_parameters.body.lr * static_parameters.body.wr, static_parameters.body.lf * static_parameters.body.wr, static_parameters.body.lr * static_parameters.body.wl, static_parameters.body.lf * static_parameters.body.wl]) / ((static_parameters.body.lr + static_parameters.body.lf) * (static_parameters.body.wl + static_parameters.body.wr)) * static_parameters.gravity).reshape(-1, 1), 105 | dumper_force = np.zeros((4, 1), dtype=float)) 106 | 107 | self.x_rf.wheel_forces_transformed_force2vehicle_sys[2, :] = self.f_zr.wheel_load_z 108 | 109 | self.compiled_wheel_forces = np.array([self.x_rf.fx, self.x_rf.fy, self.f_zr.wheel_load_z]) 110 | 111 | # Creating vector for chassis method 112 | self.sum_f_wheel = np.zeros(3, dtype=float) # Sum of wheel forces 113 | 114 | self.slip_x = np.zeros(4) 115 | self.slip_y = np.zeros(4) 116 | self.slip_y_rate = np.zeros(4) 117 | 118 | 119 | self.powertrain_net_torque = np.zeros(4) 120 | self.suspension_force = suspension_force 121 | 122 | 123 | @property 124 | def delta(self): 125 | return self._delta 126 | 127 | @delta.setter 128 | def delta(self, value): 129 | if np.isnan(value).any() or np.isinf(value).any(): 130 | raise ValueError(f"delta was set as {value}") 131 | self._delta = value 132 | 133 | @property 134 | def engine_w(self): 135 | return self._engine_w 136 | 137 | @engine_w.setter 138 | def engine_w(self, value): 139 | if np.isnan(value).any() or np.isinf(value).any(): 140 | raise ValueError(f"engine_w was set as {value}") 141 | self._engine_w = value 142 | 143 | @property 144 | def gear(self): 145 | return self._gear 146 | 147 | @gear.setter 148 | def gear(self, value): 149 | if np.isnan(value).any() or np.isinf(value).any(): 150 | raise ValueError(f"gear was set as {value}") 151 | self._gear = value 152 | 153 | @property 154 | def wheel_w_vel(self): 155 | return self._wheel_w_vel 156 | 157 | @wheel_w_vel.setter 158 | def wheel_w_vel(self, value): 159 | if np.isnan(value).any() or np.isinf(value).any(): 160 | raise ValueError(f"wheel_w_vel was set as {value}") 161 | self._wheel_w_vel = value 162 | 163 | @property 164 | def x_a(self): 165 | return self._x_a 166 | 167 | @x_a.setter 168 | def x_a(self, value): 169 | if np.isnan(value).any() or np.isinf(value).any(): 170 | raise ValueError(f"x_a was set as {value}") 171 | self._x_a = value 172 | 173 | @property 174 | def reference_zCG(self): 175 | return self._reference_zCG 176 | 177 | @reference_zCG.setter 178 | def reference_zCG(self, value): 179 | if np.isnan(value).any() or np.isinf(value).any(): 180 | raise ValueError(f"reference_zCG was set as {value}") 181 | self._reference_zCG = value 182 | 183 | @property 184 | def x_rr(self): 185 | return self._x_rr 186 | 187 | @x_rr.setter 188 | def x_rr(self, value): 189 | if np.isnan(value).any() or np.isinf(value).any(): 190 | raise ValueError(f"x_rr was set as {value}") 191 | self._x_rr = value 192 | 193 | @property 194 | def x_rf(self): 195 | return self._x_rf 196 | 197 | @x_rf.setter 198 | def x_rf(self, value): 199 | if np.isnan(value).any() or np.isinf(value).any(): 200 | raise ValueError(f"x_rf was set as {value}") 201 | self._x_rf = value 202 | 203 | @property 204 | def displacement(self): 205 | return self._displacement 206 | 207 | @displacement.setter 208 | def displacement(self, value): 209 | if np.isnan(value).any() or np.isinf(value).any(): 210 | raise ValueError(f"displacement was set as {value}") 211 | self._displacement = value 212 | 213 | @property 214 | def f_zr(self): 215 | return self._f_zr 216 | 217 | @f_zr.setter 218 | def f_zr(self, value): 219 | if np.isnan(value).any() or np.isinf(value).any(): 220 | raise ValueError(f"f_zr was set as {value}") 221 | self._f_zr = value 222 | 223 | @property 224 | def f_za(self): 225 | return self._f_za 226 | 227 | @f_za.setter 228 | def f_za(self, value): 229 | if np.isnan(value).any() or np.isinf(value).any(): 230 | raise ValueError(f"f_za was set as {value}") 231 | self._f_za = value 232 | 233 | @property 234 | def compiled_wheel_forces(self): 235 | return self._compiled_wheel_forces 236 | 237 | @compiled_wheel_forces.setter 238 | def compiled_wheel_forces(self, value): 239 | if np.isnan(value).any() or np.isinf(value).any(): 240 | raise ValueError(f"compiled_wheel_forces was set as {value}") 241 | self._compiled_wheel_forces = value 242 | 243 | @property 244 | def sum_f_wheel(self): 245 | return self._sum_f_wheel 246 | 247 | @sum_f_wheel.setter 248 | def sum_f_wheel(self, value): 249 | if np.isnan(value).any() or np.isinf(value).any(): 250 | raise ValueError(f"sum_f_wheel was set as {value}") 251 | self._sum_f_wheel = value 252 | 253 | @property 254 | def slip_x(self): 255 | return self._slip_x 256 | 257 | @slip_x.setter 258 | def slip_x(self, value): 259 | if np.isnan(value).any() or np.isinf(value).any(): 260 | raise ValueError(f"slip_x was set as {value}") 261 | self._slip_x = value 262 | 263 | @property 264 | def slip_y(self): 265 | return self._slip_y 266 | 267 | @slip_y.setter 268 | def slip_y(self, value): 269 | if np.isnan(value).any() or np.isinf(value).any(): 270 | raise ValueError(f"slip_y was set as {value}") 271 | self._slip_y = value 272 | 273 | @property 274 | def powertrain_net_torque(self): 275 | return self._powertrain_net_torque 276 | 277 | @powertrain_net_torque.setter 278 | def powertrain_net_torque(self, value): 279 | if np.isnan(value).any() or np.isinf(value).any(): 280 | raise ValueError(f"powertrain_net_torque was set as {value}") 281 | self._powertrain_net_torque = value 282 | 283 | @property 284 | def suspension_force(self): 285 | return self._suspension_force 286 | 287 | @suspension_force.setter 288 | def suspension_force(self, value): 289 | if np.isnan(value).any() or np.isinf(value).any(): 290 | raise ValueError(f"suspension_force was set as {value}") 291 | self._suspension_force = value 292 | 293 | 294 | def main(): 295 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 296 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 297 | test_function = CurrentStates 298 | function_name = test_function.__name__ 299 | logger = LocalLogger(function_name).logger 300 | logger.setLevel('INFO') 301 | static_parameters = StaticParameters("../../Audi_r8.yaml") 302 | parameters = CurrentStates(static_parameters, logger=logger) 303 | pass 304 | 305 | 306 | if __name__ == '__main__': 307 | main() 308 | -------------------------------------------------------------------------------- /vehicle_dynamics/modules/body.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model - Body Class 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 9 | from vehicle_dynamics.structures.StateVector import StateVector 10 | from vehicle_dynamics.structures.WheelHubForce import WheelHubForce 11 | from vehicle_dynamics.structures.Displacement import Displacement 12 | from vehicle_dynamics.utils.CurrentStates import CurrentStates 13 | from vehicle_dynamics.utils.import_data_CM import import_data_CM 14 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 15 | from vehicle_dynamics.structures.OutputStates import OutputStates 16 | from vehicle_dynamics.structures.Manoeuvre import Manoeuvre 17 | from vehicle_dynamics.utils.plot_function import plot_function 18 | 19 | import numpy as np 20 | import logging 21 | from copy import copy 22 | import yaml 23 | 24 | 25 | class Body(): 26 | def __init__(self, static_parameters, logger: logging.Logger): 27 | self.static_parameters = static_parameters 28 | self.logger = logger 29 | 30 | def access_z_road(self, x, y): 31 | # TODO implement topography 32 | z = 0. 33 | return z 34 | 35 | def road(self, current_state): 36 | for k in range(4): 37 | # displacement.zs[k][2] = self.access_z_road(self.displacement.zs[k][0],self.displacement.zs[k][1]) 38 | current_state.displacement.road[k] = 0. 39 | 40 | return current_state 41 | 42 | def body(self, current_state: CurrentStates): 43 | """ 44 | body is a function that calculates the current wheel loads (z) 45 | 46 | Required current_state from static_parameters: 47 | 1. suspension_spring_rate 48 | 2. suspension_damping_rate 49 | 50 | Required Arguments: 51 | 1. f_zr 52 | 1.01 wheel_load_z 53 | 2.displacement 54 | 2.01 za : position of the connection point of the suspension with the dampner spring system 55 | 2.02 zs : Z velocity of the wheel 56 | 2.03 l_stat: constant 57 | 2.04 za_dot: velocity of the chassis point 58 | 3.vehicle_fixed2inertial_system 59 | Returns: 60 | 1. f_zr.wheel_load_z 61 | 62 | """ 63 | vx = current_state.x_a.vx 64 | vy = current_state.x_a.vy 65 | Ф = current_state.x_a.roll 66 | θ = current_state.x_a.pitch 67 | Ψ = current_state.x_a.yaw 68 | sin_Ф = current_state.x_a.sin_roll 69 | cos_Ф = current_state.x_a.cos_roll 70 | sin_θ = current_state.x_a.sin_pitch 71 | cos_θ = current_state.x_a.cos_pitch 72 | sin_Ψ = current_state.x_a.sin_yaw 73 | cos_Ψ = current_state.x_a.cos_yaw 74 | θdot = current_state.x_a.wx 75 | Фdot = current_state.x_a.wy 76 | Ψdot = current_state.x_a.wz 77 | θdotdot = current_state.x_a.wx_dot 78 | Фdotdot = current_state.x_a.wy_dot 79 | Ψdotdot = current_state.x_a.wz_dot 80 | h = self.static_parameters.body.sz 81 | m = self.static_parameters.body.mass 82 | 83 | sum_f_wheel = np.sum(current_state.x_rf.wheel_forces_transformed_force2vehicle_sys, axis=1) 84 | 85 | current_state = self.road(current_state) # placeholder to consider road status 86 | 87 | # x-position from Vehicle CoG to the front axle [m] 88 | long_f = self.static_parameters.body.lf 89 | # x-position from Vehicle Cog to the rear axle [m] 90 | long_r = self.static_parameters.body.lr 91 | # y-position from Vehicle CoG to the left chassis point [m] 92 | lat_l = self.static_parameters.body.wl 93 | # y-position from Vehicle CoG to the right chassis point [m] 94 | lat_r = self.static_parameters.body.wr 95 | 96 | l = long_r + long_f 97 | w = lat_l + lat_r 98 | 99 | 100 | longitudinal_distance = np.array([long_f, -long_r, long_f, -long_r]) 101 | lateral_distance = np.array([lat_l, lat_l, - lat_r, - lat_r]) 102 | 103 | zCG = current_state.x_a.z 104 | static_cg_height = current_state.reference_zCG 105 | 106 | displacement_suspension = zCG - static_cg_height - (longitudinal_distance * sin_θ) + (lateral_distance * sin_Ф) - current_state.displacement.road 107 | 108 | current_state.displacement.suspension_dot = (displacement_suspension - current_state.displacement.suspension) / self.static_parameters.time_step 109 | current_state.displacement.suspension = displacement_suspension 110 | 111 | sprung_mass = self.static_parameters.sprung_mass 112 | 113 | damping_force = -(self.static_parameters.suspension.damping_rate * current_state.displacement.suspension_dot) 114 | spring_force = -(self.static_parameters.suspension.spring_rate * displacement_suspension) 115 | 116 | anti_roll_bar_force = self.static_parameters.suspension.roll_bar_stiffness * sin_Ф / (2 * lateral_distance) 117 | 118 | suspension_force = spring_force + damping_force + anti_roll_bar_force 119 | 120 | current_state.suspension_force = suspension_force 121 | 122 | front_wheels_sum = sprung_mass[0] + sprung_mass[2] 123 | rear_wheels_sum = sprung_mass[1] + sprung_mass[3] 124 | 125 | static_force = self.static_parameters.wd * self.static_parameters.body.mass 126 | 127 | 128 | ξ_lon = np.array([-1/2, 1/2, -1/2, 1/2]) 129 | ξ_lat = np.array([- long_r/w, - long_f/w, long_r/w, long_f/w]) 130 | 131 | lateral_force_transfer = ξ_lat * (m * current_state.x_a.acc_y * self.static_parameters.suspension.roll_centre_height / self.static_parameters.track_width) 132 | 133 | longitudinal_force_transfer = ξ_lon * (m* current_state.x_a.acc_x * self.static_parameters.suspension.pitch_centre_height / self.static_parameters.wheel_base) 134 | 135 | current_state.f_zr.wheel_load_z = (sprung_mass+self.static_parameters.suspension.unsprung_mass) * self.static_parameters.gravity + suspension_force + longitudinal_force_transfer + lateral_force_transfer 136 | 137 | current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[2, :] = current_state.f_zr.wheel_load_z 138 | 139 | 140 | # CHASSIS 141 | 142 | fy_fl = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, 0] 143 | fy_rl = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, 1] 144 | fy_fr = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, 2] 145 | fy_rr = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, 3] 146 | 147 | fx_fl = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, 0] 148 | fx_rl = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, 1] 149 | fx_fr = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, 2] 150 | fx_rr = current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, 3] 151 | 152 | #self.logger.info(sum_f_wheel[0]) 153 | 154 | current_state.x_a.acc_x = (sum_f_wheel[0] - 0.5 * self.static_parameters.aerodynamics_front * vx**2) / m 155 | current_state.x_a.vx += (current_state.x_a.acc_x + vy * Ψdot) * self.static_parameters.time_step 156 | 157 | if current_state.x_a.vx <= 0.0: 158 | current_state.x_a.acc_x = 0.0 159 | current_state.x_a.vx = 0.0 160 | 161 | 162 | current_state.x_a.acc_y = (sum_f_wheel[1]) / m 163 | current_state.x_a.acc_z = sum(suspension_force) / m 164 | 165 | current_state.x_a.vy += (current_state.x_a.acc_y - vx * Ψdot) * self.static_parameters.time_step 166 | current_state.x_a.vz += current_state.x_a.acc_z * self.static_parameters.time_step 167 | 168 | vxx, vyx = vx * cos_Ψ, vy * -sin_Ψ 169 | vxy, vyy = vx * sin_Ψ, vy * cos_Ψ 170 | 171 | current_state.x_a.x += (vxx + vyx) * self.static_parameters.time_step # (vxx + vyx) 172 | current_state.x_a.y += (vxy + vyy) * self.static_parameters.time_step # (vxy + vyy) 173 | current_state.x_a.z += current_state.x_a.vz * self.static_parameters.time_step 174 | 175 | # Chassis rotation 176 | 177 | Ix = self.static_parameters.body.i_x_s 178 | Iy = self.static_parameters.body.i_y_s 179 | Iz = self.static_parameters.body.i_z 180 | h = self.static_parameters.body.sz 181 | l = self.static_parameters.body.wr # halftrack 182 | 183 | Fx = sum_f_wheel[0] 184 | Fy = sum_f_wheel[1] 185 | 186 | #Mz = ((fy_fl + fy_fr) * self.static_parameters.body.lf - (fy_rl + fy_rr) * self.static_parameters.body.lr + (fx_rr + fx_fr - fx_rl - fx_fl) * l) 187 | Mz = ((fy_fl + fy_fr) * self.static_parameters.body.lf - (fy_rl + fy_rr) * self.static_parameters.body.lr + (fx_rr + fx_fr) * (self.static_parameters.body.wr) + (fx_rl + fx_fl) * (-self.static_parameters.body.wl)) 188 | 189 | hsr = self.static_parameters.suspension.roll_centre_height - h 190 | hsp = self.static_parameters.suspension.pitch_centre_height - h 191 | 192 | 193 | current_state.x_a.wx_dot = (np.sum(lateral_distance * suspension_force) + np.sum(sprung_mass) * hsr * (current_state.x_a.acc_y + self.static_parameters.gravity * sin_Ф)) / Ix 194 | current_state.x_a.wy_dot = ((-1) * ((np.sum(longitudinal_distance * suspension_force) + np.sum(sprung_mass) * hsp * (current_state.x_a.acc_x - self.static_parameters.gravity * sin_θ)) / Iy)) 195 | 196 | current_state.x_a.wz_dot = Mz / Iz #- h * (Fx * sin_Ф + Fy * sin_θ * cos_Ф)) / (Ix * (sin_θ**2) + (cos_θ**2) * (Iy * (sin_Ф**2) + Iz * (cos_Ф**2))) 197 | 198 | current_state.x_a.wx += current_state.x_a.wx_dot * self.static_parameters.time_step 199 | current_state.x_a.wy += current_state.x_a.wy_dot * self.static_parameters.time_step 200 | current_state.x_a.wz += (current_state.x_a.wz_dot * self.static_parameters.time_step) 201 | 202 | # Angular position 203 | current_state.x_a.roll += (current_state.x_a.wx * self.static_parameters.time_step) 204 | current_state.x_a.pitch += (current_state.x_a.wy * self.static_parameters.time_step) 205 | current_state.x_a.yaw += (current_state.x_a.wz * self.static_parameters.time_step) 206 | 207 | return current_state 208 | 209 | 210 | def main(static_parameters, current_state, data, logger, savefig=False): 211 | import yaml 212 | from tqdm import tqdm 213 | 214 | body = Body(static_parameters, logger) 215 | 216 | test_function = body.body 217 | function_name = test_function.__name__ 218 | 219 | steering = data["steering"] 220 | throttle = data["throttle"] 221 | brake = data["brake"] 222 | points = len(brake) 223 | 224 | Fx_fl = data["Fx_fl"] 225 | Fx_fr = data["Fx_fr"] 226 | Fx_rl = data["Fx_rl"] 227 | Fx_rr = data["Fx_rr"] 228 | 229 | Fy_fl = data["Fy_fl"] 230 | Fy_fr = data["Fy_fr"] 231 | Fy_rl = data["Fy_rl"] 232 | Fy_rr = data["Fy_rr"] 233 | 234 | time = data["time"] 235 | manoeuvre = Manoeuvre(steering, throttle, brake, time) 236 | logger.info("loaded SimulationData") 237 | simulation_range = range(0, len(time)) 238 | output_states = OutputStates() 239 | 240 | for i in tqdm(simulation_range): 241 | forces_in_x = [Fx_fl[i], Fx_rl[i], Fx_fr[i], Fx_rr[i]] 242 | forces_in_y = [Fy_fl[i], Fy_rl[i], Fy_fr[i], Fy_rr[i]] 243 | current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[0, :] = forces_in_x 244 | current_state.x_rf.wheel_forces_transformed_force2vehicle_sys[1, :] = forces_in_y 245 | 246 | output_states.set_states(test_function(current_state)) 247 | plot_function(output_states, manoeuvre, data, savefig, 248 | plot_type="body", 249 | xlim=(-.25, data["time"][-1] - data["time"][0]+0.25), 250 | static_parameters=static_parameters) 251 | 252 | 253 | if __name__ == '__main__': 254 | import pickle 255 | test_function = Body 256 | function_name = test_function.__name__ 257 | logger = LocalLogger(function_name).logger 258 | logger.setLevel('INFO') 259 | 260 | PATH_TO_DATA = "../../example_data/Braking.pickle" 261 | with open(PATH_TO_DATA,"rb")as handle: 262 | data=pickle.load(handle) 263 | 264 | from vehicle_dynamics.structures.StateVector import StateVector 265 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 266 | static_parameters = StaticParameters("../../bmw_m8.yaml", freq = 1000) 267 | 268 | initial_state = StateVector(x = np.array( data["Rel_pos_x"][0]), 269 | y = np.array(data["Rel_pos_y"][0]), 270 | vx = np.array( data["Velocity_X"][0]), 271 | yaw = np.array( data["Yaw"][0])) 272 | #yaw = np.array( (sync_adma[0].ins_yaw+125)* 3.14/180.0)) 273 | 274 | 275 | current_state = CurrentStates(static_parameters, logger=logger, initial_state=initial_state) 276 | logger.info("loaded current_state") 277 | 278 | logger.info("loaded SimulationData") 279 | main(static_parameters, current_state, data, logger, False) 280 | -------------------------------------------------------------------------------- /vehicle_dynamics/structures/CurrentStates.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model - CurrentStates Class 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | 9 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 10 | from vehicle_dynamics.structures.TireForces import TireForces 11 | from vehicle_dynamics.structures.StateVector import StateVector 12 | from vehicle_dynamics.structures.Displacement import Displacement 13 | from vehicle_dynamics.structures.StrutForce import StrutForce 14 | from vehicle_dynamics.structures.WheelHubForce import WheelHubForce 15 | from vehicle_dynamics.structures.AngularWheelPosition import AngularWheelPosition 16 | 17 | from vehicle_dynamics.utils.import_data_CM import import_data_CM 18 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 19 | 20 | from scipy.interpolate import interp1d 21 | from numpy.linalg import inv 22 | import numpy as np 23 | import yaml 24 | import math 25 | 26 | 27 | class CurrentStates(object): 28 | """This class initialize the values of a vehicular dynamic model. """ 29 | 30 | def __init__(self, static_parameters, freq = 1000, initial_state = np.zeros(15), initial_gear = 1, logger = 1): 31 | super(CurrentStates, self).__init__() 32 | self.delta = 0 33 | 34 | self.engine_w = static_parameters.powertrain.engine.idle_rpm 35 | self.gear = initial_gear # gear selector 36 | self.wheel_w_vel = np.zeros(4) 37 | 38 | # State initiate with the position, orientation and speed provided by the arguments, acc = 0; 39 | if isinstance(initial_state, np.ndarray): 40 | self.x_a = StateVector(*initial_state) # initial_state 41 | elif isinstance(initial_state, StateVector): 42 | self.x_a = initial_state 43 | 44 | self.reference_zCG = self.x_a.z 45 | 46 | Ф = self.x_a.roll 47 | θ = self.x_a.pitch 48 | Ψ = self.x_a.yaw 49 | cos = np.cos 50 | sin = np.sin 51 | sin_Ф = self.x_a.sin_roll 52 | cos_Ф = self.x_a.cos_roll 53 | sin_θ = self.x_a.sin_pitch 54 | cos_θ = self.x_a.cos_pitch 55 | sin_Ψ = self.x_a.sin_yaw 56 | cos_Ψ = self.x_a.cos_yaw 57 | 58 | # Wheel initiate stoped 59 | self.x_rr = AngularWheelPosition(pho_r=np.zeros(4), pho_r_dot = np.zeros(4), pho_r_2dot =np.zeros(4)) 60 | self.x_rf = TireForces(fx =np.zeros(4), fy = np.zeros(4), wheel_forces_transformed_force2vehicle_sys = np.ones((3, 4), dtype=float)) 61 | self.displacement = Displacement(static_suspension=(static_parameters.body.mass * static_parameters.wd * static_parameters.gravity) / static_parameters.suspension.spring_rate, 62 | suspension = np.zeros(4), 63 | suspension_dot=np.zeros(4), 64 | zr_dot=np.zeros(4), 65 | zr_2dot=np.zeros(4)) 66 | zCG = self.x_a.z 67 | static_cg_height = static_parameters.body.sz 68 | long_f = static_parameters.body.lf 69 | # x-position from Vehicle Cog to the rear axle [m] 70 | long_r = - static_parameters.body.lr 71 | # y-position from Vehicle CoG to the left chassis point [m] 72 | lat_l = static_parameters.body.wl 73 | # y-position from Vehicle CoG to the right chassis point [m] 74 | lat_r = - static_parameters.body.wr 75 | 76 | zCG = self.x_a.z 77 | static_cg_height = self.reference_zCG 78 | 79 | displacement_FL = zCG - static_cg_height - (sin_θ * long_f) + (sin_Ф * lat_l) - self.displacement.road[0] 80 | displacement_RL = zCG - static_cg_height - (sin_θ * long_r) + (sin_Ф * lat_l) - self.displacement.road[1] 81 | displacement_FR = zCG - static_cg_height - (sin_θ * long_f) + (sin_Ф * lat_r) - self.displacement.road[2] 82 | displacement_RR = zCG - static_cg_height - (sin_θ * long_r) + (sin_Ф * lat_r) - self.displacement.road[3] 83 | 84 | displacement_suspension = np.array([displacement_FL, displacement_RL, displacement_FR, displacement_RR]) 85 | 86 | self.displacement.suspension_dot = (displacement_suspension - self.displacement.suspension) / static_parameters.time_step 87 | self.displacement.suspension = displacement_suspension 88 | 89 | sprung_mass = static_parameters.body.mass * static_parameters.wd - static_parameters.suspension.unsprung_mass 90 | 91 | damping_force = (static_parameters.suspension.damping_rate * self.displacement.suspension_dot) 92 | spring_force = (static_parameters.suspension.spring_rate * displacement_suspension) 93 | 94 | anti_roll_bar_force = static_parameters.suspension.roll_bar_stiffness * sin_Ф / (2 * np.array([lat_l, lat_l, lat_r, lat_r])) 95 | 96 | suspension_force = spring_force + damping_force - anti_roll_bar_force 97 | 98 | self.f_zr = WheelHubForce(f_zr_dot=np.array([0., 0., 0., 0.]), 99 | wheel_load_z = static_parameters.body.mass * static_parameters.gravity * static_parameters.wd) 100 | 101 | static_force = static_parameters.wd * static_parameters.body.mass 102 | 103 | lateral_force_transfer = np.array([-1, -1, 1, 1]) * (np.sum(sprung_mass) * self.x_a.acc_y * static_parameters.suspension.roll_centre_height / static_parameters.track_width) 104 | 105 | longitudinal_force_transfer = np.array([-1, 1, -1, 1]) * (np.sum(sprung_mass) * self.x_a.acc_x * static_parameters.suspension.pitch_centre_height / (2 * static_parameters.wheel_base)) 106 | 107 | self.f_zr.wheel_load_z = sprung_mass * static_parameters.gravity + suspension_force + longitudinal_force_transfer + lateral_force_transfer 108 | 109 | self.f_za = StrutForce(f_za = static_parameters.body.mass * static_parameters.gravity * static_parameters.wd, 110 | f_za_dot = np.zeros(4), 111 | spring_force = (static_parameters.body.mass * np.array([static_parameters.body.lr * static_parameters.body.wr, static_parameters.body.lf * static_parameters.body.wr, static_parameters.body.lr * static_parameters.body.wl, static_parameters.body.lf * static_parameters.body.wl]) / ((static_parameters.body.lr + static_parameters.body.lf) * (static_parameters.body.wl + static_parameters.body.wr)) * static_parameters.gravity).reshape(-1, 1), 112 | dumper_force = np.zeros((4, 1), dtype=float)) 113 | 114 | self.x_rf.wheel_forces_transformed_force2vehicle_sys[2, :] = self.f_zr.wheel_load_z 115 | 116 | self.compiled_wheel_forces = np.array([self.x_rf.fx, self.x_rf.fy, self.f_zr.wheel_load_z]) 117 | 118 | # Creating vector for chassis method 119 | self.sum_f_wheel = np.zeros(3, dtype=float) # Sum of wheel forces 120 | 121 | self.slip_x = np.zeros(4) 122 | self.slip_y = np.zeros(4) 123 | self.slip_y_rate = np.zeros(4) 124 | 125 | 126 | self.powertrain_net_torque = np.zeros(4) 127 | self.suspension_force = suspension_force 128 | 129 | 130 | @property 131 | def delta(self): 132 | return self._delta 133 | 134 | @delta.setter 135 | def delta(self, value): 136 | if np.isnan(value).any() or np.isinf(value).any(): 137 | raise ValueError(f"delta was set as {value}") 138 | self._delta = value 139 | 140 | @property 141 | def engine_w(self): 142 | return self._engine_w 143 | 144 | @engine_w.setter 145 | def engine_w(self, value): 146 | if np.isnan(value).any() or np.isinf(value).any(): 147 | raise ValueError(f"engine_w was set as {value}") 148 | self._engine_w = value 149 | 150 | @property 151 | def gear(self): 152 | return self._gear 153 | 154 | @gear.setter 155 | def gear(self, value): 156 | if np.isnan(value).any() or np.isinf(value).any(): 157 | raise ValueError(f"gear was set as {value}") 158 | self._gear = value 159 | 160 | @property 161 | def wheel_w_vel(self): 162 | return self._wheel_w_vel 163 | 164 | @wheel_w_vel.setter 165 | def wheel_w_vel(self, value): 166 | if np.isnan(value).any() or np.isinf(value).any(): 167 | raise ValueError(f"wheel_w_vel was set as {value}") 168 | self._wheel_w_vel = value 169 | 170 | @property 171 | def x_a(self): 172 | return self._x_a 173 | 174 | @x_a.setter 175 | def x_a(self, value): 176 | if np.isnan(value).any() or np.isinf(value).any(): 177 | raise ValueError(f"x_a was set as {value}") 178 | self._x_a = value 179 | 180 | @property 181 | def reference_zCG(self): 182 | return self._reference_zCG 183 | 184 | @reference_zCG.setter 185 | def reference_zCG(self, value): 186 | if np.isnan(value).any() or np.isinf(value).any(): 187 | raise ValueError(f"reference_zCG was set as {value}") 188 | self._reference_zCG = value 189 | 190 | @property 191 | def x_rr(self): 192 | return self._x_rr 193 | 194 | @x_rr.setter 195 | def x_rr(self, value): 196 | if np.isnan(value).any() or np.isinf(value).any(): 197 | raise ValueError(f"x_rr was set as {value}") 198 | self._x_rr = value 199 | 200 | @property 201 | def x_rf(self): 202 | return self._x_rf 203 | 204 | @x_rf.setter 205 | def x_rf(self, value): 206 | if np.isnan(value).any() or np.isinf(value).any(): 207 | raise ValueError(f"x_rf was set as {value}") 208 | self._x_rf = value 209 | 210 | @property 211 | def displacement(self): 212 | return self._displacement 213 | 214 | @displacement.setter 215 | def displacement(self, value): 216 | if np.isnan(value).any() or np.isinf(value).any(): 217 | raise ValueError(f"displacement was set as {value}") 218 | self._displacement = value 219 | 220 | @property 221 | def f_zr(self): 222 | return self._f_zr 223 | 224 | @f_zr.setter 225 | def f_zr(self, value): 226 | if np.isnan(value).any() or np.isinf(value).any(): 227 | raise ValueError(f"f_zr was set as {value}") 228 | self._f_zr = value 229 | 230 | @property 231 | def f_za(self): 232 | return self._f_za 233 | 234 | @f_za.setter 235 | def f_za(self, value): 236 | if np.isnan(value).any() or np.isinf(value).any(): 237 | raise ValueError(f"f_za was set as {value}") 238 | self._f_za = value 239 | 240 | @property 241 | def compiled_wheel_forces(self): 242 | return self._compiled_wheel_forces 243 | 244 | @compiled_wheel_forces.setter 245 | def compiled_wheel_forces(self, value): 246 | if np.isnan(value).any() or np.isinf(value).any(): 247 | raise ValueError(f"compiled_wheel_forces was set as {value}") 248 | self._compiled_wheel_forces = value 249 | 250 | @property 251 | def sum_f_wheel(self): 252 | return self._sum_f_wheel 253 | 254 | @sum_f_wheel.setter 255 | def sum_f_wheel(self, value): 256 | if np.isnan(value).any() or np.isinf(value).any(): 257 | raise ValueError(f"sum_f_wheel was set as {value}") 258 | self._sum_f_wheel = value 259 | 260 | @property 261 | def slip_x(self): 262 | return self._slip_x 263 | 264 | @slip_x.setter 265 | def slip_x(self, value): 266 | if np.isnan(value).any() or np.isinf(value).any(): 267 | raise ValueError(f"slip_x was set as {value}") 268 | self._slip_x = value 269 | 270 | @property 271 | def slip_y(self): 272 | return self._slip_y 273 | 274 | @slip_y.setter 275 | def slip_y(self, value): 276 | if np.isnan(value).any() or np.isinf(value).any(): 277 | raise ValueError(f"slip_y was set as {value}") 278 | self._slip_y = value 279 | 280 | @property 281 | def powertrain_net_torque(self): 282 | return self._powertrain_net_torque 283 | 284 | @powertrain_net_torque.setter 285 | def powertrain_net_torque(self, value): 286 | if np.isnan(value).any() or np.isinf(value).any(): 287 | raise ValueError(f"powertrain_net_torque was set as {value}") 288 | self._powertrain_net_torque = value 289 | 290 | @property 291 | def suspension_force(self): 292 | return self._suspension_force 293 | 294 | @suspension_force.setter 295 | def suspension_force(self, value): 296 | if np.isnan(value).any() or np.isinf(value).any(): 297 | raise ValueError(f"suspension_force was set as {value}") 298 | self._suspension_force = value 299 | 300 | 301 | def main(): 302 | from vehicle_dynamics.utils.LocalLogger import LocalLogger 303 | from vehicle_dynamics.utils.StaticParameters import StaticParameters 304 | test_function = CurrentStates 305 | function_name = test_function.__name__ 306 | logger = LocalLogger(function_name).logger 307 | logger.setLevel('INFO') 308 | static_parameters = StaticParameters("../../bmw_m8.yaml") 309 | parameters = CurrentStates(static_parameters, logger=logger) 310 | pass 311 | 312 | 313 | if __name__ == '__main__': 314 | main() 315 | -------------------------------------------------------------------------------- /vehicle_dynamics/utils/plot_function.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vehicle Dynamic Model - Plot Function 3 | 4 | @author: Maikol Funk Drechsler, Yuri Poledna 5 | 6 | Funded by the European Union (grant no. 101069576). Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Climate, Infrastructure and Environment Executive Agency (CINEA). Neither the European Union nor the granting authority can be held responsible for them. 7 | """ 8 | def define_colors(): 9 | import matplotlib.pyplot as plt 10 | import matplotlib.colors as mcolors 11 | ROADVIEW_colors = {"ROADVIEW1": "#64798C", 12 | "ROADVIEW2": "#B0C6D9", 13 | "ROADVIEW3": "#F2C879", 14 | "ROADVIEW4": "#F28066"} 15 | 16 | mcolors.get_named_colors_mapping().update(ROADVIEW_colors) 17 | return mcolors.LinearSegmentedColormap.from_list("", ["ROADVIEW1", "ROADVIEW2", "ROADVIEW3"]) 18 | 19 | 20 | def plot_function(output_states=False, manoeuvre=True, data=True, savefig=False, plot_type = "all", xlim=(), fig_save_dir="../../figures",static_parameters=0, initial_iteraction=0): 21 | import matplotlib.pyplot as plt 22 | import numpy as np 23 | size = 17 24 | figsize = (size, size*.588) 25 | simulation_range = range(0, len(manoeuvre)) 26 | throttle = manoeuvre[:].throttle 27 | plt.rcParams.update({'font.size': 16}) 28 | 29 | define_colors() 30 | 31 | vehicle_time = data["car_time"] 32 | if output_states: 33 | states = output_states[:].x_a 34 | if "body" in plot_type or plot_type == "all": 35 | Fx_fl = data["Fx_fl"] 36 | Fx_fr = data["Fx_fr"] 37 | Fx_rl = data["Fx_rl"] 38 | Fx_rr = data["Fx_rr"] 39 | 40 | Fy_fl = data["Fy_fl"] 41 | Fy_fr = data["Fy_fr"] 42 | Fy_rl = data["Fy_rl"] 43 | Fy_rr = data["Fy_rr"] 44 | plt.figure(figsize=figsize) 45 | name = " Chassis Angles" 46 | plt.title(name) 47 | var_name = "Vhcl_Roll" 48 | yaw_vehicle=np.array(data["Yaw"]) 49 | plt.plot(vehicle_time,data["Roll"], "ROADVIEW1", label="M8 Roll") 50 | plt.plot(vehicle_time,data["Pitch"], "ROADVIEW2", label="M8 Pitch") 51 | plt.plot(vehicle_time, yaw_vehicle - yaw_vehicle[0], "ROADVIEW3", label="M8 Yaw") 52 | plt.plot(manoeuvre.time, [i.roll for i in output_states[:].x_a], "ROADVIEW1", linestyle="--", label="Calculated Roll") 53 | plt.plot(manoeuvre.time, [i.pitch for i in output_states[:].x_a], "ROADVIEW2", linestyle="--", label="Calculated Pitch") 54 | yaw = np.array([i.yaw for i in output_states[:].x_a]) 55 | plt.plot(manoeuvre.time, yaw - yaw[0], "ROADVIEW3", linestyle="--", label="Calculated Yaw") 56 | plt.grid() 57 | plt.legend() 58 | plt.xlim(xlim) 59 | plt.xlabel("Time (s)") 60 | plt.ylabel("Angle (rad)") 61 | plt.tight_layout() 62 | if savefig: 63 | plt.savefig(fig_save_dir + "/" + name + ".png") 64 | plt.figure(figsize=figsize) 65 | name = " Chassis Angles Velocity" 66 | plt.title(name) 67 | plt.plot(manoeuvre.time, [i.wx for i in output_states[:].x_a], "ROADVIEW1", linestyle="--", label="Calculated Roll Velocity") 68 | plt.plot(manoeuvre.time, [i.wy for i in output_states[:].x_a], "ROADVIEW2", linestyle="--", label="Calculated Pitch Velocity") 69 | plt.plot(manoeuvre.time, [i.wz for i in output_states[:].x_a], "ROADVIEW3", linestyle="--", label="Calculated Yaw Velocity") 70 | 71 | plt.plot(vehicle_time,data["Roll_Velocity"], "ROADVIEW1", label="M8 Roll Velocity") 72 | plt.plot(vehicle_time,data["Pitch_Velocity"], "ROADVIEW2", label="M8 Pitch Velocity") 73 | plt.plot(vehicle_time,data["Yaw_Velocity"], "ROADVIEW3", label="M8 Yaw Velocity") 74 | plt.xlabel("Time (s)") 75 | plt.ylabel("Angle Velocity (rad/s)") 76 | plt.legend() 77 | plt.xlim(xlim) 78 | plt.grid() 79 | plt.tight_layout() 80 | if savefig: 81 | plt.savefig(fig_save_dir + "/" + name + ".png") 82 | 83 | plt.figure(figsize=figsize) 84 | name = " Chassis Acceleration" 85 | plt.title(name) 86 | plt.plot(vehicle_time, data["Acceleration_X"], "ROADVIEW1", label="M8 Acceleration X") 87 | plt.plot(vehicle_time, data["Acceleration_Y"], "ROADVIEW2", label="M8 Acceleration Y") 88 | plt.plot(vehicle_time, data["Acceleration_Z"], "ROADVIEW3", label="M8 Acceleration Z") 89 | plt.plot(manoeuvre.time, [i.acc_x for i in output_states[:].x_a], "ROADVIEW1", linestyle="--", label="Calculated Acceleration X") 90 | plt.plot(manoeuvre.time, [i.acc_y for i in output_states[:].x_a], "ROADVIEW2", linestyle="--", label="Calculated Acceleration Y") 91 | plt.plot(manoeuvre.time, [i.acc_z for i in output_states[:].x_a], "ROADVIEW3", linestyle="--", label="Calculated Acceleration Z") 92 | plt.grid() 93 | plt.xlabel("Time (s)") 94 | plt.ylabel("Acceleration (m/s²)") 95 | plt.legend() 96 | plt.xlim(xlim) 97 | plt.tight_layout() 98 | if savefig: 99 | plt.savefig(fig_save_dir + "/" + name + ".png") 100 | plt.figure(figsize=figsize) 101 | name = " Chassis Velocity" 102 | plt.title(name) 103 | plt.plot(manoeuvre.time, data["Velocity_X"], "ROADVIEW1", label="M8 Velocity X") 104 | plt.plot(manoeuvre.time, data["Velocity_Y"], "ROADVIEW2", label="M8 Velocity Y") 105 | plt.plot(manoeuvre.time, data["Velocity_Z"], "ROADVIEW3", label="M8 Velocity Z") 106 | 107 | plt.plot(manoeuvre.time, [i.vx for i in output_states[:].x_a], "ROADVIEW1", linestyle="--", label="Calculated Velocity X") 108 | plt.plot(manoeuvre.time, [i.vy for i in output_states[:].x_a], "ROADVIEW2", linestyle="--", label="Calculated Velocity Y") 109 | plt.plot(manoeuvre.time, [i.vz for i in output_states[:].x_a], "ROADVIEW3", linestyle="--", label="Calculated Velocity Z") 110 | 111 | plt.grid() 112 | plt.xlabel("Time (s)") 113 | plt.ylabel("Velocity (m/s)") 114 | plt.legend() 115 | plt.xlim(xlim) 116 | plt.tight_layout() 117 | if savefig: 118 | plt.savefig(fig_save_dir + "/" + name + ".png") 119 | 120 | plt.figure(figsize=figsize) 121 | name = " Chassis Position" 122 | plt.title(name) 123 | plt.plot(data["Rel_pos_x"],data["Rel_pos_y"], "ROADVIEW1", label="M8 Position") 124 | 125 | plt.plot([i.x for i in output_states[:].x_a], [i.y for i in output_states[:].x_a], "ROADVIEW4", linestyle="--", label="Calculated Position") 126 | 127 | plt.grid() 128 | plt.xlabel("Position X (m)") 129 | plt.ylabel("Position Y (m)") 130 | plt.legend() 131 | plt.tight_layout() 132 | if savefig: 133 | plt.savefig(fig_save_dir + "/" + name + ".png") 134 | 135 | if "powertrain" in plot_type or plot_type == "all": 136 | gear_changes_idx =np.where(np.roll(output_states[:].gear, 1) != output_states[:].gear)[0] 137 | gear_changes = manoeuvre.time[gear_changes_idx] 138 | 139 | f, (a0, a1) = plt.subplots(2, 1, gridspec_kw={'height_ratios': [2, 2]}, figsize=figsize, sharex=True) 140 | powertrain_net_torque = [np.sum(i) for i in output_states[:].powertrain_net_torque] 141 | a0.plot(manoeuvre.time, powertrain_net_torque, "ROADVIEW1", linestyle="-", label="Calculated") 142 | 143 | powertrain_net_torque_M8 = data["powertrain_net_torque"] 144 | 145 | a0.plot(vehicle_time, powertrain_net_torque_M8, "ROADVIEW2", linestyle="--", label = "BMW M8") 146 | if len(gear_changes)>0: 147 | a0.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 148 | a1.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 149 | for j,i in enumerate(gear_changes[1:]): 150 | a0.axvline(i, color="ROADVIEW4", linestyle=":") 151 | a1.axvline(i, color="ROADVIEW4", linestyle=":") 152 | a0.legend() 153 | a0.grid() 154 | a0.set_ylabel("Powertrain Net Torque (Nm)") 155 | a0.set_xlim(xlim) 156 | a1.plot(manoeuvre.time, manoeuvre.throttle, "ROADVIEW3", linestyle = "-", label = "Gas pedal(BMW M8)") 157 | a1.plot(manoeuvre.time, manoeuvre.brake, "ROADVIEW4", linestyle = "-", label = "Brake pedal(BMW M8)") 158 | a1.step(manoeuvre.time, output_states[:].gear, "ROADVIEW1", linestyle = "-", label = "Calculated Gear Number") 159 | a1.legend(loc=1) 160 | a1.set_xlabel("Time (s)") 161 | a1.grid() 162 | a1.set_xlim(xlim) 163 | a1.set_ylim([-0.1, 2.3]) 164 | plt.subplots_adjust(wspace = 0, hspace = 0) 165 | if savefig: 166 | plt.savefig(fig_save_dir + "/Powertrain Nettorque.png") 167 | 168 | f, (a0, a1) = plt.subplots(2, 1, gridspec_kw = {'height_ratios': [2, 2]}, figsize = figsize, sharex=True) 169 | 170 | a0.plot(manoeuvre.time, (np.array(output_states[:].engine_w) * 30 / np.pi), "ROADVIEW1", linestyle = "--", label = "Calculated") 171 | a0.set_ylabel("Engine Rotation (RPM)") 172 | 173 | a0.plot(vehicle_time, data["rpm"], "ROADVIEW2", label = "BMW M8") 174 | a0.legend() 175 | a0.grid() 176 | a0.set_xlim(xlim) 177 | 178 | a1.plot(vehicle_time, data["gear"], "ROADVIEW2", linestyle = "-", label = "Gear Number (BMW M8)") 179 | a1.step(manoeuvre.time, output_states[:].gear, "ROADVIEW1", linestyle = "-", label = "Gear Number (Calculated)") 180 | a1.plot(manoeuvre.time, manoeuvre.throttle, "ROADVIEW3", linestyle = "-", label = "Gas pedal (BMW M8)") 181 | a1.plot(manoeuvre.time, manoeuvre.brake, "ROADVIEW4", linestyle = "-", label = "Brake pedal (BMW M8)") 182 | a1.set_xlim(xlim) 183 | a1.set_ylim([-0.1, 2.3]) 184 | a1.grid() 185 | if len(gear_changes)>0: 186 | a0.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 187 | a1.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 188 | for j,i in enumerate(gear_changes[1:]): 189 | a0.axvline(i, color="ROADVIEW4", linestyle=":") 190 | a1.axvline(i, color="ROADVIEW4", linestyle=":") 191 | a1.legend(loc =1) 192 | a1.set_xlabel("Time (s)") 193 | plt.subplots_adjust(wspace = 0, hspace = 0) 194 | if savefig: 195 | plt.savefig(fig_save_dir + "/Powertrain RPM.png") 196 | 197 | if "wheels" in plot_type or plot_type == "all": 198 | 199 | 200 | gear_changes_idx =np.where(np.roll(output_states[:].gear, 1) != output_states[:].gear)[0] 201 | gear_changes = manoeuvre.time[gear_changes_idx] 202 | 203 | f, (a0, a1) = plt.subplots(2, 1, gridspec_kw={'height_ratios': [3, 1]}, figsize=figsize, sharex=True) 204 | powertrain_net_torque = [np.sum(i) for i in output_states[:].powertrain_net_torque] 205 | a0.plot(manoeuvre.time, powertrain_net_torque, "ROADVIEW3", linestyle="--", label="Calculated") 206 | 207 | 208 | powertrain_net_torque_M8 = np.array(data["powertrain_net_torque"]) 209 | a0.plot(vehicle_time, powertrain_net_torque_M8, "ROADVIEW1", label = "BMW M8") 210 | if len(gear_changes)>0: 211 | a0.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 212 | a1.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 213 | for j,i in enumerate(gear_changes[1:]): 214 | a0.axvline(i, color="ROADVIEW4", linestyle=":") 215 | a1.axvline(i, color="ROADVIEW4", linestyle=":") 216 | a0.legend() 217 | a0.grid() 218 | a0.set_ylabel("Powertrain Net Torque (Nm)") 219 | a0.set_xlim(xlim) 220 | a1.plot(manoeuvre.time, manoeuvre.throttle, "ROADVIEW3", linestyle = "--", label = "Gas pedal(BMW M8)") 221 | a1.plot(manoeuvre.time, manoeuvre.brake, "ROADVIEW4", linestyle = "--", label = "Brake pedal(BMW M8)") 222 | a1.step(manoeuvre.time, output_states[:].gear, "ROADVIEW1", linestyle = "-", label = "Calculated Gear Number") 223 | a1.legend(loc=1) 224 | a1.set_xlabel("Time (s)") 225 | a1.grid() 226 | a1.set_xlim(xlim) 227 | plt.subplots_adjust(wspace = 0, hspace = 0) 228 | if savefig: 229 | plt.savefig(fig_save_dir + "/Powertrain Nettorque.png") 230 | 231 | f, (a0, a1) = plt.subplots(2, 1, gridspec_kw = {'height_ratios': [3, 1]}, figsize = figsize, sharex=True) 232 | 233 | a0.plot(manoeuvre.time, (np.array(output_states[:].engine_w) * 30 / np.pi), "ROADVIEW1", linestyle = "--", label = "Calculated") 234 | a0.set_ylabel("Engine Rotation (RPM)") 235 | 236 | a0.plot(vehicle_time, data["rpm"], "ROADVIEW2", label = "BMW M8") 237 | a0.legend() 238 | a0.grid() 239 | a0.set_xlim(xlim) 240 | 241 | a1.plot(vehicle_time, data["gear"], "ROADVIEW2", linestyle = "-", label = "Gear Number (BMW M8)") 242 | a1.step(manoeuvre.time, output_states[:].gear, "ROADVIEW1", linestyle = "-", label = "Gear Number (Calculated)") 243 | a1.plot(manoeuvre.time, manoeuvre.throttle, "ROADVIEW3", linestyle = "-", label = "Gas pedal (BMW M8)") 244 | a1.plot(manoeuvre.time, manoeuvre.brake, "ROADVIEW4", linestyle = "-", label = "Brake pedal (BMW M8)") 245 | a1.set_xlim(xlim) 246 | a1.grid() 247 | if len(gear_changes)>0: 248 | a0.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 249 | a1.axvline(gear_changes[0], color="ROADVIEW4", linestyle=":", label="Gear Change Moment") 250 | for j,i in enumerate(gear_changes[1:]): 251 | a0.axvline(i, color="ROADVIEW4", linestyle=":") 252 | a1.axvline(i, color="ROADVIEW4", linestyle=":") 253 | a1.legend(loc =1) 254 | a1.set_xlabel("Time (s)") 255 | plt.subplots_adjust(wspace = 0, hspace = 0) 256 | if savefig: 257 | plt.savefig(fig_save_dir + "/Powertrain RPM.png") 258 | 259 | 260 | vx = data["Velocity_X"] 261 | MINIMUM_SPEED_VALUE = 10 262 | 263 | 264 | slip_fr = np.zeros(len(vx)) 265 | slip_rr = np.zeros(len(vx)) 266 | slip_fl = np.zeros(len(vx)) 267 | slip_rl = np.zeros(len(vx)) 268 | 269 | v_wheel_fl = data["v_wheel_fl"] 270 | v_wheel_fr = data["v_wheel_fr"] 271 | v_wheel_rl = data["v_wheel_rl"] 272 | v_wheel_rr = data["v_wheel_rr"] 273 | 274 | 275 | for i in range(len(vx)): 276 | 277 | if (abs(static_parameters.tire.dynamic_radius[0] * v_wheel_fl[i]) <= MINIMUM_SPEED_VALUE) and (abs(vx[i]) <= MINIMUM_SPEED_VALUE): 278 | slip_fl[i] = ((((static_parameters.tire.dynamic_radius[0] * v_wheel_fl[i]) - vx[i]) / (MINIMUM_SPEED_VALUE))) 279 | slip_fl[i] = ((((static_parameters.tire.dynamic_radius[0] * v_wheel_fl[i]) - vx[i]) / np.maximum(np.absolute(static_parameters.tire.dynamic_radius[0] * v_wheel_fl[i]), np.absolute(vx[i])))) 280 | 281 | if (abs(static_parameters.tire.dynamic_radius[0] * v_wheel_fr[i]) <= MINIMUM_SPEED_VALUE) and (abs(vx[i]) <= MINIMUM_SPEED_VALUE): 282 | slip_fr[i] = ((((static_parameters.tire.dynamic_radius[0] * v_wheel_fr[i]) - vx[i]) / (MINIMUM_SPEED_VALUE))) 283 | slip_fr[i] = ((((static_parameters.tire.dynamic_radius[0] * v_wheel_fr[i]) - vx[i]) / np.maximum(np.absolute(static_parameters.tire.dynamic_radius[0] * v_wheel_fr[i]), np.absolute(vx[i])))) 284 | 285 | if (abs(static_parameters.tire.dynamic_radius[1] * v_wheel_rl[i]) <= MINIMUM_SPEED_VALUE) and (abs(vx[i]) <= MINIMUM_SPEED_VALUE): 286 | slip_rl[i] = ((((static_parameters.tire.dynamic_radius[1] * v_wheel_rl[i]) - vx[i]) / (MINIMUM_SPEED_VALUE))) 287 | slip_rl[i] = ((((static_parameters.tire.dynamic_radius[1] * v_wheel_rl[i]) - vx[i]) / np.maximum(np.absolute(static_parameters.tire.dynamic_radius[1] * v_wheel_rl[i]), np.absolute(vx[i])))) 288 | 289 | if (abs(static_parameters.tire.dynamic_radius[1] * v_wheel_rr[i]) <= MINIMUM_SPEED_VALUE) and (abs(vx[i]) <= MINIMUM_SPEED_VALUE): 290 | slip_rr[i] = ((((static_parameters.tire.dynamic_radius[1] * v_wheel_rr[i]) - vx[i]) / (MINIMUM_SPEED_VALUE))) 291 | slip_rr[i] = ((((static_parameters.tire.dynamic_radius[1] * v_wheel_rr[i]) - vx[i]) / np.maximum(np.absolute(static_parameters.tire.dynamic_radius[1] * v_wheel_rr[i]), np.absolute(vx[i])))) 292 | 293 | f, ((a0, a1),(a2,a3)) = plt.subplots(2, 2, figsize=figsize, sharex=True,sharey=True) 294 | a0.plot(manoeuvre.time, slip_fl, "ROADVIEW1", label="Longitudinal Slip FL") 295 | a0.plot(manoeuvre.time, [i[0] for i in output_states[:].slip_x], "ROADVIEW1", linestyle ="--", label ="Calculated Longitudinal Slip FL") 296 | a0.set_xlim(xlim) 297 | a0.legend() 298 | a0.set_ylim(-0.2, 0.2) 299 | a0.set_xlabel("Time (s)") 300 | a0.set_ylabel("Wheel Slip") 301 | a2.set_xlabel("Time (s)") 302 | a2.set_ylabel("Wheel Slip") 303 | a2.plot(manoeuvre.time, slip_rl, "ROADVIEW2", label="Longitudinal Slip RL") 304 | a2.plot(manoeuvre.time, [i[1] for i in output_states[:].slip_x], "ROADVIEW2", linestyle ="--", label ="Calculated Longitudinal Slip RL") 305 | a2.set_xlim(xlim) 306 | a2.legend() 307 | a1.set_ylim(-0.2, 0.2) 308 | a1.set_xlabel("Time (s)") 309 | a1.set_ylabel("Wheel Slip") 310 | a1.plot(manoeuvre.time, slip_fr, "ROADVIEW3", label="Longitudinal Slip FR") 311 | a1.plot(manoeuvre.time, [i[2] for i in output_states[:].slip_x], "ROADVIEW3", linestyle ="--", label ="Calculated Longitudinal Slip FR") 312 | a1.set_xlim(xlim) 313 | a1.legend() 314 | a1.set_ylim(-0.2, 0.2) 315 | a3.set_ylim(-0.2, 0.2) 316 | a3.set_xlabel("Time (s)") 317 | a3.set_ylabel("Wheel Slip") 318 | a3.plot(manoeuvre.time, slip_rr, "ROADVIEW4", label="Longitudinal Slip RR") 319 | a3.plot(manoeuvre.time, [i[3] for i in output_states[:].slip_x], "ROADVIEW4", linestyle ="--", label ="Calculated Longitudinal Slip RR") 320 | a3.set_xlim(xlim) 321 | a3.legend() 322 | plt.tight_layout() 323 | if savefig: 324 | plt.savefig(fig_save_dir + "/Unoptimized wheel_slip_long_subplot-wheelLoad.png") 325 | 326 | f, ((a0, a1),(a2,a3)) = plt.subplots(2, 2, figsize=figsize, sharex=True,sharey=True) 327 | a0.plot(vehicle_time, data["slip_FL"], "ROADVIEW1", label="Lateral Slip FL") 328 | a0.plot(manoeuvre.time, [i[0] for i in output_states[:].slip_y], "ROADVIEW1", linestyle ="--", label ="Calculated Lateral Slip FL") 329 | a0.set_xlim(xlim) 330 | a0.legend() 331 | a0.set_ylim(-0.2, 0.2) 332 | a0.set_xlabel("Time (s)") 333 | a0.set_ylabel("Wheel Slip") 334 | a2.set_xlabel("Time (s)") 335 | a2.set_ylabel("Wheel Slip") 336 | a2.plot(vehicle_time, data["slip_RL"], "ROADVIEW2", label="Lateral Slip RL") 337 | a2.plot(manoeuvre.time, [i[1] for i in output_states[:].slip_y], "ROADVIEW2", linestyle ="--", label ="Calculated Lateral Slip RL") 338 | a2.set_xlim(xlim) 339 | a2.legend() 340 | a1.set_ylim(-0.2, 0.2) 341 | a1.set_xlabel("Time (s)") 342 | a1.set_ylabel("Wheel Slip") 343 | a1.plot(vehicle_time, data["slip_FR"], "ROADVIEW3", label="Lateral Slip FR") 344 | a1.plot(manoeuvre.time, [i[2] for i in output_states[:].slip_y], "ROADVIEW3", linestyle ="--", label ="Calculated Lateral Slip FR") 345 | a1.set_xlim(xlim) 346 | a1.legend() 347 | a1.set_ylim(-0.2, 0.2) 348 | a3.set_ylim(-0.2, 0.2) 349 | a3.set_xlabel("Time (s)") 350 | a3.set_ylabel("Wheel Slip") 351 | a3.plot(vehicle_time, data["slip_RR"], "ROADVIEW4", label="Lateral Slip RR") 352 | a3.plot(manoeuvre.time, [i[3] for i in output_states[:].slip_y], "ROADVIEW4", linestyle ="--", label ="Calculated Lateral Slip RR") 353 | a3.set_xlim(xlim) 354 | a3.legend() 355 | plt.tight_layout() 356 | if savefig: 357 | plt.savefig(fig_save_dir + "/Unoptimized wheel_slip_side_subplot-wheelLoad.png") 358 | 359 | plt.figure(figsize=figsize) 360 | plt.plot(vehicle_time, data["slip_FL"], "ROADVIEW1", label="Lateral Slip FL") 361 | plt.plot(vehicle_time, data["slip_RL"], "ROADVIEW2", label="Lateral Slip RL") 362 | plt.plot(vehicle_time, data["slip_FR"], "ROADVIEW3", label="Lateral Slip FR") 363 | plt.plot(vehicle_time, data["slip_RR"], "ROADVIEW4", label="Lateral Slip RR") 364 | plt.legend() 365 | plt.tight_layout() 366 | if savefig: 367 | plt.savefig(fig_save_dir + "/Unoptimized wheel_slip_side_subplot-wheelLoad.png") 368 | 369 | f, ((a0, a1),(a2,a3)) = plt.subplots(2, 2, figsize=figsize, sharex=True,sharey=True) 370 | a0.plot(manoeuvre.time, data["v_wheel_fl"], "ROADVIEW1", label="Wheel Velocity FL") 371 | a0.plot(manoeuvre.time, [i[0] for i in output_states[:].wheel_w_vel], "ROADVIEW1", linestyle ="--", label ="Calculated Wheel Velocity FL") 372 | a0.set_xlim(xlim) 373 | a0.legend() 374 | a0.set_xlabel("Time (s)") 375 | a0.set_ylabel("Wheel Velocity") 376 | a2.set_xlabel("Time (s)") 377 | a2.set_ylabel("Wheel Velocity") 378 | a2.plot(manoeuvre.time, data["v_wheel_rl"], "ROADVIEW2", label="Wheel Velocity RL") 379 | a2.plot(manoeuvre.time, [i[1] for i in output_states[:].wheel_w_vel], "ROADVIEW2", linestyle ="--", label ="Calculated Wheel Velocity RL") 380 | a2.set_xlim(xlim) 381 | a2.legend() 382 | a1.set_xlabel("Time (s)") 383 | a1.set_ylabel("Wheel Velocity") 384 | a1.plot(manoeuvre.time, data["v_wheel_fr"], "ROADVIEW3", label="Wheel Velocity FR") 385 | a1.plot(manoeuvre.time, [i[2] for i in output_states[:].wheel_w_vel], "ROADVIEW3", linestyle ="--", label ="Calculated Wheel Velocity FR") 386 | a1.set_xlim(xlim) 387 | a1.legend() 388 | a3.set_xlabel("Time (s)") 389 | a3.set_ylabel("Wheel Velocity") 390 | a3.plot(manoeuvre.time, data["v_wheel_rr"], "ROADVIEW4", label="Wheel Velocity RR") 391 | a3.plot(manoeuvre.time, [i[3] for i in output_states[:].wheel_w_vel], "ROADVIEW4", linestyle ="--", label ="Calculated Wheel Velocity RR") 392 | a3.set_xlim(xlim) 393 | a3.legend() 394 | plt.tight_layout() 395 | if savefig: 396 | plt.savefig(fig_save_dir + "/Unoptimized wheel_velocity_subplot-wheelLoad.png") 397 | # x_rf.fx 398 | f, ((a0, a1),(a2,a3)) = plt.subplots(2, 2, figsize=figsize, sharex=True,sharey=True) 399 | 400 | a0.plot(manoeuvre.time, [i.fy[0] for i in output_states[:].x_rf], "ROADVIEW1", linestyle ="--", label ="Fy FL") 401 | a0.set_xlim(xlim) 402 | a0.legend() 403 | a0.set_xlabel("Time (s)") 404 | a0.set_ylabel("Wheel Forces in y") 405 | a2.set_xlabel("Time (s)") 406 | a2.set_ylabel("Wheel Forces in y") 407 | a2.plot(manoeuvre.time, [i.fy[1] for i in output_states[:].x_rf], "ROADVIEW2", linestyle ="--", label ="Fy RL") 408 | a2.set_xlim(xlim) 409 | a2.legend() 410 | a1.set_xlabel("Time (s)") 411 | a1.set_ylabel("Wheel Forces in y") 412 | a1.plot(manoeuvre.time, [i.fy[2] for i in output_states[:].x_rf], "ROADVIEW3", linestyle ="--", label ="Fy FR") 413 | a1.set_xlim(xlim) 414 | a1.legend() 415 | a3.set_xlabel("Time (s)") 416 | a3.set_ylabel("Wheel Forces in y") 417 | a3.plot(manoeuvre.time, [i.fy[3] for i in output_states[:].x_rf], "ROADVIEW4", linestyle ="--", label ="Fy RR") 418 | a3.set_xlim(xlim) 419 | a3.legend() 420 | plt.tight_layout() 421 | if savefig: 422 | plt.savefig(fig_save_dir + "/Unoptimized wheel_velocity_subplot-forcesY.png") 423 | 424 | plt.show() 425 | --------------------------------------------------------------------------------