├── .gitignore ├── LICENSE ├── README.md ├── algorithm_expand_convex_polygon.pdf ├── animation_SCR_vs_SL.gif ├── code ├── +config │ ├── #README.m │ ├── base_scenario.m │ ├── base_vehicle.m │ ├── compute_ST_flying_initialization.m │ ├── config.m │ ├── init_config.m │ ├── scenario_1_vehicle.m │ ├── scenario_SCR_vs_SL.m │ ├── scenario_SL_reduce_checkpoints.m │ ├── scenario_SL_vs_SCR.m │ ├── scenario_endless_race.m │ ├── scenario_flying_start.m │ ├── scenario_lin_vs_ST.m │ ├── scenario_main_vehicle_SL_vs_SCR.m │ ├── scenario_main_vehicle_lin_vs_ST.m │ ├── scenario_models_comparison_linear.m │ ├── scenario_n_laps_race.m │ ├── scenario_paper_SL.m │ ├── scenario_race_various_vehicles.m │ ├── vehicle_SCR.m │ ├── vehicle_ST_Kloock.m │ ├── vehicle_ST_Liniger.m │ ├── vehicle_avoidance.m │ ├── vehicle_blocking.m │ ├── vehicle_lin_Liniger.m │ ├── vehicle_lin_Liniger_ST_Liniger.m │ └── vehicle_paper_SL.m ├── +controller │ ├── +track_SCR │ │ ├── find_closest_most_forward_polygon_index.m │ │ ├── find_closest_most_forward_polygon_indices.m │ │ ├── generate.m │ │ ├── t1_tesselate.m │ │ ├── t2_merge.m │ │ ├── t3_overlap.m │ │ └── t9_add_forward_direction.m │ ├── +track_SL │ │ ├── README.m │ │ ├── find_closest_checkpoint_index.m │ │ ├── find_closest_checkpoint_indices.m │ │ └── reduce_checkpoints.m │ ├── create_QP.m │ ├── get_acceleration_ellipses.m │ ├── run_SCP.m │ ├── shift_prev_data.m │ └── solve_QP.m ├── +evaluation │ ├── lap_time.m │ ├── lin_vs_st.m │ ├── paper.m │ ├── plot_t_opts.m │ ├── plot_track_with_speed.m │ ├── race_line.m │ └── sl_vs_scr.m ├── +model │ ├── +track │ │ ├── HockenheimGrandPrix.m │ │ ├── HockenheimShort.m │ │ ├── HockenheimShortCarMaker.m │ │ ├── HockenheimShortCarMaker.mat │ │ ├── add_turn.m │ │ ├── add_turn_N100.m │ │ ├── add_turn_N30.m │ │ ├── add_turn_N40.m │ │ ├── add_turn_N50.m │ │ ├── spline_to_checkpoints.m │ │ ├── testCircle.m │ │ ├── testCircuitA.m │ │ ├── testCircuitC.m │ │ ├── testCircuitE.m │ │ ├── testCircuitF.m │ │ ├── testCircuitLiniger.m │ │ ├── testCircuitSpline.m │ │ ├── testOvalA.m │ │ ├── testOvalB.m │ │ ├── testOvalC.m │ │ ├── testOvalD.m │ │ └── testOvalE.m │ └── +vehicle │ │ ├── Base.m │ │ ├── BaseOde.m │ │ ├── Linear.m │ │ ├── SingleTrack.m │ │ ├── SingleTrackWAccelerationController.m │ │ ├── state_st2lin.m │ │ ├── vector_global2local.m │ │ ├── vector_local2global.m │ │ └── velocity_global2local.m ├── +plots │ ├── #README.m │ ├── Base.m │ ├── Dashboard.m │ ├── DashboardAcceleration.m │ ├── Race.m │ ├── TrackCheckpoints.m │ └── TrackPolygons.m ├── +sim │ ├── #README.m │ ├── init_log.m │ ├── init_ws.m │ ├── run.m │ ├── simulate_ode.m │ └── update_administrative_data.m ├── +utils │ ├── +poly │ │ ├── cleanse_convex_polygon.m │ │ ├── con2vert.m │ │ ├── enlarge_polygon_into_forward_direction.m │ │ ├── get_track_polygon_vertices.m │ │ ├── plot_vertices.m │ │ ├── poly2vert.m │ │ ├── vert2con.m │ │ └── vert2poly.m │ ├── PID.m │ ├── exportAllFigures.m │ ├── exportFigure.m │ ├── getRwthColors.m │ ├── isToolboxAvailable.m │ ├── mod1.m │ ├── rat2str.m │ ├── removePiPeriodicity.m │ ├── sanitizeFilename.m │ ├── set_figure_properties.m │ └── yline_compatiblity.m ├── LICENSE ├── run ├── run.m ├── run_all.m ├── run_parameter_study.m └── run_replay.m ├── data ├── LICENSE └── singleTrackAMax.mat ├── environment ├── Dockerfile └── README.txt ├── metadata ├── README.txt └── metadata.yml └── results └── .gitkeep /.gitignore: -------------------------------------------------------------------------------- 1 | # Matlab files 2 | *.asv 3 | 4 | 5 | # custom files 6 | /data/* 7 | !/data/LICENSE 8 | /results/* 9 | !/results/.gitkeep -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016-2021 Bassam Alrifaee 4 | Copyright (c) 2017 Janis Maczijewski 5 | Copyright (c) 2021 Theodor Mario Henneken 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sequential Convex Programming Methods for Real-time Optimal Trajectory Planning in Autonomous Vehicle Racing 2 | 3 | [![Paper](https://img.shields.io/badge/-Paper-00629B?logo=IEEE)](https://doi.org/10.1109/TIV.2022.3168130) 4 | [![Repository](https://img.shields.io/badge/-GitHub-181717?logo=GitHub)](https://github.com/embedded-software-laboratory/sequential-convex-programming) 5 | [![Video](https://img.shields.io/badge/-Video-FF0000?logo=YouTube)](https://youtu.be/7Iwh980JMCs) 6 | [![Open in Code Ocean](https://codeocean.com/codeocean-assets/badge/open-in-code-ocean.svg)](https://codeocean.com/capsule/6818033/tree) 7 | 8 | ![GIF showing SCR and SL](https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/master/animation_SCR_vs_SL.gif) 9 | 10 | ## Abstract 11 | Optimization problems for trajectory planning in autonomous vehicle racing are characterized by their nonlinearity and nonconvexity. Instead of solving these optimization problems, usually a convex approximation is solved instead to achieve a high update rate. We present a real-time-capable model predictive control (MPC) trajectory planner based on a nonlinear single-track vehicle model and Pacejka's magic tire formula for autonomous vehicle racing. After formulating the general nonconvex trajectory optimization problem, we form a convex approximation using sequential convex programming (SCP). The state of the art convexifies track constraints using sequential linearization (SL), which is a method of relaxing the constraints. Solutions to the relaxed optimization problem are not guaranteed to be feasible in the nonconvex optimization problem. We propose sequential convex restriction (SCR) as a method to convexify track constraints. SCR guarantees that resulting solutions are feasible in the nonconvex optimization problem. We show recursive feasibility of solutions to the restricted optimization problem. The MPC is evaluated on a scaled version of the Hockenheimring racing track in simulation. The results show that an MPC using SCR yields faster lap times than an MPC using SL, while still being real-time capable. 12 | 13 | ## Instructions 14 | 0. clone or download this repo and open MATLAB in this folder 15 | 1. open folder 'code' via ```cd code``` in MATLAB 16 | 2. optional: choose a pre-configured scenario in file 'run.m' 17 | Alternatively, configure an individual configuration as described in 18 | the next paragraph 19 | 2. run the scenario via ```run()``` 20 | 21 | ## Paper Results Reproduction 22 | The script `code\+evaluation\paper.m` reproduces the simulation results. Afterwards, the results are available in the folder `results\`. 23 | ## Configuration 24 | In the folder 'code/+config', all configurations of scenarios and vehicles are stored. You can combine the building blocks to your likes or even create a completely new configuration 25 | 26 | ## Requirements 27 | - MATLAB 28 | - Version: recommended R2021a, minimum R2019a 29 | - Symbolic Math Toolbox 30 | - Statistics and Machine Learning Toolbox (required for evaluation only) 31 | - Solver 32 | - recommended IBM ILOG CPLEX Optimization Studio 12.10 33 | - alternatively, MATLAB's `quadprog` via 'Symbolic Math Toolbox' 34 | 35 | tested on UNIX (Ubuntu 18.04 64-bit) and Windows 10 64-bit, MATLAB 36 | R2021a, R2019b, R2019a 37 | 38 | ## Acknowledgements 39 | This research is supported by the Deutsche Forschungsgemeinschaft (DFG, German Research Foundation) within the Priority Program SPP 1835 Cooperative Interacting Automobiles and the Post Graduate Program GRK 1856 Integrated Energy Supply Modules for Roadbound E-Mobility. 40 | 41 | ## Reference 42 | 43 |
44 | 45 | [1] P. Scheffe, T. Henneken, M. Kloock, B. Alrifaee. 46 | "Sequential Convex Programming Methods for Real-time Optimal Trajectory Planning in Autonomous Vehicle Racing" 47 | 48 |

49 | 50 | ``` 51 | @ARTICLE{scheffe2022sequential, 52 | author={Scheffe, Patrick and Henneken, Theodor Mario and Kloock, Maximilian and Alrifaee, Bassam}, 53 | journal={IEEE Transactions on Intelligent Vehicles}, 54 | title={Sequential Convex Programming Methods for Real-time Optimal Trajectory Planning in Autonomous Vehicle Racing}, 55 | year={2022}, 56 | volume={}, 57 | number={}, 58 | pages={1-1}, 59 | doi={10.1109/TIV.2022.3168130} 60 | } 61 | ``` 62 | 63 |

64 |
-------------------------------------------------------------------------------- /algorithm_expand_convex_polygon.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/algorithm_expand_convex_polygon.pdf -------------------------------------------------------------------------------- /animation_SCR_vs_SL.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/animation_SCR_vs_SL.gif -------------------------------------------------------------------------------- /code/+config/#README.m: -------------------------------------------------------------------------------- 1 | % this package contains all configurations 2 | 3 | %% Config Types 4 | % there are 3 config types: 5 | % - config 6 | % overall settings 7 | % - scenarios 8 | % adds scenario specific settings to config, such as track, vehicles, 9 | % obstacles, etc. 10 | % - vehicles 11 | % are added in a scenario config, set vehicle-specific settings such 12 | % as model, weights, start position, etc. 13 | % 14 | % Naming Convention for models & parameters 15 | % 'vehicle__ 16 | % if not the same models and parameters are used for controller & simulation, it expands to 17 | % 'vehicle____' 18 | 19 | %% How to Use 20 | % each type has a default config, 'scenario' and 'vehicle'. This ensures shared parameters. 21 | 22 | % To change certain settings, you can use the existing configuration files 23 | % by cascading them, e.g. for a vehicle 24 | % 1. getting default vehicle parameters 25 | % 2. change vehicle model 26 | % 3. change track discreitization to SCR 27 | vehicle_config = ... 28 | config.vehicle_SCR(... 29 | config.vehicle_ST_Liniger(... 30 | config.base_vehicle())); 31 | -------------------------------------------------------------------------------- /code/+config/base_scenario.m: -------------------------------------------------------------------------------- 1 | function cfg = base_scenario(cfg) 2 | % base configuration for any scenario 3 | % contains all possible options 4 | cfg.scn.description = 'base scenario'; 5 | 6 | cfg.scn.obstacles = {}; 7 | cfg.scn.vhs = {}; 8 | 9 | %% General 10 | cfg.race.n_laps = 1; % Number of laps to be driven 11 | 12 | % only simulation for main vehicle (vehicle number 1) 13 | % remaining vehicles are only for comparison of overlayed controller outputs 14 | % main vehicle should have the most sophisticated simulation vehicle 15 | % model, other vehicles' simulation models aren't used 16 | cfg.scn.is_main_vehicle_only = false; 17 | 18 | %% Track 19 | % e.g. HockenheimShort, testCircuitA testOvalB testOvalD, testCircuitLiniger 20 | cfg.scn.track_handle = @model.track.HockenheimShort; 21 | % TODO SCR only works with hockenheim_simple? 22 | cfg.scn.track_SCR_epsilon_area_tolerance = .025; % [m^2], on a 1:1 scale. will be scaled according to track creation scale 23 | cfg.scn.track_polygons_filename = 'track_polygons.mat'; 24 | 25 | % minimum distance between checkpoints 26 | % zero won't change anything at given track 27 | % will be scaled arroding to track scale 28 | cfg.scn.track_checkpoints_distance_min = 0; 29 | 30 | %% Vehicles 31 | % CAVE here you need to add vehicles 32 | % ensure that the start position of the vehicles is on the first 33 | % lap regarding the track center-checkpoints (find examples in scenario 34 | % files) 35 | end -------------------------------------------------------------------------------- /code/+config/base_vehicle.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = base_vehicle(cfg) 2 | % base configuration for any vehicle 3 | % contains all possible options 4 | cfg_vh.description = 'base vehicle'; 5 | 6 | % required for ST to lin Liniger conversion 7 | cfg_vh.dataFileSingleTrackAMax = [cfg.dataPath 'singleTrackAMax.mat']; 8 | 9 | %% Controller: General Optimization 10 | cfg_vh.p.SCP_iterations = 1; 11 | % SL 1, SCR 2, Botz 1 12 | 13 | % CAVE: needs at least 2 p.SCP_iterations 14 | cfg_vh.p.isBlockingEnabled = false; 15 | cfg_vh.p.areObstaclesConsidered = false; 16 | 17 | cfg_vh.p.Hp = 40; % Number of prediction steps 18 | % SL 40, SCR 20, Botz 20, Liniger 40 19 | cfg_vh.p.dt_controller = 0.1; % [s] size of prediction step for controller 20 | % SL 0.15, SCR 0.5, Botz 0.1, Liniger 0.02 21 | 22 | % simulation step size is only relevant, if a controller to transform 23 | % inputs from controller to simulation model is neccessary. 24 | % in other cases, MATLAB's ODE solvers choose step-sizes by themselves 25 | cfg_vh.p.dt_simulation = cfg_vh.p.dt_controller/10; % [s] size of simulation step 26 | 27 | cfg_vh.p.S = 1e5; % weight for slack 28 | % SL 10, SCR 1e5, Botz 1e40, (Liniger 250) 29 | cfg_vh.p.Q = 10; % weight for maximization of position on track 30 | % SL 1, SCR 1, Botz 1, (Liniger 0.1 ... 10) 31 | cfg_vh.p.R = 10 * diag([10 .1]); % weight for control changes over time 32 | % SL 0.01, SCR 0.01, Botz 500, (Liniger 0.01 ... 1) 33 | 34 | 35 | %% Contoller: Miscellaneous Modelling 36 | % Acceleration: number of tangents around the ellipses 37 | cfg_vh.p.n_acceleration_limits = 16; 38 | 39 | % Linearization (SL): size of Trust Region for position 40 | % FIXME scale trust region size with track 41 | cfg_vh.p.trust_region_size = 0.1; % [m] adds/subtracts to position (SL only) 42 | % large trust region sizes yields more numerical issues or errors 43 | % SL 50, SCR not required, Botz 0.06 44 | 45 | %% Controller: Approximation Method 46 | % arbitrary IDs, saved for later usage 47 | cfg_vh.approximationSL = 10; cfg_vh.approximationSCR = 20; 48 | % choose approximation 49 | cfg_vh.approximation = cfg_vh.approximationSL; % 'approximationSL' or 'approximationSL' 50 | 51 | 52 | %% Model 53 | % CAVE: model params should match across controller and simulation model 54 | cfg_vh.model_controller_handle = @model.vehicle.Linear; 55 | cfg_vh.modelParams_controller = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 56 | cfg_vh.model_simulation_handle = cfg_vh.model_controller_handle; 57 | cfg_vh.modelParams_simulation = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 58 | 59 | %% Geometric 60 | % xStart [pos_x pox_y v_x v_y yaw dyaw/dt] will be initialized to match model states 61 | % grid start positions for 1 to 8 vehicles 62 | cfg_vh.x_starts = [... 63 | [0.1 0.05 0 0 0 0]'... 64 | [0.5 -0.05 0 0 0 0]'... 65 | [0.9 0.05 0 0 0 0]'... 66 | [1.3 -0.05 0 0 0 0]'... 67 | [1.7 0.05 0 0 0 0]'... 68 | [2.1 -0.05 0 0 0 0]'... 69 | [2.5 0.05 0 0 0 0]'... 70 | [2.8 -0.05 0 0 0 0]']; 71 | cfg_vh.x_start = cfg_vh.x_starts(:, 1); 72 | 73 | % FIXME adapt to scale. Define in vehicle model 74 | cfg_vh.lengthVal = 0.075; % obstacle's size measured along its direction of movement [m] 75 | cfg_vh.widthVal = 0.045; % obstacle's size measured at right angels to its direction of movement[m] 76 | 77 | % obstacles are modeled as rotated rectangles that can move with 78 | % constant speed and direction. 79 | cfg_vh.distSafe = 'Circle'; % Chose either 'Circle' or 'Ellipse' or 'CircleImpr' or 'EllipseImpr' 80 | cfg_vh.distSafe2CenterVal_1 = round(sqrt((cfg_vh.lengthVal/2)^2 + (cfg_vh.widthVal/2)^2),2,'significant'); 81 | cfg_vh.distSafe2CenterVal_2 = [0.09;0.06]; % Definition of ellipsis (two semi-axis) 82 | end -------------------------------------------------------------------------------- /code/+config/compute_ST_flying_initialization.m: -------------------------------------------------------------------------------- 1 | function [x_0, X_controller_start] = compute_ST_flying_initialization() 2 | filepath = "../results/tmp/x_init_flying.mat"; 3 | try 4 | f = load(filepath); % loads x_0 and X_controller_start 5 | x_0 = f.x_0; 6 | X_controller_start = f.X_controller_start; 7 | catch 8 | scenarios = run(true); 9 | scenario = scenarios(4); % scr 10 | scenario.outputPath = '../results/tmp/'; 11 | n_laps = 2; 12 | scenario = config.scenario_n_laps_race(scenario, n_laps); 13 | output_file = sim.run(scenario); 14 | sim_result = load(output_file); 15 | i_last_1 = find([sim_result.log.vehicles{1}.lap_count] == 0,1, 'last'); 16 | X_controller_start = sim_result.log.vehicles{1}(i_last_1).X_controller; 17 | X_controller_start = X_controller_start + [0;0;0;0;2*pi;0]; 18 | trackWidth = 0.25; 19 | x_0 = sim_result.log.vehicles{1}(i_last_1).x_0; 20 | x_0(1,1) = 0.1; 21 | x_0(2,1) = trackWidth/2; 22 | x_0(4,1) = 0; 23 | x_0(5,1) = 0; 24 | x_0(6,1) = 0; 25 | % v_long = x(3); 26 | % v_lat = x(4); 27 | % yaw = x(5); 28 | % dyaw = x(6); 29 | save(filepath, ... 30 | 'x_0', 'X_controller_start' ... 31 | ); 32 | end 33 | end -------------------------------------------------------------------------------- /code/+config/config.m: -------------------------------------------------------------------------------- 1 | function cfg = config() 2 | cfg = struct; 3 | 4 | % disable trace in case of warnings 5 | warning("Disabling warning's backtracing") 6 | warning off backtrace 7 | 8 | %% Main Parameters 9 | % folder of CPLEX' MATLAB connector & all necessary run files 10 | % cfg.env.cplex.path = 'D:/#local Apps/CPLEX_MATLAB_x64'; 11 | % typical installation dir: 12 | cfg.env.cplex.path = 'C:/Program Files/IBM/ILOG/CPLEX_Studio1210/cplex/matlab/x64_win64'; 13 | 14 | 15 | %% CPLEX Detection 16 | % if CPLEX is in path: use it 17 | if exist(cfg.env.cplex.path, 'dir') 18 | addpath(cfg.env.cplex.path); 19 | if exist('cplexqp', 'file') == 6 20 | disp('Using CPLEX for solving QP') 21 | cfg.env.cplex.is_available = true; 22 | else 23 | fprintf("Using MATLAB for solving QP as CPLEX files in path '%s' not existing\nMATLAB's quadprog could fail when compared to CPLEX\n", cfg.env.cplex.path) 24 | cfg.env.cplex.is_available = false; 25 | end 26 | else 27 | fprintf("Using MATLAB for solving QP as CPLEX path '%s' is not existing\nMATLAB's quadprog could fail when compared to CPLEX\n", cfg.env.cplex.path) 28 | cfg.env.cplex.is_available = false; 29 | end 30 | 31 | % if use MATLAB's quadprog, check if toolbox is available 32 | if ~cfg.env.cplex.is_available && ~utils.isToolboxAvailable('Optimization Toolbox') 33 | error("neither CPLEX nor MATLAB's 'Optimization Toolbox' is available - install any of those"); 34 | end 35 | 36 | 37 | 38 | 39 | cfg.startTimeStr = datestr(now, 'yyyy.mm.dd_HH_MM_SS'); 40 | 41 | % paths (must have trailing slash) 42 | if usejava('desktop') % if run graphically 43 | cfg.outputPath = ['../results/', cfg.startTimeStr, '/']; 44 | else % if on CodeOcean (or headless, in general) 45 | cfg.outputPath = '../results/'; 46 | end 47 | cfg.dataPath = '../data/'; 48 | cfg.tmpPath = '../results/tmp/'; 49 | 50 | %% Logging 51 | cfg.log.DEBUG = 4; 52 | cfg.log.LOG = 3; 53 | cfg.log.INFO = 2; 54 | cfg.log.WARN = 1; 55 | cfg.log.ERROR = 0; 56 | 57 | cfg.log.level = cfg.log.DEBUG; % log level: choose from above 58 | 59 | %% Plotting 60 | cfg.plot.is_enabled = true; 61 | cfg.plot.has_accelerations = true; 62 | cfg.plot.has_focus_on_vehicle = false; 63 | cfg.plot.grayscale = false; 64 | 65 | % fixed plots drawn at initialization 66 | % CAVE: hard-coded in init, here only for proper figure order 67 | plots.TrackPolygons(1); 68 | 69 | % init loop plots - make sure to give unique figure numbers 70 | cfg.plot.plots_to_draw = { 71 | plots.Race(10) 72 | plots.Dashboard(11) 73 | plots.DashboardAcceleration(12)}; 74 | end -------------------------------------------------------------------------------- /code/+config/init_config.m: -------------------------------------------------------------------------------- 1 | function cfg = init_config(cfg) 2 | % initializes given cfg 3 | % 4 | % Initializes handles and automatic detections 5 | % 6 | % Returns: initialized config struct 7 | 8 | if isempty(cfg.scn.vhs) 9 | error('scenario without vehicles!') 10 | end 11 | 12 | % create output & temp dir if non-existing 13 | if ~isfolder(cfg.outputPath); mkdir(cfg.outputPath); end 14 | if ~isfolder(cfg.dataPath); mkdir(cfg.dataPath); end 15 | if ~isfolder(cfg.tmpPath); mkdir(cfg.tmpPath); end 16 | 17 | %% Vehicle Models 18 | for i = 1:length(cfg.scn.vhs) 19 | % detect if model is linear 20 | cfg.scn.vhs{i}.isControlModelLinear = isequal(cfg.scn.vhs{i}.model_controller_handle, @model.vehicle.Linear); 21 | cfg.scn.vhs{i}.isSimulationModelLinear = isequal(cfg.scn.vhs{i}.model_simulation_handle, @model.vehicle.Linear); 22 | 23 | % Instantiate vehicle models 24 | cfg.scn.vhs{i}.model_controller = ... 25 | cfg.scn.vhs{i}.model_controller_handle(cfg.scn.vhs{i}.p.Hp, cfg.scn.vhs{i}.p.dt_controller, cfg.scn.vhs{i}.modelParams_controller); 26 | % initialization of the jacobians for linearization is crucial for 27 | % gaining computational speed in case of controllers 28 | if ~cfg.scn.vhs{i}.isControlModelLinear 29 | cfg.scn.vhs{i}.model_controller.precomputeJacobian(); 30 | end 31 | 32 | cfg.scn.vhs{i}.model_simulation = ... 33 | cfg.scn.vhs{i}.model_simulation_handle(cfg.scn.vhs{i}.p.Hp, cfg.scn.vhs{i}.p.dt_simulation, cfg.scn.vhs{i}.modelParams_simulation); 34 | 35 | if ~isfield(cfg.scn.vhs{i}.modelParams_controller, 'bounds') 36 | warning("Vehicle's %i controller model has no bounds (for linear models it doesn't matter)", i); 37 | end 38 | if ~isfield(cfg.scn.vhs{i}.modelParams_simulation, 'bounds') 39 | warning("Vehicle's %i simulation model has no bounds (for linear models it doesn't matter)", i); 40 | end 41 | 42 | % reduce start states to match simulation model states 43 | if ~isequal(size(cfg.scn.vhs{i}.x_start), [6 1]) 44 | warning("size of start states doesn't match - it's ok in case of replay of linear model - else: investigate"); 45 | end 46 | cfg.scn.vhs{i}.x_start = cfg.scn.vhs{i}.x_start(1:cfg.scn.vhs{i}.model_simulation.nx); 47 | 48 | %% Inputs 49 | % SL Acceleration: pre-compute for speed 50 | delta_angle = 2*pi / cfg.scn.vhs{i}.p.n_acceleration_limits; 51 | tmp = (1:cfg.scn.vhs{i}.p.n_acceleration_limits)' .* delta_angle; 52 | cfg.scn.vhs{i}.p.acceleration_cos = cos(tmp); 53 | cfg.scn.vhs{i}.p.acceleration_sin = sin(tmp); 54 | end 55 | 56 | %% Approximation 57 | for i = 1:length(cfg.scn.vhs) 58 | cfg.scn.vhs{i}.approximationIsSL = ... 59 | cfg.scn.vhs{i}.approximation == cfg.scn.vhs{i}.approximationSL; 60 | cfg.scn.vhs{i}.approximationIsSCR = ... 61 | cfg.scn.vhs{i}.approximation == cfg.scn.vhs{i}.approximationSCR; 62 | 63 | if ~cfg.scn.vhs{i}.approximationIsSL && ~cfg.scn.vhs{i}.approximationIsSCR 64 | throw ME('No valid approximation chosen') 65 | end 66 | end 67 | 68 | %% Track 69 | [cfg.scn.track, cfg.scn.track_creation_scale] = cfg.scn.track_handle(); 70 | % Reduce checkpoints 71 | cfg.scn.track = controller.track_SL.reduce_checkpoints(cfg.scn.track, cfg.scn.track_checkpoints_distance_min, cfg.scn.track_creation_scale); 72 | % Preallocate for speed 73 | cfg.scn.track_center = [cfg.scn.track.center]; 74 | 75 | % Polygon Creation (for SCR) 76 | % if any vehicle uses SCR controller 77 | cfg_array = [cfg.scn.vhs{:}]; 78 | if any([cfg_array.approximationIsSCR]) 79 | warning("Disabling 'MATLAB:polyshape:repairedBySimplify' warnings") 80 | warning off MATLAB:polyshape:repairedBySimplify 81 | 82 | try 83 | f=load([cfg.tmpPath, cfg.scn.track_polygons_filename]); 84 | cfg.scn.track_SCR = f.track_SCR; 85 | catch 86 | disp('Generating SCR track model - this can take a few seconds') 87 | [cfg.scn.track_SCR, track_tesselated, track_merged] = controller.track_SCR.generate(cfg.scn.track, cfg.scn.track_creation_scale, cfg.scn.track_SCR_epsilon_area_tolerance); 88 | track_SCR = cfg.scn.track_SCR; 89 | save([cfg.tmpPath, cfg.scn.track_polygons_filename], ... 90 | 'track_SCR', 'track_tesselated', 'track_merged' ... 91 | ); 92 | 93 | % plot track 94 | plots.TrackPolygons(1).plot(cfg.scn.track, track_tesselated, track_merged, cfg.scn.track_SCR, cfg.scn.track_creation_scale, cfg.scn.track_SCR_epsilon_area_tolerance); 95 | 96 | end 97 | end 98 | 99 | % Plotting 100 | cfg.plot.grayscale = false; 101 | 102 | end -------------------------------------------------------------------------------- /code/+config/scenario_1_vehicle.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_1_vehicle(cfg, vehicle_config_handle) 2 | % wrapper for a scenario with one givenvehicle 3 | cfg.scn.description = [cfg.scn.description '\nwith 1 vehicle only']; 4 | 5 | cfg.scn.vhs{end + 1} = vehicle_config_handle(config.base_vehicle(cfg)); 6 | end -------------------------------------------------------------------------------- /code/+config/scenario_SCR_vs_SL.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_SCR_vs_SL(cfg) 2 | % Showcasing SL vs SCR track discretization 3 | cfg.scn.description = [cfg.scn.description '\nwith comparison of SL vs SCR track discretization controller']; 4 | 5 | %% Vehicles 6 | vehicle_default = config.vehicle_ST_Liniger(config.base_vehicle(cfg)); 7 | 8 | 9 | % vehicle 1: SCR 10 | cfg.scn.vhs{end + 1} = config.vehicle_SCR(vehicle_default); 11 | 12 | % vehicle 2: SL 13 | cfg.scn.vhs{end + 1} = vehicle_default; -------------------------------------------------------------------------------- /code/+config/scenario_SL_reduce_checkpoints.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_SL_reduce_checkpoints(cfg) 2 | % reduce number of checkpoints of track discretization 3 | cfg.scn.description = [cfg.scn.description '\nwith reduced number of checkpoints']; 4 | 5 | % will be scaled arroding to track scale 6 | % equals 1m on 1:1 scale, ~0.023m on 1:43 scale 7 | cfg.scn.track_checkpoints_distance_min = 0.023 * 43/1; -------------------------------------------------------------------------------- /code/+config/scenario_SL_vs_SCR.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_SL_vs_SCR(cfg) 2 | % Showcasing SL vs SCR track discretization 3 | cfg.scn.description = [cfg.scn.description '\nwith comparison of SL vs SCR track discretization controller']; 4 | 5 | %% Vehicles 6 | vehicle_default = config.vehicle_ST_Liniger(config.base_vehicle(cfg)); 7 | 8 | % vehicle 1: SL 9 | cfg.scn.vhs{end + 1} = vehicle_default; 10 | 11 | % vehicle 2: SCR 12 | cfg.scn.vhs{end + 1} = config.vehicle_SCR(vehicle_default); -------------------------------------------------------------------------------- /code/+config/scenario_endless_race.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_endless_race(cfg) 2 | % making race endless (or close to) 3 | cfg.scn.description = [cfg.scn.description '\nwith endless race']; 4 | 5 | cfg.race.n_laps = round(realmax); % Number of laps to be driven 6 | end -------------------------------------------------------------------------------- /code/+config/scenario_flying_start.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_flying_start(cfg) 2 | % making race endless (or close to) 3 | cfg.scn.description = [cfg.scn.description '\nwith flying start']; 4 | 5 | [x_0, X_controller_start] = config.compute_ST_flying_initialization(); 6 | 7 | for i = 1:numel(cfg.scn.vhs) 8 | cfg.scn.vhs{i}.x_start = x_0; 9 | cfg.scn.vhs{i}.X_controller_start = X_controller_start; 10 | end 11 | end -------------------------------------------------------------------------------- /code/+config/scenario_lin_vs_ST.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_lin_vs_ST(cfg) 2 | % Showcasing linear vs. non-linear vehicle model controller 3 | cfg.scn.description = [cfg.scn.description '\nwith linear vs single-track vehicle model controller']; 4 | 5 | %% Vehicles 6 | vehicle_default = config.base_vehicle(cfg); 7 | 8 | % vehicle 1: single-track liniger 9 | vh = config.vehicle_ST_Liniger(vehicle_default); 10 | cfg.scn.vhs{end + 1} = vh; 11 | 12 | % vehicle 2: linear liniger 13 | vh = config.vehicle_lin_Liniger(vehicle_default); 14 | cfg.scn.vhs{end + 1} = vh; 15 | end -------------------------------------------------------------------------------- /code/+config/scenario_main_vehicle_SL_vs_SCR.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_main_vehicle_SL_vs_SCR(cfg) 2 | % Showcasing SL vs SCR track discretization 3 | cfg.scn.description = [cfg.scn.description '\nwith comparison of SL vs SCR track discretization controller (executed on one vehicle)']; 4 | 5 | cfg.scn.is_main_vehicle_only = true; 6 | 7 | %% Vehicles 8 | vehicle_default = config.vehicle_ST_Liniger(config.base_vehicle(cfg)); 9 | 10 | % vehicle 1: SCR 11 | cfg.scn.vhs{end + 1} = config.vehicle_SCR(vehicle_default); 12 | 13 | % vehicle 2: SL 14 | vehicle_default.p.trust_region_size = vehicle_default.p.trust_region_size * 0.5; 15 | cfg.scn.vhs{end + 1} = vehicle_default; -------------------------------------------------------------------------------- /code/+config/scenario_main_vehicle_lin_vs_ST.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_main_vehicle_lin_vs_ST(cfg) 2 | % Showcasing linear vs. non-linear vehicle model controller overlayed 3 | cfg.scn.description = [cfg.scn.description '\nwith comparison of lin vs ST model controller (executed on one vehicle)']; 4 | 5 | cfg.scn.is_main_vehicle_only = true; 6 | 7 | %% Vehicles 8 | vehicle_default = config.base_vehicle(cfg); 9 | 10 | % vehicle 1: single-track liniger 11 | vh = config.vehicle_ST_Liniger(vehicle_default); 12 | cfg.scn.vhs{end + 1} = vh; 13 | 14 | % vehicle 2: linear liniger 15 | vh = config.vehicle_lin_Liniger(vehicle_default); 16 | cfg.scn.vhs{end + 1} = vh; 17 | end -------------------------------------------------------------------------------- /code/+config/scenario_models_comparison_linear.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_models_comparison_linear(cfg) 2 | % EXPERIMENTAL scenario to compare vehicle models: 3 | % single-track (controller & simulation) vs. 4 | % linear/single-track (controller/simulation) vs. 5 | % linear(controller & simulation) 6 | cfg.scn.description = [cfg.scn.description '\nwith ST/ST vs lin/ST vs lin/lin controller/simulation']; 7 | 8 | 9 | %% Vehicles 10 | base_vehicle = config.base_vehicle(cfg); 11 | 12 | % vehicle 1: single-track liniger 13 | cfg.scn.vhs{end + 1} = config.vehicle_ST_Liniger(base_vehicle); 14 | 15 | % vehicle 2: linear liniger controller, single-track liniger simulation 16 | cfg.scn.vhs{end + 1} = config.vehicle_lin_Liniger_ST_Liniger(base_vehicle); 17 | 18 | % vehicle 3: linear liniger 19 | cfg.scn.vhs{end + 1} = config.vehicle_lin_Liniger(base_vehicle); 20 | end -------------------------------------------------------------------------------- /code/+config/scenario_n_laps_race.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_n_laps_race(cfg, n_laps) 2 | % making race endless (or close to) 3 | cfg.scn.description = [cfg.scn.description '\n' sprintf('with %i laps race', n_laps)]; 4 | 5 | cfg.race.n_laps = n_laps; % Number of laps to be driven 6 | end -------------------------------------------------------------------------------- /code/+config/scenario_paper_SL.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_paper_SL(cfg) 2 | % scenario similar to original SL paper 3 | % (B. Alrifaee and J. Maczijewski, “Real-time Trajectory optimization for 4 | % Autonomous Vehicle Racing using Sequential Linearization,” in 2018 IEEE 5 | % Intelligent Vehicles Symposium (IV), Changshu, Jun. 2018, pp. 476–483. 6 | % doi: 10.1109/IVS.2018.8500634.) 7 | cfg.scn.description = [cfg.scn.description '\nreplicating "SL paper" settings']; 8 | 9 | %% Track 10 | cfg.scn.track_handle = @model.track.HockenheimShortCarMaker; 11 | cfg.scn.track_SCR_epsilon_area_tolerance = .5; % [m^2] 12 | 13 | %% Vehicles 14 | vehicle_ = config.vehicle_paper_SL(config.base_vehicle(cfg)); 15 | 16 | vehicle_.p.Hp = 40; 17 | vehicle_.p.dt_controller = 0.15; % [s] size of prediction step for controller 18 | % SL 0.15, SCR 0.5, Botz 0.1, Liniger 0.02 19 | 20 | % simulation step size is only relevant, if a controller to transform 21 | % inputs from controller to simulation model is neccessary. 22 | % in other cases, MATLAB's ODE solvers choose step-sizes by themselves 23 | vehicle_.p.dt_simulation = vehicle_.p.dt_controller/10; % [s] size of simulation step 24 | 25 | vehicle_.p.S = 10; % weight for slack 26 | vehicle_.p.Q = 1; % weight for maximization of position on track 27 | vehicle_.p.R = 0.01 * diag([10 .1]); % weight for control changes over time 28 | 29 | 30 | %% Contoller: Miscellaneous Modelling 31 | % Acceleration: number of tangents around the ellipses 32 | vehicle_.p.n_acceleration_limits = 16; 33 | vehicle_.p.trust_region_size = 50; % [m] adds/subtracts to position 34 | 35 | %% 36 | cfg.scn.vhs{end + 1} = vehicle_; 37 | end -------------------------------------------------------------------------------- /code/+config/scenario_race_various_vehicles.m: -------------------------------------------------------------------------------- 1 | function cfg = scenario_race_various_vehicles(cfg) 2 | % Showcasing SL vs SCR track discretization 3 | cfg.scn.description = [cfg.scn.description '\nwith a race with obstacle and vehicle avoidance with various vehicles/controllers']; 4 | 5 | %% Vehicles 6 | vehicle_default = config.vehicle_avoidance(config.vehicle_ST_Liniger(config.base_vehicle(cfg))); 7 | % make less aggressive 8 | vehicle_default.p.trust_region_size = vehicle_default.p.trust_region_size * 0.7; 9 | vehicle_default.p.Q = vehicle_default.p.Q * 0.5; 10 | 11 | % vehicle 1: SL 12 | vehicle_1 = vehicle_default; 13 | vehicle_1.x_start = vehicle_1.x_starts(:, 8); 14 | % enforce overtaking situations by reducing max velocity 15 | vehicle_1.modelParams_controller.bounds(2, 3) = 1.5; 16 | cfg.scn.vhs{end + 1} = vehicle_1; 17 | 18 | % vehicle 2: SCR 19 | vehicle_default.x_start = vehicle_default.x_starts(:, 7); 20 | cfg.scn.vhs{end + 1} = vehicle_default; 21 | vehicle_default.x_start = vehicle_default.x_starts(:, 6); 22 | cfg.scn.vhs{end + 1} = vehicle_default; 23 | % vehicle_default.x_start = vehicle_default.x_starts(:, 5); 24 | % cfg.scn.vhs{end + 1} = vehicle_default; 25 | % vehicle_default.x_start = vehicle_default.x_starts(:, 4); 26 | % cfg.scn.vhs{end + 1} = vehicle_default; 27 | % vehicle_default.x_start = vehicle_default.x_starts(:, 3); 28 | % cfg.scn.vhs{end + 1} = vehicle_default; 29 | % vehicle_default.x_start = vehicle_default.x_starts(:, 2); 30 | % cfg.scn.vhs{end + 1} = vehicle_default; 31 | % vehicle_default.x_start = vehicle_default.x_starts(:, 1); 32 | % cfg.scn.vhs{end + 1} = vehicle_default; -------------------------------------------------------------------------------- /code/+config/vehicle_SCR.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_SCR(cfg_vh) 2 | % adapts vehicle's apprxoimation to SCR 3 | cfg_vh.description = [cfg_vh.description '\nwith SCR track discretization']; 4 | 5 | cfg_vh.approximation = cfg_vh.approximationSCR; % % 'approximationSL' or 'approximationSL' 6 | end -------------------------------------------------------------------------------- /code/+config/vehicle_ST_Kloock.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_ST_Kloock(cfg_vh) 2 | % adapts vehicle to Single Track Model Kloock 3 | cfg_vh.description = [cfg_vh.description '\nwith single-track vehicle model & Kloock params']; 4 | 5 | %% Model 6 | % CAVE: model params should match across controller and simulation model 7 | cfg_vh.model_controller_handle = @model.vehicle.SingleTrack; 8 | cfg_vh.modelParams_controller = model.vehicle.SingleTrack.getParamsKloockRC_1_43_WithLinigerBounds(); 9 | cfg_vh.model_simulation_handle = cfg_vh.model_controller_handle; 10 | cfg_vh.modelParams_simulation = model.vehicle.SingleTrack.getParamsKloockRC_1_43_WithLinigerBounds(); 11 | end -------------------------------------------------------------------------------- /code/+config/vehicle_ST_Liniger.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_ST_Liniger(cfg_vh) 2 | % adapts vehicle to Single Track Model Liniger 3 | cfg_vh.description = [cfg_vh.description '\nwith single-track vehicle model & Liniger params']; 4 | 5 | %% from Parameter Study (HockenheimShort, SL & SCR) 6 | cfg_vh.p.Q = 1.5; % or even more?2.6 for maximum aggressive but close to instable; % weight for maximization of position on track 7 | cfg_vh.p.R = diag([90 90]); % 41 for maximum aggrasion: 14.6s. 90: 15.1 % weight for control changes over time 8 | cfg_vh.p.trust_region_size = 1.6; % [m] adds/subtracts to position (SL only) 9 | 10 | %% Model 11 | % CAVE: model params should match across controller and simulation model 12 | cfg_vh.model_controller_handle = @model.vehicle.SingleTrack; 13 | cfg_vh.modelParams_controller = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 14 | cfg_vh.model_simulation_handle = cfg_vh.model_controller_handle; 15 | cfg_vh.modelParams_simulation = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 16 | end -------------------------------------------------------------------------------- /code/+config/vehicle_avoidance.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_avoidance(cfg_vh) 2 | % adapts vehicle's apprxoimation to SCR 3 | cfg_vh.description = [cfg_vh.description '\nwith collision avoidance']; 4 | 5 | cfg_vh.p.areObstaclesConsidered = true; 6 | if cfg_vh.p.SCP_iterations < 2 7 | cfg_vh.p.SCP_iterations = 2; 8 | end 9 | end -------------------------------------------------------------------------------- /code/+config/vehicle_blocking.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_blocking(cfg_vh) 2 | % adapts vehicle's apprxoimation to SCR 3 | cfg_vh.description = [cfg_vh.description '\nwith blocking of other vehicles']; 4 | 5 | cfg_vh.p.isBlockingEnabled = true; 6 | end -------------------------------------------------------------------------------- /code/+config/vehicle_lin_Liniger.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_lin_Liniger(cfg_vh) 2 | % adapts vehicle to linear Liniger model 3 | cfg_vh.description = [cfg_vh.description '\nwith linear model & params from ST Liniger']; 4 | 5 | %% somewhat optimized (HockenheimShort, SL & SCR) 6 | cfg_vh.p.Q = 1; % weight for maximization of position on track 7 | cfg_vh.p.R = diag([.1 .1]); % weight for control changes over time 8 | cfg_vh.p.trust_region_size = 1.6; % [m] adds/subtracts to position (SL only) 9 | 10 | %% Model 11 | % CAVE: model params should match across controller and simulation model 12 | cfg_vh.model_controller_handle = @model.vehicle.Linear; 13 | cfg_vh.modelParams_controller = model.vehicle.Linear.getParamsSingleTrackLiniger(cfg_vh.p.dt_controller, cfg_vh.dataFileSingleTrackAMax); 14 | cfg_vh.model_simulation_handle = cfg_vh.model_controller_handle; 15 | % CAVE FIXME linear models get simulated differently --> using 16 | % `dt_controller` instead of `dt_simulation` for now 17 | cfg_vh.modelParams_simulation = model.vehicle.Linear.getParamsSingleTrackLiniger(cfg_vh.p.dt_controller, cfg_vh.dataFileSingleTrackAMax); -------------------------------------------------------------------------------- /code/+config/vehicle_lin_Liniger_ST_Liniger.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_lin_Liniger_ST_Liniger(cfg_vh) 2 | % adapts vehicle to linear Liniger control and single-track liniger simulation model 3 | cfg_vh.description = [cfg_vh.description '\nwith linear controll / single-track model & params from ST Liniger']; 4 | 5 | warning('vehicle_lin_Liniger_ST_Liniger is non-functional!') 6 | 7 | cfg_vh = config.vehicle_lin_Liniger(cfg_vh); 8 | cfg_vh.model_simulation_handle = @model.vehicle.SingleTrackWAccelerationController; 9 | cfg_vh.modelParams_simulation = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 10 | end -------------------------------------------------------------------------------- /code/+config/vehicle_paper_SL.m: -------------------------------------------------------------------------------- 1 | function cfg_vh = vehicle_paper_SL(cfg_vh) 2 | % adapts vehicle similar to original SL paper 3 | % (B. Alrifaee and J. Maczijewski, “Real-time Trajectory optimization for 4 | % Autonomous Vehicle Racing using Sequential Linearization,” in 2018 IEEE 5 | % Intelligent Vehicles Symposium (IV), Changshu, Jun. 2018, pp. 476–483. 6 | % doi: 10.1109/IVS.2018.8500634.) 7 | cfg_vh.description = [cfg_vh.description '\nreplicating "SL paper" settings (linear models, specific weights & MPC params)']; 8 | 9 | %% Controller: General Optimization 10 | cfg_vh.p.SCP_iterations = 1; 11 | 12 | cfg_vh.p.Hp = 40; % Number of prediction steps 13 | cfg_vh.p.dt_controller = 0.15; % Size of prediction step 14 | cfg_vh.p.dt_simulation = cfg_vh.p.dt_controller/10; % [s] size of simulation step 15 | 16 | cfg_vh.p.S = 10; % Penalty weight for slack (was 1e30 for usage in quad objective with BotzBicycle) 17 | cfg_vh.p.Q = 1; 18 | cfg_vh.p.R = 0.01 * eye(2); % Penalty weight for control changes over time 19 | 20 | cfg_vh.approximation = cfg_vh.approximationSL; 21 | 22 | cfg_vh.x_start = [0 0 .1 0 0 0]'; 23 | 24 | %% Contoller: Miscellaneous Modelling 25 | % Linearization (SL): size of Trust Region for position 26 | cfg_vh.p.trust_region_size = 50; % from Janis, 1:1 scale 27 | 28 | %% Model 29 | % CAVE: model params should match across controller and simulation model 30 | cfg_vh.model_controller_handle = @model.vehicle.Linear; 31 | cfg_vh.modelParams_controller = model.vehicle.Linear.getParamsF1CarMaker(cfg_vh.p.dt_controller); 32 | cfg_vh.model_simulation_handle = cfg_vh.model_controller_handle; 33 | cfg_vh.modelParams_simulation = cfg_vh.modelParams_controller; 34 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/find_closest_most_forward_polygon_index.m: -------------------------------------------------------------------------------- 1 | function [polygon_index, distance_to_polygon] = find_closest_most_forward_polygon_index(position, track) 2 | % for given position, find the most forward track polygon. As a 3 | % fallback, the closest polygon is given out (e.g., in case if outside 4 | % track limits or numerical issues) 5 | % 6 | % FIXME: limitations of accuracy: vehicle's position can slip into next 7 | % polygon enforcing a sub-optimal racing line - but still being consistent 8 | % at round time and beating SL. Especially happens at corners 9 | 10 | %% find closest polygon to and polygons containing position 11 | constraints_upscaled = track.constraints_upscaled; 12 | distance_to_polygon_min = Inf; 13 | % polygons containing position 14 | containing_polygons_indices = []; 15 | for j = 1:length(constraints_upscaled) 16 | % distance to edge of polygon 17 | % max because polygon is in halfspace-represenation -> 18 | % negative: inside polygon; positive: outside 19 | % case negative: edge which is closest 20 | % case positive: edge which is furthest away 21 | distance_to_polygon = max(constraints_upscaled(j).A * position - constraints_upscaled(j).b); 22 | 23 | 24 | % save closest polygon (actually not required as soon as polygon 25 | % conatining position is found) 26 | if distance_to_polygon_min > distance_to_polygon 27 | distance_to_polygon_min = distance_to_polygon; 28 | distance_to_polygon_min_index = j; 29 | end 30 | 31 | % save polygon if containing position 32 | if distance_to_polygon <= 0 33 | containing_polygons_indices(end + 1) = j; %#ok 34 | end 35 | 36 | % debug: plot every polygon and position 37 | %figure(998) 38 | %clf 39 | %plot(polygon_vertices(1, :), polygon_vertices(2, :)) % polygon 40 | %axis equal 41 | %hold on 42 | %plot(positions(1, k), positions(2, k), 'r+') % position 43 | end 44 | 45 | % Save closest or most forward polygon index 46 | % if any position-containing polygons found: choose most forward 47 | % one 48 | if ~isempty(containing_polygons_indices) 49 | % calculate differences between polygons 50 | % wrapping around from nth to 1st polygon (with absolute 51 | % difference) 52 | % distance between each polygon in [#polygons] 53 | distance_between_polygon_indices = diff(... 54 | [containing_polygons_indices... 55 | containing_polygons_indices(1) + length(constraints_upscaled)]); 56 | 57 | % we define the last polygon cointaing position as the one having the 58 | % largest distance to the next polygon in [#polygons] 59 | [~,most_forward_polygon_index_index] = max(distance_between_polygon_indices); 60 | 61 | % extract 62 | polygon_index = containing_polygons_indices(most_forward_polygon_index_index); 63 | else 64 | warning("position couldn't be located inside any polygon. Choosing closest polygon instead. (This can happen if track area was slackened or due to numerical issues)") 65 | polygon_index = distance_to_polygon_min_index; 66 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/find_closest_most_forward_polygon_indices.m: -------------------------------------------------------------------------------- 1 | function polygon_indices = find_closest_most_forward_polygon_indices(positions, track, Hp) 2 | % For each trajectory point, find the most forward track polygon. As a 3 | % fallback, the closest polygon is given out (e.g., in case if outside 4 | % track limits or numerical issues) 5 | 6 | polygon_indices = nan(Hp, 1); 7 | 8 | for k = 1:Hp 9 | polygon_indices(k) = controller.track_SCR.find_closest_most_forward_polygon_index(positions(:, k), track); 10 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/generate.m: -------------------------------------------------------------------------------- 1 | function [track_fullSCR, track_tesselated, track_merged] = generate(checkpoints, track_scale, epsilon_area_tolerance) 2 | % generate restricted track subset 3 | 4 | %% Actual SCR 5 | % 1) tesselation (based on discret track checkpoints) 6 | track_tesselated = controller.track_SCR.t1_tesselate(checkpoints); 7 | % 2) merge polygons 8 | track_merged = controller.track_SCR.t2_merge(track_tesselated, track_scale, epsilon_area_tolerance); 9 | % 3) add overlaps (first forwards, than backwards [herbey unioning forward an backward overlap]) 10 | track_overlapped_forward = controller.track_SCR.t3_overlap(checkpoints, track_merged, track_scale, true); 11 | track_overlapped = controller.track_SCR.t3_overlap(checkpoints, track_merged, track_scale, false, track_overlapped_forward); 12 | 13 | % calculate forward directions of polygons 14 | track_fullSCR = controller.track_SCR.t9_add_forward_direction(track_overlapped, checkpoints); 15 | 16 | 17 | %% enlarge track_merged's constraints for polygon search (/ extract centroids) 18 | % if you want to find actual (non-overlapped) polygon: 19 | % use `track_merged` instead of `track_overlapped` 20 | if false 21 | track_find_polygons = track_merged; %#ok<*UNRCH> 22 | % if you want to find most forward polygon (fastest lap times) 23 | else 24 | track_find_polygons = track_overlapped; 25 | end 26 | 27 | constraints_upscaled = struct; 28 | %centroids = nan(2, length(track_fullSCR.polygons)); 29 | for j = 1:length(track_find_polygons.polygons) 30 | poly_current = utils.poly.vert2poly(utils.poly.get_track_polygon_vertices(j, track_find_polygons)); 31 | 32 | % enlarge polygon slighlty for numerical reasons 33 | [poly_current_center(1), poly_current_center(2)] = poly_current.centroid; 34 | % scale down w.r.t. center 35 | % scaling down enlarged so that only track width is 36 | % reduced minimallistically (as enlarged and next 37 | % neighbour overlap --> scaling has no effect here) 38 | poly_current = poly_current.scale(1 + 1e-3, poly_current_center); 39 | 40 | [constraints_upscaled(j).A, constraints_upscaled(j).b] = utils.poly.vert2con(utils.poly.poly2vert(poly_current)'); 41 | 42 | %[centroids(1, j), centroids(2, j)] = poly_current.centroid(); 43 | end 44 | % save 45 | track_fullSCR.constraints_upscaled = constraints_upscaled; 46 | %track_fullSCR.centroids = centroids; 47 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/t1_tesselate.m: -------------------------------------------------------------------------------- 1 | function track = t1_tesselate(checkpoints) 2 | % tesselate track from discrete checkpoints to polygons 3 | track = struct; 4 | 5 | % Interlace left and right track boundary points 6 | track.vertices = nan(2, 2*length(checkpoints)); 7 | track.vertices(:, 1:2:end) = [checkpoints.left]; 8 | track.vertices(:, 2:2:end) = [checkpoints.right]; 9 | 10 | % Generate initial set of polygons (quadrilaterals) that cover the track 11 | for i = 1:length(checkpoints) 12 | track.polygons(i).vertex_indices = utils.mod1(2*i + (-1:2), size(track.vertices, 2)); 13 | end 14 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/t2_merge.m: -------------------------------------------------------------------------------- 1 | function [track, epsilon_area_tolerance_scaled] = t2_merge(track, track_scale, epsilon_area_tolerance) 2 | % merge polygons to convex regions 3 | % 4 | % Inputs 5 | % track 6 | % track_scale [m] 7 | % epsilon_area_tolerance [m^2] 8 | 9 | % iterate in reverse: deleting polygons in the loop 10 | for i = length(track.polygons):-1:2 11 | % construct polygon indices 12 | idx_1 = track.polygons(i).vertex_indices; 13 | idx_2 = track.polygons(i-1).vertex_indices; 14 | % union indices of adjacent polygons 15 | idx_union = union(idx_1, idx_2); 16 | 17 | % calculate hull areas & indices 18 | [~, area_1] = convhull(track.vertices(:, idx_1)'); 19 | [~, area_2] = convhull(track.vertices(:, idx_2)'); 20 | % simplify: only keep hull vertices 21 | [indices_hull, area_merged] = convhull(track.vertices(:, idx_union)', 'simplify', true); 22 | 23 | % merge polygons if their union is (almost) convex (almost same area) 24 | epsilon_area_tolerance_scaled = epsilon_area_tolerance * (track_scale^2); 25 | if area_merged - (area_1 + area_2) <= epsilon_area_tolerance_scaled 26 | % delete polygon 1 27 | track.polygons(i) = []; 28 | % replace polygon 2 with union (first and last entry of hull is duplicated) 29 | track.polygons(i-1).vertex_indices = idx_union(indices_hull(2:end)); 30 | end 31 | end 32 | end -------------------------------------------------------------------------------- /code/+controller/+track_SCR/t3_overlap.m: -------------------------------------------------------------------------------- 1 | function track_new = t3_overlap(checkpoints, track, track_scale, direction_is_forward, track_overlapped) 2 | % Inputs 3 | % checkpoints: just for plotting 4 | % track 5 | % track_overlapped: in case one direction was run already, contains 6 | % overlapped track for merging of forward and backward overlaps. 7 | % if first run, should equal arg `track` 8 | % track_scale [m] 9 | 10 | % convert to constraints 11 | for i = 1:length(track.polygons) 12 | [track.polygons(i).A, track.polygons(i).b] = utils.poly.vert2con(track.vertices(:,track.polygons(i).vertex_indices)'); 13 | end 14 | 15 | track_new = struct; 16 | track_new.vertices = nan(2,0); 17 | 18 | 19 | debug_ = false; 20 | if debug_ 21 | figure(997) %#ok<*UNRCH> % for whole file ok 22 | clf 23 | hold on 24 | plots.Race.plot_track(checkpoints); 25 | end 26 | 27 | % find neighbor intersections 28 | for i_p = 1:length(track.polygons) 29 | %% find shared vertices of directly neighboured polygons 30 | % leveraging track definition: each polygon has exactly two neighbours 31 | % with exactly on full shared edge each (equaling exactly two vertices 32 | % each) 33 | % 34 | % NOTE: following sections (`%%`) will reference documentation from 35 | % file '/algorithm_expand_convex_polygon.pdf' 36 | % 37 | % Subscripts 38 | % p: current polygon to check 39 | % f: next forward polygon neighbour 40 | % b: next backward polygon neighbour 41 | 42 | %% (0) get current polygon 43 | p = utils.poly.vert2poly(utils.poly.get_track_polygon_vertices(i_p, track)); 44 | if debug_; plot(p); end 45 | %% (1) enlarge current polygon 46 | p_enlarged = utils.poly.vert2poly(utils.poly.enlarge_polygon_into_forward_direction(i_p, track, track_scale, direction_is_forward, debug_)); 47 | if debug_; plot(p_enlarged); end 48 | 49 | %% (2) get next polygon index 50 | if direction_is_forward 51 | i_f = utils.mod1(i_p + 1, length(track.polygons)); 52 | else 53 | i_f = utils.mod1(i_p - 1, length(track.polygons)); 54 | end 55 | %% (3) repeat as long as forward neighbours lay in current polygon's enlarged region 56 | while true 57 | if debug_ 58 | figure(997) 59 | clf 60 | hold on 61 | plots.Race.plot_track(checkpoints); 62 | plot(p_enlarged) 63 | end 64 | 65 | %% (3a) get next neighbour polygon 66 | p_next_neighbour = utils.poly.vert2poly(utils.poly.get_track_polygon_vertices(i_f, track)); 67 | if debug_; plot(p_next_neighbour); end 68 | 69 | %% (3b) if next forward neighbour overlaps enlarged current polygon 70 | if p_enlarged.overlaps(p_next_neighbour) 71 | %% (3b1) enlarge next forward neighbour 72 | p_next_neighbour_enlarged = utils.poly.vert2poly(utils.poly.enlarge_polygon_into_forward_direction(i_f, track, track_scale, direction_is_forward, debug_)); 73 | if debug_; plot(p_next_neighbour_enlarged); end 74 | 75 | %% (3b2) get overlap between current enlarged polygon and enlarged 76 | % neighbour 77 | p_overlap_enlarged_n_next_neighbour = p_enlarged.intersect(p_next_neighbour_enlarged); 78 | if debug_; plot(p_overlap_enlarged_n_next_neighbour); end 79 | 80 | %% (3b3) divide current enlarged polygon by overlap with next 81 | % neighbour 82 | % yields a polygon with two separated regions: 83 | % on before overlap (backwards), one after (forwards) 84 | % get p_enlarged's center point 85 | [p_overlap_enlarged_n_next_neighbour_center(1), p_overlap_enlarged_n_next_neighbour_center(2)] = p_overlap_enlarged_n_next_neighbour.centroid; 86 | % scale down w.r.t. center 87 | % scaling down enlarged so that only track width is 88 | % reduced minimallistically (as enlarged and next 89 | % neighbour overlap --> scaling has no effect here) 90 | p_overlap_enlarged_n_next_neighbour_upscaled = p_overlap_enlarged_n_next_neighbour.scale(1 + 1e-3, p_overlap_enlarged_n_next_neighbour_center); 91 | if debug_; plot(p_overlap_enlarged_n_next_neighbour_upscaled); end 92 | 93 | % re-try to divide enlarged area as before 94 | p_enlarged_divided_by_neighbour_overlap = p_enlarged.subtract(p_overlap_enlarged_n_next_neighbour_upscaled); 95 | if debug_; plot(p_enlarged_divided_by_neighbour_overlap); end 96 | %if debug_; scatter(p_enlarged_divided_by_neighbour_overlap.Vertices(:, 1), p_enlarged_divided_by_neighbour_overlap.Vertices(:, 2)); end 97 | 98 | if p_enlarged_divided_by_neighbour_overlap.NumRegions == 1 99 | % edge case: neighbour doesn't divide p_enlarged, 100 | % because it's only sticking into p_enlarged but not 101 | % laying inside 102 | p_enlarged_retain = p_enlarged_divided_by_neighbour_overlap; 103 | else 104 | % case 3 regions can happen soldomly, if neighbour 105 | % restricts track like a triangle but has same normal 106 | % as the current polygon 107 | % --> especially at joint point (when track circle is 108 | % close) when running in backward direction 109 | % 110 | % integrated special exclusion (so that we assert as 111 | % tight as possible, detecting possible errors as early 112 | % as possible) 113 | if p_enlarged_divided_by_neighbour_overlap.NumRegions == 3 && i_p == 1 && i_f == length(track.polygons) 114 | warning('Ignoring set division issue at track joint (where circle is closed). Caused by slight misalignments from track creation') 115 | else 116 | assert(p_enlarged_divided_by_neighbour_overlap.NumRegions == 2, 'Numerical issues? Please investigate!') 117 | end 118 | 119 | 120 | %% (3b4) check which polygon is part of original polygon (= overlaps) 121 | % should be the first polygon due to track definition 122 | tf_p_part_of_original = overlaps([p; p_enlarged_divided_by_neighbour_overlap.regions]); 123 | 124 | % choose original overlapping polygon 125 | % we want: polygon 2 and 3 (columns) relation to first polygon (row) 126 | p_divided_selection_index = tf_p_part_of_original(1, 2:3); 127 | temp = p_enlarged_divided_by_neighbour_overlap.regions; 128 | p_enlarged_retain = temp(p_divided_selection_index); 129 | end 130 | if debug_; plot(p_enlarged_retain); end 131 | 132 | %% (3b5) union: reatining old p_enlarged parts, add overlap with next neighbour 133 | [p_enlarged_retain_center(1), p_enlarged_retain_center(2)] = p_enlarged_retain.centroid; 134 | % scale down w.r.t. center 135 | % scaling down enlarged so that only track width is 136 | % reduced minimallistically (as enlarged and next 137 | % neighbour overlap --> scaling has no effect here) 138 | p_enlarged_retain_upscaled = p_enlarged_retain.scale(1 + 1e-3, p_enlarged_retain_center); 139 | 140 | p_enlarged = union(p_enlarged_retain_upscaled, p_overlap_enlarged_n_next_neighbour_upscaled); 141 | 142 | % downscale again (else we exponentially increase polygon size 143 | % over loop iterations) 144 | 145 | [p_enlarged_center(1), p_enlarged_center(2)] = p_enlarged.centroid; 146 | % scale down w.r.t. center 147 | % scaling down enlarged so that only track width is 148 | % reduced minimallistically (as enlarged and next 149 | % neighbour overlap --> scaling has no effect here) 150 | p_enlarged = p_enlarged.scale(1 - 1e-3, p_enlarged_center); 151 | 152 | if debug_; plot(p_enlarged, 'EdgeColor', 'r'); end 153 | %if debug_; drawnow; end 154 | %% (3c) 155 | else 156 | %% (3c1) no overlapping --> maximum convex enlargement reached 157 | break 158 | %% (3d) 159 | end 160 | 161 | %% (3e) for-loop: forward index 162 | if direction_is_forward 163 | i_f = utils.mod1(i_f + 1, length(track.polygons)); 164 | else 165 | i_f = utils.mod1(i_f - 1, length(track.polygons)); 166 | end 167 | end 168 | if debug_; plot(p_enlarged, 'EdgeColor', 'r'); end 169 | %if debug_; drawnow; pause(0.2); end 170 | 171 | %% save output 172 | track_new.polygons(i_p).vertex_indices = length(track_new.vertices) + (1:length(utils.poly.poly2vert(p_enlarged))); 173 | track_new.vertices = [track_new.vertices utils.poly.poly2vert(p_enlarged)]; 174 | [track_new.polygons(i_p).A, track_new.polygons(i_p).b] = utils.poly.vert2con(utils.poly.poly2vert(p_enlarged)'); 175 | end 176 | 177 | %% merge forward and backward overlap, if previous overlaps given 178 | if exist('track_overlapped', 'var') 179 | track_merged_overlaps = struct; 180 | track_merged_overlaps.vertices = nan(2,0); 181 | 182 | for i_p = 1:length(track.polygons) 183 | if debug_ 184 | figure(997) 185 | clf 186 | hold on 187 | plots.Race.plot_track(checkpoints); 188 | end 189 | 190 | % get overlaps in both directions 191 | p_overlap_prev = utils.poly.vert2poly(utils.poly.get_track_polygon_vertices(i_p, track_overlapped)); 192 | p_overlap_curr = utils.poly.vert2poly(utils.poly.get_track_polygon_vertices(i_p, track_new)); 193 | if debug_; plot(p_overlap_prev); plot(p_overlap_curr); end 194 | 195 | % merge overlaps 196 | p_overlap = p_overlap_prev.union(p_overlap_curr); 197 | if debug_; plot(p_overlap, 'EdgeColor', 'r'); end 198 | 199 | %% save output 200 | track_merged_overlaps.polygons(i_p).vertex_indices = length(track_merged_overlaps.vertices) + (1:length(utils.poly.poly2vert(p_overlap))); 201 | track_merged_overlaps.vertices = [track_merged_overlaps.vertices utils.poly.poly2vert(p_overlap)]; 202 | [track_merged_overlaps.polygons(i_p).A, track_merged_overlaps.polygons(i_p).b] = utils.poly.vert2con(utils.poly.poly2vert(p_overlap)'); 203 | end 204 | 205 | track_new = track_merged_overlaps; 206 | end 207 | end 208 | -------------------------------------------------------------------------------- /code/+controller/+track_SCR/t9_add_forward_direction.m: -------------------------------------------------------------------------------- 1 | function track = t9_add_forward_direction(track, checkpoints) 2 | % average the length norm forward vector (mean of x and y)? for every 3 | % polygon 4 | n_checkpoints = length(checkpoints); 5 | forward_vectors = [checkpoints.forward_vector]; 6 | for i = 1:length(track.polygons) 7 | A = track.polygons(i).A; 8 | b = track.polygons(i).b; 9 | 10 | % get all checkpoints in current polygon 11 | included_checkpoints_selector = ... 12 | all(A * [checkpoints.center] < repmat(b, 1, n_checkpoints)); 13 | 14 | % average forward vectors of included checkpoints 15 | track.polygons(i).forward_direction = ... 16 | mean(forward_vectors(:, included_checkpoints_selector), 2); 17 | end -------------------------------------------------------------------------------- /code/+controller/+track_SL/README.m: -------------------------------------------------------------------------------- 1 | % SL formulation already matches the track models, thus no further track 2 | % generation herein is required -------------------------------------------------------------------------------- /code/+controller/+track_SL/find_closest_checkpoint_index.m: -------------------------------------------------------------------------------- 1 | function [cp_index, distance] = find_closest_checkpoint_index(position, cp_center) 2 | % find closest checkpoint of track to given state (position) x 3 | % Inputs: 4 | % cp: track or subset of track 5 | % pos: position of (x, y) 6 | 7 | % expand positions to match size of checkpoints 8 | position_repeated = repmat(position, 1, length(cp_center)); 9 | 10 | % take euclidian distance (for x and y) for each center-checkpoint 11 | euclidian_distance = sum((cp_center - position_repeated).^2); 12 | 13 | % save index of closest checkpoint 14 | [distance, cp_index] = min(euclidian_distance); -------------------------------------------------------------------------------- /code/+controller/+track_SL/find_closest_checkpoint_indices.m: -------------------------------------------------------------------------------- 1 | function cp_indices = find_closest_checkpoint_indices(positions, cp_center, Hp) 2 | % find closest checkpoints of track to given states (position) x 3 | % Inputs: 4 | % cp: track or subset of track 5 | % pos: position of (x, y) 6 | % Hp: number of checkpoint to check, starting from x(1). Supply 1 if 7 | % only one checkpoint/state pair is of interest 8 | 9 | % preallocate for speed 10 | cp_indices = nan(1, Hp); 11 | 12 | for k = 1:Hp 13 | % save index of closest checkpoint 14 | [cp_indices(k), ~] = controller.track_SL.find_closest_checkpoint_index(positions(:, k), cp_center); 15 | end -------------------------------------------------------------------------------- /code/+controller/+track_SL/reduce_checkpoints.m: -------------------------------------------------------------------------------- 1 | function checkpoints = reduce_checkpoints(checkpoints, distance_min, track_creation_scale) 2 | % reduces numbber of checkpoints to have minimum given distance 3 | 4 | if distance_min > 0 5 | n_checkpoints_original = length(checkpoints); 6 | 7 | % iterate in reverse: deleting polygons in the loop 8 | for i = length(checkpoints):-1:2 9 | [~, distance_cps] = controller.track_SL.find_closest_checkpoint_index(checkpoints(i).center, checkpoints(i - 1).center); 10 | 11 | % if distance to last checkpoint is smaller than threshold 12 | if distance_cps < distance_min * track_creation_scale 13 | % delete current checkpoint 14 | checkpoints(i - 1) = []; 15 | end 16 | end 17 | 18 | fprintf('\tReduced number of track checkpoints\n\t\t from %i to %i (reduction to %i%%)\n',... 19 | n_checkpoints_original, length(checkpoints), round(length(checkpoints)/n_checkpoints_original*100)); 20 | end -------------------------------------------------------------------------------- /code/+controller/get_acceleration_ellipses.m: -------------------------------------------------------------------------------- 1 | % Calculates a tangent to the elliptical acceleration constraints. 2 | % p = parameter struct 3 | % i = index of tangent 4 | % x = [px,py,vx,vy] (previous state vector) 5 | 6 | % Resulting constraint: Ax * [px,py,vx,vy] + Au * [ax,ay] <= b 7 | function [Au, b] = get_acceleration_ellipses(modelParams_controller,p,k,x) 8 | vx = x(3); 9 | vy = x(4); 10 | v_sq = vx^2 + vy^2; 11 | v = sqrt(v_sq); 12 | 13 | v_idx = modelParams_controller.v_idx(v); 14 | ay_max = modelParams_controller.a_lateral_max(v_idx); 15 | 16 | ax_max = modelParams_controller.a_forward_max(v_idx) .* ones(size(k)); 17 | ax_max(p.acceleration_cos <= 0) = modelParams_controller.a_backward_max(v_idx); 18 | 19 | % simultaneously converting acceleration from global to vehicle reference frame 20 | % vectorized from 21 | %Au = zeros(length(i), 2); 22 | % for j = 1:length(i) 23 | % Au(j, :) = 1/sqrt(v_sq + 0.01) .* [ay_max*c(j) ax_max(j)*s(j)] * [vx vy; -vy vx]; 24 | % end 25 | Au = 1/sqrt(v_sq + 0.01) .* [... 26 | ay_max*p.acceleration_cos .* vx + ax_max.*p.acceleration_sin .* -vy... 27 | ay_max*p.acceleration_cos .* vy + ax_max.*p.acceleration_sin .* vx]; 28 | b = ax_max * ay_max; 29 | end -------------------------------------------------------------------------------- /code/+controller/run_SCP.m: -------------------------------------------------------------------------------- 1 | function controller_output = run_SCP(cfg,... 2 | vhs, obstacleTable, blockingTable, i_vehicle) 3 | % run RTI/SCP 4 | 5 | %% Ease access 6 | vh = cfg.scn.vhs{i_vehicle}; 7 | 8 | % (Fixed) initial state 9 | x_0 = vhs{i_vehicle}.x_0; % 1 stage: last cycle's x(1) 10 | 11 | % Decision variables (recover from last controller output) 12 | % warm start for solver: use states x and inputs u from last time-step 13 | X_opt = vhs{i_vehicle}.X_opt; % Hp stages: last cycle's x(2), x(3), ..., x(Hp-1), x(Hp), x(Hp) 14 | U_opt = vhs{i_vehicle}.U_opt; % Hp stages: last cycle's u(1), u(2), u(3), ..., u(Hp-1), u(Hp) 15 | 16 | % Preallocate for speed 17 | controller_output = struct( ... 18 | 'X_opt', [], ... 19 | 'U_opt', [], ... 20 | 'log_opt', [], ... 21 | 'checkpoint_indices', [], ... 22 | 'track_polygon_indices', [], ... 23 | 't_opt', num2cell(zeros(1,vh.p.SCP_iterations)) ... 24 | ); 25 | 26 | %% Optimization Iterations 27 | for i = 1:vh.p.SCP_iterations 28 | timer = tic; 29 | 30 | if vh.approximationIsSCR 31 | % For each point of the projected trajectory, find the index 32 | % of the track polygon index 33 | track_polygons = cfg.scn.track_SCR.polygons; 34 | track_polygon_indices = controller.track_SCR.find_closest_most_forward_polygon_indices(... 35 | X_opt(vh.model_controller.idx_pos, :), cfg.scn.track_SCR, vh.p.Hp); 36 | else 37 | track_polygons = NaN; 38 | track_polygon_indices = NaN; 39 | end 40 | 41 | % not necessary for SCR, but vehicle obstacles & blocking 42 | % For each point of the projected trajectory, find the index 43 | % of the euclidian-distance-closest track checkpoint 44 | checkpoint_indices = controller.track_SL.find_closest_checkpoint_indices(... 45 | X_opt(vh.model_controller.idx_pos, :), cfg.scn.track_center, vh.p.Hp); 46 | 47 | %% Formulate QP 48 | [n_vars, idx_x, idx_u, idx_slack, objective_quad, objective_lin, ... 49 | A_ineq, b_ineq, A_eq, b_eq, bound_lower, bound_upper] = ... 50 | controller.create_QP(... 51 | cfg.scn.vhs, x_0, X_opt, U_opt,... 52 | cfg.scn.track, checkpoint_indices,... 53 | track_polygons, track_polygon_indices,... 54 | i, i_vehicle,... 55 | obstacleTable, blockingTable, vhs); 56 | 57 | %% Solve QP 58 | % update states x and inputs u with optimized results 59 | [X_opt, U_opt, log_solver] =... 60 | controller.solve_QP(... 61 | cfg.env.cplex.is_available,... 62 | n_vars, idx_x, idx_u, idx_slack,... 63 | objective_quad, objective_lin,... 64 | A_ineq, b_ineq, A_eq, b_eq, bound_lower, bound_upper,... 65 | cfg.scn.track_creation_scale); 66 | 67 | %% Save Output 68 | controller_output(i).X_opt = X_opt; 69 | controller_output(i).U_opt = U_opt; 70 | controller_output(i).log_solver = log_solver; 71 | controller_output(i).checkpoint_indices = checkpoint_indices; 72 | controller_output(i).track_polygon_indices = track_polygon_indices; 73 | controller_output(i).t_opt = toc(timer); 74 | 75 | try 76 | x_prev = controller_output(i-1).X_opt; 77 | x_now = controller_output(i).X_opt; 78 | min_change = 1e-2; 79 | if norm(x_prev-x_now) 0.1 90 | warning('Lateral deviation unavoidable'); 91 | end 92 | 93 | fprintf('vehicle %i t_opt %6.0fms flag %i iter %3i slack %6.2f fval %6.1f\n',... 94 | i_vehicle, sum([controller_output.t_opt]) * 1000,... 95 | log_solver.exitflag, i, log_solver.slack,... 96 | log_solver.fval); 97 | end -------------------------------------------------------------------------------- /code/+controller/shift_prev_data.m: -------------------------------------------------------------------------------- 1 | function ws = shift_prev_data(n_vhs, ws) 2 | % Forward controller data to new time-step 3 | % advance by one step (with terminal constraints) yields: 4 | for v = 1:n_vhs 5 | % Save trajectory for convex constraint plots 6 | ws.vhs{v}.X_controller_prev = ws.vhs{v}.X_controller; 7 | 8 | % update initial state 9 | ws.vhs{v}.x_0 = ws.vhs{v}.x_0_next; 10 | 11 | % keep last state entry 12 | % X_{n-1} = X_{n} due to terminal constraint = standstill 13 | ws.vhs{v}.X_controller(:, 1:end-1) = ws.vhs{v}.X_controller(:, 2:end); 14 | 15 | % zero last input 16 | % U_{n} = zeros() due to terminal constraint = standstill 17 | ws.vhs{v}.U_controller(:, 1:end-1) = ws.vhs{v}.U_controller(:, 2:end); 18 | ws.vhs{v}.U_controller(:, end) = 0 .* ws.vhs{v}.U_controller(:, end); 19 | end -------------------------------------------------------------------------------- /code/+controller/solve_QP.m: -------------------------------------------------------------------------------- 1 | function [X_opt, U_opt, log] =... 2 | solve_QP(... 3 | isCplexAvailable, n_vars, idx_x, idx_u, idx_slack,... 4 | quad_objective, lin_objective,... 5 | A_ineq, b_ineq, A_eq, b_eq, bound_lower, bound_upper,... 6 | track_creation_scale) 7 | %% Solve QP 8 | log = struct; 9 | 10 | % use CPLEX to solve QP, if available, else fallback to MATLAB 11 | if isCplexAvailable 12 | [qp_vars,log.fval,log.exitflag,log.output,log.lambda] = ... 13 | cplexqp(quad_objective,lin_objective,A_ineq,b_ineq,A_eq,b_eq,bound_lower,bound_upper,[],[]); 14 | 15 | % Exitflag values: (acc. to 16 | % https://www.ibm.com/docs/en/icos/12.9.0?topic=functions-cplexqp, 17 | % refering to https://www.ibm.com/docs/en/icos/12.9.0?topic=functions-cplexmiqcp) 18 | 19 | % "If you want to write the model out to a file, then you need to set: 20 | % opt.exportmodel = 'myModel.lp';" 21 | switch log.exitflag 22 | % case ~=1 and >=0: CPLEX qp has problematic solution 23 | case 6; warning('CPLEX qp exitflag 6: Non-optimal Solution available.'); 24 | case 5; warning('CPLEX qp exitflag 5: Solution with numerical issues.'); 25 | case 1 %disp('CPLEX qp exitflag 1: Function converged to a solution x.'); 26 | case 0; warning('CPLEX qp exitflag 0: Number of iterations exceeded options.MaxIter.'); 27 | % case < 0: CPLEX qp has no solution 28 | case -1; warning('CPLEX qp exitflag -1: Aborted.'); 29 | case -2; warning('CPLEX qp exitflag -2: No feasible point was found.'); 30 | case -3; warning('CPLEX qp exitflag -3: Problem is unbounded.'); 31 | case -4; warning('CPLEX qp exitflag -4: NaN value was encountered during execution of the algorithm.'); 32 | case -5; warning('CPLEX qp exitflag -5: Both primal and dual problems are infeasible.'); 33 | case -7; warning('CPLEX qp exitflag -7: Search direction became too small. No further progress could be made.'); 34 | case -8; warning('CPLEX qp exitflag -8: Problem is infeasible or unbounded.'); 35 | case -9; warning('CPLEX qp exitflag -9: Limit reached.'); 36 | otherwise; warning('CPLEX qp exitflag: unknown'); 37 | end 38 | else 39 | options = optimoptions('quadprog',... 40 | 'Display', 'none',... 41 | 'StepTolerance', 1e-15,... 42 | 'OptimalityTolerance', 1e-13,... 43 | 'ConstraintTolerance', 1e-12 ... 44 | ); 45 | quad_objective_sp = sparse(quad_objective); 46 | [qp_vars,log.fval,log.exitflag,log.output,log.lambda] = ... 47 | quadprog(quad_objective_sp,lin_objective,A_ineq,b_ineq,A_eq,b_eq,bound_lower,bound_upper,[],options); 48 | 49 | % exitflags acc. to quadprog documentation 50 | switch log.exitflag 51 | % case ~=1 and >=0: MATLAB quadprog has problematic solution 52 | case 1 %disp('MATLAB quadprog exitflag 1: Function converged to the solution x.'); 53 | case 0; warning('MATLAB quadprog exitflag 0: Number of iterations exceeded options.MaxIterations.'); 54 | % case < 0: MATLAB quadprog has no solution 55 | case -2; warning('MATLAB quadprog exitflag -2: Problem is infeasible. Or, for "interior-point-convex", the step size was smaller than options.StepTolerance, but constraints were not satisfied.'); 56 | case -3; warning('MATLAB quadprog exitflag -3: Problem is unbounded.'); 57 | % "'interior-point-convex' Algorithm"-specific exit flags (default algorithm) 58 | case 2; warning('MATLAB quadprog exitflag 2: Step size was smaller than options.StepTolerance, constraints were satisfied.'); 59 | case -6; warning('MATLAB quadprog exitflag -6: Nonconvex problem detected.'); 60 | case -8; warning('MATLAB quadprog exitflag -8: Unable to compute a step direction.'); 61 | otherwise; warning('MATLAB quadprog exitflag: unknown'); 62 | end 63 | end 64 | 65 | if (length(qp_vars) == n_vars) && isnan(qp_vars(1)) 66 | error('Solver exceeds time limit!\n'); 67 | elseif length(qp_vars) ~= n_vars 68 | % options = cplexoptimset('ExportModel', 'failedProblem.lp'); 69 | % [qp_vars,optimization_log.fval,optimization_log.exitflag,optimization_log.output,optimization_log.lambda] = ... 70 | % quadprog(quad_objective_sp,lin_objective,A_ineq,b_ineq,A_eq,b_eq,bound_lower,bound_upper,[],options); 71 | error('Solver failed!'); 72 | end 73 | 74 | % save results 75 | X_opt = qp_vars(idx_x)'; 76 | U_opt = qp_vars(idx_u)'; 77 | log.slack = qp_vars(idx_slack); 78 | 79 | if log.slack > 4.e-9 * track_creation_scale 80 | warning('Slackened track constraints with slack = %.2e!', log.slack) 81 | end 82 | end -------------------------------------------------------------------------------- /code/+evaluation/lap_time.m: -------------------------------------------------------------------------------- 1 | function sim_result = lap_time(has_changed) 2 | if (nargin==0) 3 | has_changed=false; 4 | end 5 | scenarios = run(true); 6 | scenario = scenarios(22); % race 7 | scenario.outputPath = '../results/t_lap/'; 8 | output_file = [scenario.outputPath, 'log.mat']; 9 | if (has_changed) || (~isfile(output_file)) 10 | % scenario = scenarios(4); % scr 11 | % scenario = scenarios(10); % sl 12 | n_laps = 2; 13 | scenario = config.scenario_n_laps_race(scenario, n_laps); 14 | 15 | % iterations for SL 16 | assert(scenario.scn.vhs{1}.approximation == scenario.scn.vhs{1}.approximationSL); 17 | scenario.scn.vhs{1}.p.SCP_iterations = 1; 18 | % trust region 19 | if (scenario.env.cplex.is_available) 20 | scenario.scn.vhs{1}.p.trust_region_size = 1.6; 21 | else 22 | scenario.scn.vhs{1}.p.trust_region_size = 0.8; 23 | end 24 | 25 | 26 | output_file = sim.run(scenario); 27 | end 28 | 29 | sim_result = load(output_file); 30 | 31 | n_vehicles = numel(sim_result.cfg.scn.vhs); 32 | n_laps = sim_result.cfg.race.n_laps; 33 | t_lap = zeros(n_laps, n_vehicles); 34 | 35 | fname_t_lap = [sim_result.cfg.outputPath, 't_lap.txt']; 36 | delete(fname_t_lap); 37 | fid = fopen(fname_t_lap, 'a'); 38 | for i_vehicle = 1:n_vehicles 39 | % Lap Time 40 | if scenario.scn.vhs{i_vehicle}.approximation == scenario.scn.vhs{i_vehicle}.approximationSL 41 | convexification_method = "SL"; 42 | else 43 | convexification_method = "SCR"; 44 | end 45 | for i_lap = 1:n_laps 46 | correcting_first_lap = i_lap==1; 47 | t_lap(i_lap,i_vehicle) = (... 48 | sum([sim_result.log.vehicles{i_vehicle}.lap_count] == i_lap-1) ... 49 | - correcting_first_lap ... 50 | ) * sim_result.cfg.scn.vhs{i_vehicle}.p.dt_controller; 51 | fprintf(fid, "Lap %i using %3s, t_lap: %f\n", ... 52 | i_lap, convexification_method, t_lap(i_lap,i_vehicle)... 53 | ); 54 | fprintf("Lap %i using %3s, t_lap: %f\n", ... 55 | i_lap, convexification_method, t_lap(i_lap,i_vehicle)... 56 | ); 57 | end 58 | end 59 | fclose(fid); 60 | end -------------------------------------------------------------------------------- /code/+evaluation/lin_vs_st.m: -------------------------------------------------------------------------------- 1 | function sim_result = lin_vs_st(has_changed) 2 | 3 | if (nargin==0) 4 | has_changed=false; 5 | end 6 | scenarios = run(true); 7 | scenario = scenarios(30); 8 | scenario.outputPath = '../results/lin_vs_st/'; 9 | output_file = [scenario.outputPath, 'log.mat']; 10 | if (has_changed) || (~isfile(output_file)) 11 | if (scenario.env.cplex.is_available) 12 | scenario.scn.vhs{1}.p.trust_region_size = 1.6; 13 | else 14 | scenario.scn.vhs{1}.p.trust_region_size = 0.8; 15 | end 16 | output_file = sim.run(scenario); 17 | end 18 | 19 | sim_result = load(output_file); 20 | %% Visualization setup 21 | % recreate figure handles 22 | sim_result.cfg.plot.plots_to_draw = config.config().plot.plots_to_draw; 23 | % re-init config (so that objects & figure handles are present again) 24 | sim_result.cfg = config.init_config(sim_result.cfg); 25 | sim_result.cfg.plot.grayscale = true; 26 | 27 | %% Visualization 28 | disp('run plotting') 29 | j = 71; 30 | % reconstruct ws from log 31 | ws = sim_result.log.lap{j}; 32 | for i = 1:length(sim_result.cfg.scn.vhs) 33 | ws.vhs{i} = sim_result.log.vehicles{i}(j); 34 | end 35 | 36 | fig = figure(202); 37 | clf; 38 | plots.Race(202).plot_track(sim_result.cfg.scn.track); 39 | hold on 40 | n_vehicles = numel(sim_result.cfg.scn.vhs); 41 | c = [0.1; 0.4] .* [1 1 1]; 42 | for i_vehicle = 1:n_vehicles 43 | %% Planned trajectory 44 | plot(ws.vhs{i_vehicle}.X_controller(1,:) ... 45 | ,ws.vhs{i_vehicle}.X_controller(2,:) ... 46 | ,'.-','color',c(i_vehicle, :),'MarkerSize',7 ... 47 | ); 48 | 49 | %% Vehicle Box 50 | vehLength = sim_result.cfg.scn.vhs{i_vehicle}.lengthVal; 51 | vehWidth = sim_result.cfg.scn.vhs{i_vehicle}.widthVal; 52 | if length(ws.vhs{i_vehicle}.x_0_controller) <= 4 % if vehicle control model is linear 53 | % Vehicle box (is old version: get vehicle 54 | % direction from current vehicle velocity vector) 55 | if ws.vhs{i_vehicle}.x_0_controller(3:4) ~= [0;0] 56 | dist = ws.vhs{i_vehicle}.x_0_controller(3:4); 57 | else 58 | dist = [1;0]; 59 | end 60 | dist = dist / norm(dist); 61 | else 62 | dist = [cos(ws.vhs{i_vehicle}.x_0_controller(5));sin(ws.vhs{i_vehicle}.x_0_controller(5))]; % yaw angle 63 | end 64 | R = [dist [-dist(2); dist(1)]]; 65 | vehicleRectangle = R * [vehLength/2 0;0 vehWidth/2] * [1 1 -1 -1;1 -1 -1 1] + repmat(ws.vhs{i_vehicle}.x_0_controller(1:2),1,4); 66 | fill(vehicleRectangle(1,:),vehicleRectangle(2,:),c(i_vehicle, :)); 67 | end 68 | 69 | % i_race = 1; 70 | % sim_result.cfg.plot.plots_to_draw{i_race}.plot(sim_result.cfg, ws); 71 | % fig = sim_result.cfg.plot.plots_to_draw{i_race}.figure_handle; 72 | 73 | legend('','','','','Single-Track Model','','Linear Model','','Location','southeast') 74 | xlim([-1.3 2.2]); 75 | ylim([-3.5 -0.8]); 76 | utils.set_figure_properties(fig,'paper',6); 77 | exportgraphics(fig, [sim_result.cfg.outputPath 'lin_vs_st.pdf'], 'ContentType','vector'); -------------------------------------------------------------------------------- /code/+evaluation/paper.m: -------------------------------------------------------------------------------- 1 | % Race with lap times, computation time 2 | evaluation.lap_time(); 3 | 4 | % linear vs ST model 5 | evaluation.lin_vs_st(); 6 | 7 | % SCR vs SL, infeasible nonconvex SL solution 8 | evaluation.sl_vs_scr(); 9 | 10 | % SCR vs SL race lines 11 | evaluation.race_line(); -------------------------------------------------------------------------------- /code/+evaluation/plot_t_opts.m: -------------------------------------------------------------------------------- 1 | %% Plot simulation times of different vehicles 2 | %% How to 3 | % 1. load a "log.mat" from the output directory into workspace 4 | % 2. run script 5 | 6 | %% extract t_opts 7 | t_opts = cell(1, length(log.vehicles)); 8 | 9 | % for every vehicle 10 | for v = 1:length(log.vehicles) 11 | 12 | % NOTE: yields p.iterations * simulation_steps outputs 13 | controller_output = [log.vehicles{v}.controller_output]; 14 | 15 | % squash into shape: 16 | % columns: controller iteration 17 | % rows: SCP iterations inside controller iteration 18 | t_opts{v} = reshape([controller_output.t_opt], [], length(log.vehicles{v})); 19 | end 20 | 21 | %% Statistics 22 | stat = struct; 23 | for v = 1:length(t_opts) 24 | % sum SCP iterations 25 | t_opt_per_iteration = sum(t_opts{v}, 1); 26 | 27 | % calc staistics 28 | stat(v).median = median(t_opt_per_iteration); 29 | % if 'Statistics and Machine Learning Toolbox' available 30 | if utils.isToolboxAvailable('Statistics and Machine Learning Toolbox') 31 | stat(v).prctile_99 = prctile(t_opt_per_iteration, 99); 32 | else 33 | warning("'Statistics and Machine Learning Toolbox' is not installed, thus not calculating percentile"); 34 | stat(v).prctile_99 = 0; 35 | end 36 | stat(v).max = max(t_opt_per_iteration); 37 | end 38 | 39 | %% Plot 40 | figure_handle_number = 1010; 41 | f = figure(figure_handle_number); 42 | clf(f) 43 | set(f, 'Name', 'Optimization Time Statistics'); 44 | 45 | % vehicle (categories) 46 | cats = cell(1, length(stat)); 47 | for v = 1:length(cats) 48 | cats{v} = sprintf('Vehicle %i', v); 49 | end 50 | cats = categorical(cats); 51 | 52 | if verLessThan('matlab', '9.7') 53 | warning('MATLAB version too old, not plotting statistics. Instead printed:') 54 | disp(cats); 55 | disp(stat.median); 56 | disp(stat.prctile_99); 57 | disp(stat.max); 58 | else 59 | bar(cats, 1000 .* [[stat.median]' [stat.prctile_99]' [stat.max]']); 60 | ylabel('Solver Time [ms]') 61 | legend('Median','99th Percentile','Maximum', 'location', 'northwest') 62 | end 63 | 64 | %% Display vehicle config 65 | for v = 1:length(cfg.scn.vhs) 66 | fprintf('Vehicle %i\n', v); 67 | disp(cfg.scn.vhs{v}) 68 | end -------------------------------------------------------------------------------- /code/+evaluation/plot_track_with_speed.m: -------------------------------------------------------------------------------- 1 | %% Plot simulation times of different vehicles 2 | %% How to 3 | % 1. load a "log.mat" from the output directory into workspace 4 | % 2. run script 5 | 6 | colors = utils.getRwthColors(100); 7 | 8 | for vel_index = [1 2 3] % [v v_long v_lat] 9 | %% Plot track 10 | if vel_index == 1 11 | figure_handle_number = 1020; 12 | elseif vel_index == 2 13 | figure_handle_number = 1021; 14 | else 15 | figure_handle_number = 1022; 16 | end 17 | f = figure(figure_handle_number); 18 | clf(f) 19 | plots.Race(figure_handle_number).plot_track(cfg.scn.track); 20 | hold on 21 | if vel_index == 1 22 | set(figure_handle_number, 'Name', "Vehicle 1's Velocity v on Track"); 23 | elseif vel_index == 2 24 | set(figure_handle_number, 'Name', "Vehicle 1's Longitudinal Velocity v_{long} on Track"); 25 | else 26 | set(figure_handle_number, 'Name', "Vehicle 1's Lateral Velocity v_{lat} on Track"); 27 | end 28 | 29 | %% Plot Driven Paths 30 | %% (1) one Vehicle 31 | x_0 = [log.vehicles{1}.x_0]; 32 | X = x_0(1, :); 33 | Y = x_0(2, :); 34 | velX = x_0(3, :); 35 | velY = x_0(4, :); 36 | vel = sqrt(velX.^2 + velY.^2); 37 | 38 | if vel_index == 1 39 | CData = [vel; vel]; 40 | elseif vel_index == 2 41 | CData = [velX; velX]; 42 | else 43 | CData = [velY; velY]; 44 | end 45 | 46 | plot(X, Y, 'Color', colors(1,:), 'LineWidth', 2) 47 | 48 | % velocity-dependent coloring, inlcuding legend 49 | surface('XData', [X; X], ... % N.B. XYZC Data must have at least 2 cols 50 | 'YData', [Y; Y], ... 51 | 'ZData', [zeros(size(X)); zeros(size(X))], ... 52 | 'CData', CData, ... 53 | 'FaceColor', 'none', ... 54 | 'EdgeColor', 'interp', ... 55 | 'Marker', 'none',... 56 | 'LineWidth', 2); 57 | colormap(jet(64)) 58 | %caxis([0 3]); % set limit 59 | c = colorbar; 60 | c.TickLabelInterpreter = 'latex'; 61 | c.FontSize = 11; 62 | c.Label.String = '[m/s]'; 63 | c.Label.Interpreter = 'latex'; 64 | c.Label.FontSize = 11; 65 | 66 | 67 | %% (4) two Vehicles no Block (velX-veh2:175, step 137) 68 | % scenario = twoVehScenario(); 69 | % 70 | % nbStates2 = length(log.vehicles{1,2}.lapLog{1,1}) + 1; 71 | % X2 = nan(1,nbStates2); 72 | % Y2 = nan(1,nbStates2); 73 | % for i = 1:nbStates2 74 | % X2(i) = log.vehicles{1,2}.currLapLog(i).x_0(1); 75 | % Y2(i) = log.vehicles{1,2}.currLapLog(i).x_0(2); 76 | % end 77 | % plot(X2,Y2,'Color', color100(2,:),'LineWidth', 1) 78 | % hold on 79 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,2}.currLapLog(137).x_0, log.vehicles{1,2}.currLapLog(137).U_opt(1), color100(2,:)) 80 | % hold on 81 | % 82 | % nbStates1 = length(log.vehicles{1,1}.lapLog{1,1}) + 1; 83 | % X1 = nan(1,nbStates1); 84 | % Y1 = nan(1,nbStates1); 85 | % for i = 1:nbStates1 86 | % X1(i) = log.vehicles{1,1}.currLapLog(i).x_0(1); 87 | % Y1(i) = log.vehicles{1,1}.currLapLog(i).x_0(2); 88 | % end 89 | % plot(X1,Y1,'Color', color100(5,:),'LineWidth', 1) 90 | % hold on 91 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,1}.currLapLog(137).x_0, log.vehicles{1,1}.currLapLog(137).U_opt(1), color100(5,:)) 92 | 93 | 94 | %% (5) two Vehicles with Block (velX-veh2:175, step 137 + 165) 95 | % scenario = twoVehScenario(); 96 | % 97 | % nbStates2 = length(log.vehicles{1,2}.lapLog{1,1}) + 1; 98 | % X2 = nan(1,nbStates2); 99 | % Y2 = nan(1,nbStates2); 100 | % for i = 1:nbStates2 101 | % X2(i) = log.vehicles{1,2}.currLapLog(i).x_0(1); 102 | % Y2(i) = log.vehicles{1,2}.currLapLog(i).x_0(2); 103 | % end 104 | % plot(X2,Y2,'Color', color100(2,:),'LineWidth', 1) 105 | % hold on 106 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,2}.currLapLog(137).x_0, log.vehicles{1,2}.currLapLog(137).U_opt(1), color100(2,:)) 107 | % hold on 108 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,2}.currLapLog(165).x_0, log.vehicles{1,2}.currLapLog(165).U_opt(1), color100(2,:)) 109 | % hold on 110 | % 111 | % nbStates1 = length(log.vehicles{1,1}.lapLog{1,1}) + 1; 112 | % X1 = nan(1,nbStates1); 113 | % Y1 = nan(1,nbStates1); 114 | % for i = 1:nbStates1 115 | % X1(i) = log.vehicles{1,1}.currLapLog(i).x_0(1); 116 | % Y1(i) = log.vehicles{1,1}.currLapLog(i).x_0(2); 117 | % end 118 | % plot(X1,Y1,'Color', color100(5,:),'LineWidth', 1) 119 | % hold on 120 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,1}.currLapLog(137).x_0, log.vehicles{1,1}.currLapLog(137).U_opt(1), color100(5,:)) 121 | % hold on 122 | % plot_vehicle(scenario.vehicles{1,1}, log.vehicles{1,1}.currLapLog(165).x_0, log.vehicles{1,1}.currLapLog(165).U_opt(1), color100(5,:)) 123 | 124 | 125 | %% (7) one-four-eight Vehicles 126 | % nbStates1 = length(log1.vehicles{1,1}.lapLog{1,1}) + 1; 127 | % X1 = nan(1,nbStates1); 128 | % Y1 = nan(1,nbStates1); 129 | % vel1 = nan(1,nbStates1); 130 | % for i = 1:nbStates1 131 | % X1(i) = log1.vehicles{1,1}.currLapLog(i).x_0(1); 132 | % Y1(i) = log1.vehicles{1,1}.currLapLog(i).x_0(2); 133 | % vel1Xi = log1.vehicles{1,1}.currLapLog(i).x_0(3); 134 | % vel1Yi = log1.vehicles{1,1}.currLapLog(i).x_0(4); 135 | % vel1(i) = sqrt(vel1Xi^2 + vel1Yi^2); 136 | % end 137 | % surface('XData', [X1; X1], ... % N.B. XYZC Data must have at least 2 cols 138 | % 'YData', [Y1; Y1], ... 139 | % 'ZData', [zeros(size(X1));zeros(size(X1))], ... 140 | % 'CData', [vel1; vel1], ... 141 | % 'FaceColor', 'none', ... 142 | % 'EdgeColor', 'interp', ... 143 | % 'Marker', 'none',... 144 | % 'LineWidth',1.5); 145 | % 146 | % nbStates2 = length(log2.vehicles{1,1}.lapLog{1,1}) + 1; 147 | % X2 = nan(1,nbStates2); 148 | % Y2 = nan(1,nbStates2); 149 | % vel2 = nan(1,nbStates2); 150 | % for i = 1:nbStates2 151 | % X2(i) = log2.vehicles{1,1}.currLapLog(i).x_0(1); 152 | % Y2(i) = log2.vehicles{1,1}.currLapLog(i).x_0(2); 153 | % vel2Xi = log2.vehicles{1,1}.currLapLog(i).x_0(3); 154 | % vel2Yi = log2.vehicles{1,1}.currLapLog(i).x_0(4); 155 | % vel2(i) = sqrt(vel2Xi^2 + vel2Yi^2); 156 | % end 157 | % surface('XData', [X2; X2], ... % N.B. XYZC Data must have at least 2 cols 158 | % 'YData', [Y2; Y2], ... 159 | % 'ZData', [zeros(size(X2));zeros(size(X2))], ... 160 | % 'CData', [vel2; vel2], ... 161 | % 'FaceColor', 'none', ... 162 | % 'EdgeColor', 'interp', ... 163 | % 'Marker', 'none',... 164 | % 'LineWidth',1.5); 165 | % 166 | % colormap(jet(64)) 167 | % caxis([0 3]); 168 | % c = colorbar; 169 | % c.TickLabelInterpreter = 'latex'; 170 | % c.FontSize = 11; 171 | % c.Label.String = '[m/s]'; 172 | % c.Label.Interpreter = 'latex'; 173 | % c.Label.FontSize = 11; 174 | 175 | 176 | hold off 177 | end -------------------------------------------------------------------------------- /code/+evaluation/race_line.m: -------------------------------------------------------------------------------- 1 | function sim_result = race_line(has_changed) 2 | if (nargin==0) 3 | has_changed=false; 4 | end 5 | scenarios = run(true); 6 | scenario = scenarios(23); % race 7 | scenario.outputPath = '../results/race_line/'; 8 | output_file = [scenario.outputPath, 'log.mat']; 9 | if (has_changed) || (~isfile(output_file)) 10 | % scenario = scenarios(4); % scr 11 | % scenario = scenarios(10); % sl 12 | n_laps = 1; 13 | scenario = config.scenario_n_laps_race(scenario, n_laps); 14 | scenario = config.scenario_flying_start(scenario); 15 | 16 | % iterations for SL 17 | assert(scenario.scn.vhs{2}.approximation == scenario.scn.vhs{2}.approximationSL); 18 | scenario.scn.vhs{2}.p.SCP_iterations = 1; 19 | % trust region 20 | if (scenario.env.cplex.is_available) 21 | scenario.scn.vhs{2}.p.trust_region_size = 1.6; 22 | else 23 | scenario.scn.vhs{2}.p.trust_region_size = 0.6; 24 | end 25 | 26 | output_file = sim.run(scenario); 27 | end 28 | sim_result = load(output_file); 29 | n_vehicles = numel(sim_result.cfg.scn.vhs); 30 | 31 | c = [0.1; 0.4] .* [1 1 1]; 32 | l = ["-", "--"]; 33 | m = [".",".","+", "x"]; 34 | 35 | fig = figure(200); 36 | clf; 37 | plots.Race(200).plot_track(sim_result.cfg.scn.track); 38 | hold on 39 | for i_vehicle = 1:n_vehicles 40 | % Path 41 | indices_lap_2 = [sim_result.log.vehicles{i_vehicle}.lap_count] == 0; 42 | % assumes 10 steps per simulation 43 | n_steps_sim = 10; 44 | assert(size(sim_result.log.vehicles{i_vehicle}(1).x_sim,2),n_steps_sim); 45 | indices_lap_2_sim = logical(kron(indices_lap_2, ones(1,n_steps_sim))); 46 | % i_last_1 = find([sim_result.log.vehicles{i_vehicle}.lap_count] == 0,1, 'last'); 47 | % indices_lap_2(i_last_1) = 1; 48 | x_all = horzcat(sim_result.log.vehicles{i_vehicle}.x_sim); 49 | x = x_all(:,indices_lap_2_sim); 50 | X = x(1, :); 51 | Y = x(2, :); 52 | plot(X, Y, ... 53 | 'Color', c(i_vehicle,:), ... 54 | 'LineWidth', 0.5, ... 55 | 'LineStyle', l(i_vehicle), ... 56 | 'Marker', m(i_vehicle) ... 57 | , 'MarkerIndices',1:10*n_steps_sim:length(Y) ... 58 | , 'MarkerSize', 7 ... 59 | ); 60 | end 61 | legend('','','','','SCR','SL'); 62 | xlim([-1.3 5.8]) 63 | ylim([-4.5 0.3]) 64 | hold off 65 | 66 | fig.CurrentAxes.Legend.Location = 'southeast'; 67 | utils.set_figure_properties(fig,'paper',6); 68 | exportgraphics(fig, [sim_result.cfg.outputPath '/race_line.pdf'], 'ContentType','vector'); 69 | end -------------------------------------------------------------------------------- /code/+evaluation/sl_vs_scr.m: -------------------------------------------------------------------------------- 1 | function sim_result = sl_vs_scr(has_changed) 2 | 3 | if (nargin==0) 4 | has_changed=false; 5 | end 6 | scenarios = run(true); 7 | scenario = scenarios(31); 8 | scenario.outputPath = '../results/sl_vs_scr/'; 9 | output_file = [scenario.outputPath, 'log.mat']; 10 | if (has_changed) || (~isfile(output_file)) 11 | output_file = sim.run(scenario); 12 | end 13 | 14 | sim_result = load(output_file); 15 | %% Visualization setup 16 | % recreate figure handles 17 | sim_result.cfg.plot.plots_to_draw = config.config().plot.plots_to_draw; 18 | % re-init config (so that objects & figure handles are present again) 19 | sim_result.cfg = config.init_config(sim_result.cfg); 20 | sim_result.cfg.plot.grayscale = true; 21 | 22 | %% Visualization 23 | disp('run plotting') 24 | j = 5; 25 | % reconstruct ws from log 26 | ws = sim_result.log.lap{j}; 27 | for i = 1:length(sim_result.cfg.scn.vhs) 28 | ws.vhs{i} = sim_result.log.vehicles{i}(j); 29 | end 30 | 31 | i_race = 1; 32 | sim_result.cfg.plot.plots_to_draw{i_race}.plot(sim_result.cfg, ws); 33 | 34 | fig = sim_result.cfg.plot.plots_to_draw{i_race}.figure_handle; 35 | 36 | legend('','','','','','','SCR','','','','SL','','','','','Location','northwest') 37 | xlim([3.6 5.9]); 38 | ylim([-3.2 -1.9]); 39 | utils.set_figure_properties(fig,'paper',4); 40 | exportgraphics(fig, [sim_result.cfg.outputPath 'slvsscr.pdf'], 'ContentType','vector'); 41 | 42 | 43 | % legend: v1,v2,track,track bound l/r,track bound l/r,track 44 | % start/stop,trajectory 1,previous trajectory 1,vehicle 1, convex 45 | % approximation 1, trajctory 2, previous trajectory 2, vehicle 2, SL l/r, 46 | % SL l/r -------------------------------------------------------------------------------- /code/+model/+track/HockenheimGrandPrix.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/code/+model/+track/HockenheimGrandPrix.m -------------------------------------------------------------------------------- /code/+model/+track/HockenheimShort.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = HockenheimShort 2 | % The layout of the Grand Prix Circuit of Hockenheim, Germany, has been 3 | % used as an inspiration for this layout. The track is designed for 4 | % races of model scale cars of scale 1:43. However, the track layout 5 | % has a scale of about 1:125. This makes it more difficult for the cars 6 | % to maneuver. 7 | % 8 | % FIXME which scale? trackWidth is the same as other HockenheimGP, but 9 | % states other scale 10 | creation_scale = 1/43; 11 | 12 | trackWidth = 0.25; 13 | 14 | checkpoints = struct; 15 | checkpoints.left = [0; trackWidth/2]; 16 | checkpoints.right = [0; -trackWidth/2]; 17 | checkpoints.center = [0; 0]; 18 | checkpoints.yaw = 0; 19 | checkpoints.forward_vector = [1; 0]; 20 | checkpoints.ds = 0; 21 | 22 | % section 1 23 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 4.4, trackWidth); % 3.9834 24 | checkpoints = model.track.add_turn_N30(checkpoints, -75/360, 75/360 * 0.9702 * pi, trackWidth); % D = 0.9702; alpha = 90 25 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 2.01, trackWidth); % 1.8962 26 | 27 | % section 2 28 | checkpoints = model.track.add_turn_N30(checkpoints, -105/360, 105/360 * 0.5958 * pi, trackWidth); % D = 0.5958; alpha = 105 29 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.2943, trackWidth); % 0.2943 30 | checkpoints = model.track.add_turn_N30(checkpoints, -30/360, 30/360 * 0.5958 * pi, trackWidth); % D = 0.5958; alpha = 30 31 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.6381, trackWidth); % 0.6381 32 | checkpoints = model.track.add_turn_N30(checkpoints, 45/360, 45/360 * 1.3228 * pi, trackWidth); % D = 1.3228; alpha = 30 33 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 1.9571, trackWidth); % 1.9571 34 | checkpoints = model.track.add_turn_N30(checkpoints, 45/360, 45/360 * 2.2246 * pi, trackWidth); % D = 2.2246; alpha = 45 35 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5332, trackWidth); % 0.5332 36 | checkpoints = model.track.add_turn_N30(checkpoints, -30/360, 30/360 * 1.5899 * pi, trackWidth); % D = 1.5899; alpha = 30 37 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.2704, trackWidth); % 0.2704 38 | checkpoints = model.track.add_turn_N30(checkpoints, -105/360, 105/360 * 0.7923 * pi, trackWidth); % D = 0.7923; alpha = 105 39 | 40 | % section 3 41 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.8, trackWidth); % 0.4577 42 | checkpoints = model.track.add_turn_N30(checkpoints, -90/360, 90/360 * 1.4027 * pi, trackWidth); % D = 1.4027; alpha = 75 43 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 1.7915, trackWidth); % 1.7915 44 | checkpoints = model.track.add_turn_N30(checkpoints, 135/360, 135/360 * 0.6866 * pi, trackWidth); % D = 0.6866; alpha = 135 45 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.6, trackWidth); % 0.7061 46 | checkpoints = model.track.add_turn_N30(checkpoints, 75/360, 75/360 * 1 * pi, trackWidth); % D = 0.7065; alpha = 75 47 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.2146, trackWidth); % 0.2146 48 | checkpoints = model.track.add_turn_N30(checkpoints, -30/360, 30/360 * 0.8 * pi, trackWidth); % D = 0.7178; alpha = 45 49 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5829, trackWidth); % 0.5829 50 | checkpoints = model.track.add_turn_N30(checkpoints, -75/360, 75/360 * 0.7 * pi, trackWidth); % D = 0.4948; alpha = 75 51 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5439, trackWidth); % 0.5439 52 | checkpoints = model.track.add_turn_N30(checkpoints, -120/360, 105/360 * 1.1826 * pi, trackWidth); % D = 1.1826; alpha = 120 53 | 54 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5, trackWidth); 55 | 56 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 57 | end -------------------------------------------------------------------------------- /code/+model/+track/HockenheimShortCarMaker.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = HockenheimShortCarMaker(checkpoint_distance, trackWidth) 2 | % inputs e.g. 1, 7 3 | % recorded from CarMaker's Hockenheim race track (see B. Alrifaee and J. Maczijewski, “Real-time Trajectory optimization for Autonomous Vehicle Racing using Sequential Linearization,” in 2018 IEEE Intelligent Vehicles Symposium (IV), Changshu, Jun. 2018, pp. 476–483. doi: 10.1109/IVS.2018.8500634.) 4 | % scale 1:1 5 | creation_scale = 1/1; 6 | 7 | if ~exist('checkpoint_distance', 'var'); checkpoint_distance = 1; end 8 | if ~exist('trackWidth', 'var'); trackWidth = 7; end 9 | 10 | % loads and prepares reald world (?) path of Hockenheim 11 | data = load('model/track/HockenheimShortCarMaker.mat'); 12 | path = data.vehicle_path.Data(:,1:2); 13 | 14 | checkpoints = struct('left',{},'right',{},'center',{}); 15 | 16 | %% Arclength parameterization 17 | arclen = [0 cumsum(sqrt(sum(diff(path)'.^2)))]; 18 | arclen_new = 0:1:max(arclen); 19 | path = [interp1(arclen, path(:,1), arclen_new);interp1(arclen, path(:,2), arclen_new)]'; 20 | 21 | %% Discretize 22 | first_point = path(1,:); 23 | last_point = path(1,:); 24 | for i = 2:size(path,1) 25 | % Remove some checkpoints, they are too dense: if sparse enough 26 | if norm(path(1,:) - last_point(end,:)) > checkpoint_distance 27 | if i > size(path,1)/2 && norm(path(1,:) - first_point(1,:)) < 4 % Abort before the loop closes 28 | break 29 | end 30 | last_point = path(1,:); 31 | 32 | 33 | %% Create checkpoint struct 34 | % vector tangential to movement diraction, normalized 35 | t = path(2,:)' - path(1,:)'; 36 | t = t / norm(t); 37 | 38 | % normal vector, pointing to the left (called "n" in paper) 39 | n = [0 -1;1 0] * t; 40 | 41 | % save discretization in struct 42 | checkpoints(end + 1).forward_vector = t; 43 | checkpoints(end).normal_vector = n; 44 | checkpoints(end).center = path(1,:)'; 45 | checkpoints(end).left = path(1,:)' + n * trackWidth/2; 46 | checkpoints(end).right = path(1,:)' - n * trackWidth/2; 47 | end 48 | path = circshift(path,-1); 49 | end 50 | end -------------------------------------------------------------------------------- /code/+model/+track/HockenheimShortCarMaker.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/code/+model/+track/HockenheimShortCarMaker.mat -------------------------------------------------------------------------------- /code/+model/+track/add_turn.m: -------------------------------------------------------------------------------- 1 | % Track element definition 2 | 3 | function checkpoints = add_turn(checkpoints, phi, L, width, N) 4 | % width - set to trackwidth normally 5 | % phi - segment of circle from 0.0 (straight) to 1.0 (full circle) and 6 | % direction of turn (- for right turn/clockwise, + for left 7 | % turn/anti-clockwise) 8 | % L - arc length of section 9 | % N number of segments for track element 10 | 11 | kappa = (phi*(2*pi))/L; 12 | ds = L / N; 13 | 14 | for i = 1:N 15 | checkpoints(end+1).yaw = checkpoints(end).yaw + kappa * ds; 16 | c = cos(checkpoints(end).yaw); 17 | s = sin(checkpoints(end).yaw); 18 | % vector tangential to movement direction (pointing "forward"), normalized 19 | f = [c;s]; 20 | % normal vector, pointing to the left (called "n" in paper) 21 | n = [0 -1;1 0] * f; 22 | 23 | % save discretization in struct 24 | checkpoints(end).forward_vector = f; 25 | checkpoints(end).normal_vector = n; 26 | checkpoints(end).center = checkpoints(end-1).center + f * ds; 27 | checkpoints(end).left = checkpoints(end).center + n * width/2; 28 | checkpoints(end).right = checkpoints(end).center - n * width/2; 29 | checkpoints(end).ds = ds; 30 | end 31 | end -------------------------------------------------------------------------------- /code/+model/+track/add_turn_N100.m: -------------------------------------------------------------------------------- 1 | % Track element definition 2 | 3 | function checkpoints = add_turn_N100(checkpoints, phi, L, width) 4 | checkpoints = model.track.add_turn(checkpoints, phi, L, width, 100); 5 | end -------------------------------------------------------------------------------- /code/+model/+track/add_turn_N30.m: -------------------------------------------------------------------------------- 1 | % Track element definition 2 | 3 | function checkpoints = add_turn_N30(checkpoints, phi, L, width) 4 | checkpoints = model.track.add_turn(checkpoints, phi, L, width, 30); 5 | end -------------------------------------------------------------------------------- /code/+model/+track/add_turn_N40.m: -------------------------------------------------------------------------------- 1 | % Track element definition 2 | 3 | function checkpoints = add_turn_N40(checkpoints, phi, L, width) 4 | checkpoints = model.track.add_turn(checkpoints, phi, L, width, 40); 5 | end -------------------------------------------------------------------------------- /code/+model/+track/add_turn_N50.m: -------------------------------------------------------------------------------- 1 | % Track element definition 2 | 3 | function checkpoints = add_turn_N50(checkpoints, phi, L, width) 4 | checkpoints = model.track.add_turn(checkpoints, phi, L, width, 50); 5 | end -------------------------------------------------------------------------------- /code/+model/+track/spline_to_checkpoints.m: -------------------------------------------------------------------------------- 1 | function checkpoints = spline_to_checkpoints(spline, trackWidth, N) 2 | point_coeffs = zeros(4,N); 3 | tangent_coeffs = zeros(4,N); 4 | 5 | for k = 1:N 6 | t = k/N; 7 | point_coeffs(1,k) = (1+2*t)*(1-t)^2; 8 | point_coeffs(2,k) = t*(1-t)^2; 9 | point_coeffs(3,k) = t^2*(3-2*t); 10 | point_coeffs(4,k) = t^2*(t-1); 11 | 12 | tangent_coeffs(1,k) = 6*t^2 - 6*t; 13 | tangent_coeffs(2,k) = 3*t^2 - 4*t +1; 14 | tangent_coeffs(3,k) = -6*t^2 + 6*t; 15 | tangent_coeffs(4,k) = 3*t^2 - 2*t; 16 | end 17 | 18 | checkpoints = struct; 19 | for k = 1:length(spline)-1 20 | 21 | B = [spline(k).point 2*spline(k).tangent spline(k+1).point 2*spline(k+1).tangent]; 22 | points = B * point_coeffs; 23 | tangents = B * tangent_coeffs; 24 | 25 | for j = 1:N 26 | % vector tangential to movement diraction, normalized 27 | t = tangents(:,j); 28 | t = t / norm(t); 29 | 30 | % normal vector, pointing to the left (called "n" in paper) 31 | n = [0 -1;1 0] * t; 32 | 33 | % save discretization in struct 34 | checkpoints(end + 1).forward_vector = t; % end +1 was (k-1)*N+j 35 | checkpoints(end).normal_vector = n; 36 | checkpoints(end).center = points(:,j); 37 | checkpoints(end).left = points(:,j) + trackWidth * n/2; 38 | checkpoints(end).right = points(:,j) - trackWidth * n/2; 39 | end 40 | end 41 | end 42 | 43 | -------------------------------------------------------------------------------- /code/+model/+track/testCircle.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircle 2 | % scale 1:43 3 | % by Botz 4 | 5 | % Circular track corresponding to new vehicle dimensions (length 6 | % 0.075 m, width 0.045 m) for provoked oversteering/drifting. 7 | creation_scale = 1/43; 8 | 9 | trackWidth = 0.3; 10 | 11 | checkpoints = struct; 12 | checkpoints.left = [0; trackWidth/2]; 13 | checkpoints.right = [0; -trackWidth/2]; 14 | checkpoints.center = [0; 0]; 15 | checkpoints.yaw = 0; 16 | checkpoints.forward_vector = [1; 0]; 17 | checkpoints.ds = 0; 18 | 19 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.25, trackWidth); 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.25, trackWidth); 21 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.25, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.25, trackWidth); 23 | 24 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 25 | end -------------------------------------------------------------------------------- /code/+model/+track/testCircuitA.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitA 2 | % scale 1:43 3 | % by Botz 4 | % Real track layout similar to Janis 5 | creation_scale = 1/43; 6 | 7 | trackWidth = 0.3; 8 | 9 | checkpoints = struct; 10 | checkpoints.left = [0; trackWidth/2]; 11 | checkpoints.right = [0; -trackWidth/2]; 12 | checkpoints.center = [0; 0]; 13 | checkpoints.yaw = 0; 14 | checkpoints.forward_vector = [1; 0]; 15 | checkpoints.ds = 0; 16 | 17 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 2.9, trackWidth); 18 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 2, trackWidth); 19 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.3, trackWidth); 20 | 21 | checkpoints = model.track.add_turn_N30(checkpoints, -0.1, 1.2, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, 0.1, 1.2, trackWidth); 23 | 24 | checkpoints = model.track.add_turn_N30(checkpoints, 0.5, 0.7, trackWidth); 25 | checkpoints = model.track.add_turn_N30(checkpoints, -0.5, 1.2, trackWidth); 26 | checkpoints = model.track.add_turn_N30(checkpoints, 0.5, 0.7, trackWidth); 27 | 28 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 2.4, trackWidth); 29 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.4, trackWidth); 30 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.8, trackWidth); 31 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 2.2, trackWidth); 32 | 33 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 3.5, trackWidth); 34 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.39, trackWidth); 35 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.8, trackWidth); 36 | 37 | 38 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 39 | 40 | end 41 | 42 | -------------------------------------------------------------------------------- /code/+model/+track/testCircuitC.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitC 2 | % scale 1:43 3 | % by Botz 4 | % Oval track corresponding to new vehicle dimensions (length 5 | % 0.075 m, width 0.045 m). 6 | creation_scale = 1/43; 7 | 8 | trackWidth = 0.2; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 3.4, trackWidth); 19 | 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 21 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, 0.25, 0.5, trackWidth); 23 | checkpoints = model.track.add_turn_N30(checkpoints, 0.25, 0.5, trackWidth); 24 | 25 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.7, trackWidth); 26 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5, trackWidth); 27 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.4, trackWidth); 28 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.8, trackWidth); 29 | checkpoints = model.track.add_turn_N30(checkpoints, -0.125, 0.4, trackWidth); 30 | 31 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.8, trackWidth); 32 | checkpoints = model.track.add_turn_N30(checkpoints, 0.15, 0.5, trackWidth); 33 | checkpoints = model.track.add_turn_N30(checkpoints, -0.3, 0.5, trackWidth); 34 | checkpoints = model.track.add_turn_N30(checkpoints, 0.3, 0.5, trackWidth); 35 | checkpoints = model.track.add_turn_N30(checkpoints, -0.15, 0.5, trackWidth); 36 | 37 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.7, trackWidth); 38 | checkpoints = model.track.add_turn_N30(checkpoints, -0.375, 0.5, trackWidth); 39 | 40 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 41 | end -------------------------------------------------------------------------------- /code/+model/+track/testCircuitE.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitE 2 | % scale 1:1 3 | % by Maczijewski 4 | % was called "hockenheim_simple" despite having not similarity 5 | % creating simple, artificial hockenheim track 6 | creation_scale = 1/1; 7 | 8 | trackWidth = 7; 9 | 10 | % start point 11 | checkpoints = struct; 12 | checkpoints.left = [0; trackWidth/2]; 13 | checkpoints.right = [0; -trackWidth/2]; 14 | checkpoints.center = [0; 0]; 15 | checkpoints.yaw = 0; 16 | checkpoints.forward_vector = [1; 0]; 17 | 18 | % construct track 19 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 76, trackWidth); 20 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 21 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 8, trackWidth); 22 | checkpoints = model.track.add_turn_N40(checkpoints, -0.1, 30, trackWidth); 23 | checkpoints = model.track.add_turn_N40(checkpoints, 0.1, 30, trackWidth); 24 | checkpoints = model.track.add_turn_N40(checkpoints, 0.5, 15, trackWidth); 25 | checkpoints = model.track.add_turn_N40(checkpoints, -0.5, 30, trackWidth); 26 | checkpoints = model.track.add_turn_N40(checkpoints, 0.5, 15, trackWidth); 27 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 60, trackWidth); 28 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 10, trackWidth); 29 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 20, trackWidth); 30 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 55, trackWidth); 31 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 90.3, trackWidth); 32 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 5.3, trackWidth); 33 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 20, trackWidth); 34 | 35 | % remove double entry (start & end) 36 | checkpoints = checkpoints(2:end); 37 | end -------------------------------------------------------------------------------- /code/+model/+track/testCircuitF.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitF 2 | % scale 1:1 3 | % by Maczijewski 4 | creation_scale = 1/1; 5 | 6 | trackWidth = 7; 7 | 8 | checkpoints = struct; % contains all relevatn information/a definition of the track layout 9 | 10 | checkpoints.left = [0; trackWidth/2]; 11 | checkpoints.right = [0; -trackWidth/2]; 12 | checkpoints.center = [0; 0]; % row vector with x and y coordinate 13 | checkpoints.yaw = 0; 14 | checkpoints.forward_vector = [1; 0]; 15 | 16 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 76, trackWidth); 17 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 18 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 8, trackWidth); 19 | checkpoints = model.track.add_turn_N40(checkpoints, -0.1, 30, trackWidth); 20 | checkpoints = model.track.add_turn_N40(checkpoints, 0.1, 30, trackWidth); 21 | checkpoints = model.track.add_turn_N40(checkpoints, 0.5, 15, trackWidth); 22 | checkpoints = model.track.add_turn_N40(checkpoints, -0.5, 30, trackWidth); 23 | checkpoints = model.track.add_turn_N40(checkpoints, 0.5, 15, trackWidth); 24 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 60, trackWidth); 25 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 10, trackWidth); 26 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 20, trackWidth); 27 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 55, trackWidth); 28 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 90.3, trackWidth); 29 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 5.3, trackWidth); 30 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 20, trackWidth); 31 | 32 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 33 | end 34 | -------------------------------------------------------------------------------- /code/+model/+track/testCircuitLiniger.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitLiniger 2 | % scale 1:43 3 | % by Botz 4 | % track similar to A. Liniger, A. Domahidi, and M. Morari, “Optimization-Based Autonomous Racing of 1:43 Scale RC Cars,” Optim. Control Appl. Meth., vol. 36, no. 5, pp. 628–647, Sep. 2015, doi: 10.1002/oca.2123. 5 | creation_scale = 1/43; 6 | 7 | trackWidth = 0.3; 8 | 9 | checkpoints = struct; 10 | checkpoints.left = [0; trackWidth/2]; 11 | checkpoints.right = [0; -trackWidth/2]; 12 | checkpoints.center = [0; 0]; 13 | checkpoints.yaw = 0; 14 | checkpoints.forward_vector = [1; 0]; 15 | checkpoints.ds = 0; 16 | 17 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 1.25, trackWidth); 18 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 1.5, trackWidth); 19 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 1, trackWidth); 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.8, trackWidth); 21 | 22 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.4, trackWidth); 23 | checkpoints = model.track.add_turn_N30(checkpoints, 0.25, 0.4, trackWidth); 24 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.4, trackWidth); 25 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.6, trackWidth); 26 | 27 | checkpoints = model.track.add_turn_N30(checkpoints, -0.5, 0.6, trackWidth); 28 | checkpoints = model.track.add_turn_N30(checkpoints, 0.5, 0.6, trackWidth); 29 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.3, trackWidth); 30 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.4, trackWidth); 31 | 32 | checkpoints = model.track.add_turn_N30(checkpoints, -0.375, 0.525, trackWidth); 33 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.6, trackWidth); 34 | checkpoints = model.track.add_turn_N30(checkpoints, 0.375, 1.3, trackWidth); 35 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.6, trackWidth); 36 | 37 | checkpoints = model.track.add_turn_N30(checkpoints, 0.25, 0.6, trackWidth); 38 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.3, trackWidth); 39 | checkpoints = model.track.add_turn_N30(checkpoints, 0.375, 0.525, trackWidth); 40 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 0.5, trackWidth); 41 | checkpoints = model.track.add_turn_N30(checkpoints, -0.5, 0.6, trackWidth); 42 | 43 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 1.6, trackWidth); 44 | checkpoints = model.track.add_turn_N30(checkpoints, -0.375, 0.525, trackWidth); 45 | 46 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 47 | end -------------------------------------------------------------------------------- /code/+model/+track/testCircuitSpline.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testCircuitSpline(n_spline) 2 | % n_spline e.g. 10 3 | % was called TestTrack1 4 | creation_scale = 1/1; 5 | 6 | if ~exist('n_spline', 'var'); n_spline = 10; end 7 | 8 | spline = struct; 9 | spline(1).point = [58 106]'; 10 | spline(1).tangent = [-10 -27]'; 11 | 12 | spline(2).point = [52 45]'; 13 | spline(2).tangent = [10 -30]'; 14 | 15 | spline(3).point = [50 8]'; 16 | spline(3).tangent = [-15 -5]'; 17 | 18 | spline(4).point = [22 10]'; 19 | spline(4).tangent = [-15 5]'; 20 | 21 | spline(5).point = [17 48]'; 22 | spline(5).tangent = [0 15]'; 23 | 24 | spline(6).point = [14 62]'; 25 | spline(6).tangent = [-4 2]'; 26 | 27 | spline(7).point = [3 63]'; 28 | spline(7).tangent = 1.2*[-4 3]'; 29 | 30 | spline(8).point = [11 81]'; 31 | spline(8).tangent = [12 10]'; 32 | 33 | spline(9).point = [32 115]'; 34 | spline(9).tangent = [6 20]'; 35 | 36 | spline(10).point = [52 133]'; 37 | spline(10).tangent = [15 -5]'; 38 | 39 | spline(11).point = [58 106]'; 40 | spline(11).tangent = [-10 -27]'; 41 | 42 | for i=1:length(spline) 43 | spline(i).point = spline(i).point + [-39 -6]'; 44 | end 45 | 46 | trackWidth = 4.5; 47 | checkpoints = model.track.spline_to_checkpoints(spline, trackWidth, n_spline); 48 | 49 | % remove double entry (start & end) 50 | checkpoints = checkpoints(2:end); 51 | end -------------------------------------------------------------------------------- /code/+model/+track/testOvalA.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testOvalA 2 | % scale 1:1 3 | % by Botz 4 | % Oval track corresponding to original dimensions of the vehicles 5 | % according to Maczijewski 2017. 6 | creation_scale = 1/1; 7 | 8 | trackWidth = 10; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 80, trackWidth); 19 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 20 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 21 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 80, trackWidth); 22 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 23 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 50, trackWidth); 24 | 25 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 26 | 27 | end 28 | -------------------------------------------------------------------------------- /code/+model/+track/testOvalB.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testOvalB 2 | % scale 1:43 3 | % by Botz 4 | % Oval track corresponding to new vehicle dimensions (length 5 | % 0.075 m, width 0.045 m). 6 | creation_scale = 1/43; 7 | 8 | trackWidth = 0.3; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N100(checkpoints, 0, 7, trackWidth); 19 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 21 | checkpoints = model.track.add_turn_N100(checkpoints, 0, 7, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 23 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 24 | 25 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 26 | end -------------------------------------------------------------------------------- /code/+model/+track/testOvalC.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testOvalC 2 | % scale 1:43 3 | % by Botz 4 | % Oval track corresponding to new vehicle dimensions (length 5 | % 0.075 m, width 0.045 m). 6 | creation_scale = 1/43; 7 | 8 | trackWidth = 0.3; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 3, trackWidth); 19 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.28, trackWidth); 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.28, trackWidth); 21 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 3, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.28, trackWidth); 23 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.28, trackWidth); 24 | 25 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 26 | end -------------------------------------------------------------------------------- /code/+model/+track/testOvalD.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testOvalD 2 | % scale 1:43 3 | % by Botz 4 | % Oval track corresponding to new vehicle dimensions (length 5 | % 0.075 m, width 0.045 m). 6 | creation_scale = 1/43; 7 | 8 | trackWidth = 0.2; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 3, trackWidth); 19 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 20 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 21 | checkpoints = model.track.add_turn_N50(checkpoints, 0, 3, trackWidth); 22 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 23 | checkpoints = model.track.add_turn_N30(checkpoints, -0.25, 0.5, trackWidth); 24 | 25 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 26 | end -------------------------------------------------------------------------------- /code/+model/+track/testOvalE.m: -------------------------------------------------------------------------------- 1 | function [checkpoints, creation_scale] = testOvalE 2 | % scale 1:43 3 | % by Botz 4 | % Basic oval track corresponding to new vehicle dimensions (length 5 | % 0.075 m, width 0.045 m) 6 | creation_scale = 1/43; 7 | 8 | trackWidth = 0.2; 9 | 10 | checkpoints = struct; 11 | checkpoints.left = [0; trackWidth/2]; 12 | checkpoints.right = [0; -trackWidth/2]; 13 | checkpoints.center = [0; 0]; 14 | checkpoints.yaw = 0; 15 | checkpoints.forward_vector = [1; 0]; 16 | checkpoints.ds = 0; 17 | 18 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 3, trackWidth); 19 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 0.19, trackWidth); 20 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 0.19, trackWidth); 21 | checkpoints = model.track.add_turn_N40(checkpoints, 0, 3, trackWidth); 22 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 0.19, trackWidth); 23 | checkpoints = model.track.add_turn_N40(checkpoints, -0.25, 0.19, trackWidth); 24 | 25 | checkpoints = checkpoints(2:end); % select checkpoints 2 till end 26 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/Base.m: -------------------------------------------------------------------------------- 1 | classdef Base < handle 2 | % wrapping all functions around a given model as an ODE & its 3 | % parameters: 4 | % linearization, discretization, ... 5 | 6 | properties 7 | p % parameters of the vehicle 8 | Hp % prediction horizon 9 | dt % time-step size 10 | 11 | nx % Number of state variables 12 | nu % Number of control/input variables 13 | ns % Number of states + controls 14 | 15 | idx_x % Indices of state variables 16 | idx_pos % Indices of position variables - PART OF x 17 | idx_u % Indices of control variables 18 | end 19 | 20 | methods 21 | function obj = Base(nx, nu, Hp, dt, p) 22 | % NOTE nx and nu have to match ODE 23 | obj.p = p; 24 | obj.nx = nx; 25 | obj.nu = nu; 26 | 27 | obj.Hp = Hp; 28 | obj.dt = dt; 29 | 30 | obj.ns = nx + nu; 31 | obj.idx_x = 1:nx; 32 | obj.idx_pos = 1:2; 33 | obj.idx_u = nx + (1:nu); 34 | end 35 | 36 | function dx = calc_step(obj, x, u) 37 | dx = obj.ode(obj.p, x, u); 38 | end 39 | end 40 | 41 | methods (Abstract) 42 | % Calculate discretized and linearized state-space matrices at 43 | % given working points (being state and input; given for 44 | % every predcition step) at every given step 45 | [Ad, Bd, Ed] = calculatePredictionMatrices(obj, x0, u0) 46 | 47 | % Model-defining ODE (ordinary differential equation) 48 | dX = ode(obj, x, u) 49 | end 50 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/BaseOde.m: -------------------------------------------------------------------------------- 1 | classdef BaseOde < model.vehicle.Base 2 | % base class for ODEs with symbolic differentiation and linearization 3 | 4 | properties 5 | ode_jacobian % symbolic Jacobian of the ODE 6 | end 7 | 8 | methods 9 | function obj = BaseOde(nx, nu, Hp, dt, p) 10 | % call superclass constructor 11 | obj@model.vehicle.Base(nx, nu, Hp, dt, p) 12 | end 13 | 14 | function precomputeJacobian(obj) 15 | % pre-calc symbolic linearization 16 | obj.ode_jacobian = obj.linearize_model(); 17 | end 18 | 19 | function dx = calc_step(obj, x, u) 20 | dx = obj.ode(obj.p, x, u); 21 | end 22 | 23 | function [Ad, Bd, Ed] = calculatePredictionMatrices(obj, x0, u0) 24 | % Initialize discretized matrices 25 | Ad = zeros(obj.nx, obj.nx, obj.Hp); 26 | Bd = zeros(obj.nx, obj.nu, obj.Hp); 27 | Ed = zeros(obj.nx, obj.Hp); 28 | 29 | % Calculate discretized prediction matrix for each prediction step 30 | for i = 1:obj.Hp 31 | [Ad(:,:,i), Bd(:,:,i), Ed(:,i)] = ... 32 | obj.discretize_linearized_model(x0(:,i), u0(:,i)); 33 | end 34 | end 35 | end 36 | 37 | methods (Abstract) 38 | % defined by superclass, see there 39 | dX = ode(obj, x, u) 40 | end 41 | 42 | methods (Access = private) 43 | function linearized_model = linearize_model(obj) 44 | % create linearization function ODE of given model. Returns function 45 | % which linearizes model at given state and input 46 | 47 | %% Symbolic Calculations 48 | % create symbolic state and input variable 49 | % (name, [n by m array], variable is in set of real numbers) 50 | x0 = sym('x0_', [obj.nx, 1], 'real'); 51 | u0 = sym('u0_', [obj.nu, 1], 'real'); 52 | 53 | % calculate dx (ODE at 0) 54 | dx_0 = obj.ode(x0, u0); % TODO CAVE CommonRoad uses other arg order 55 | 56 | % compute Jacobian as symbolic expression 57 | Ac = jacobian(dx_0, x0); % derivation of dx to x 58 | Bc = jacobian(dx_0, u0); % derivation of dx to u 59 | Ec = dx_0 - Ac * x0 - Bc * u0; % Taylor-series? TODO 60 | 61 | %% convert all of above symbolic expression to function handle 62 | % with 3 outputs, using 'Vars' as inputs 63 | linearized_model = matlabFunction(Ac, Bc, Ec, 'Vars', {x0, u0}); 64 | end 65 | 66 | function [Ad, Bd, Ed] = discretize_linearized_model(obj, x0, u0) 67 | % Adapt value of velocity in x-dircetion [ x0(3) = dx ] to avoid 68 | % NaN-results because system-ode devides by dX 69 | if x0(3) == 0 70 | x0(3) = eps; % assign smallest possible value in Matlab 71 | end 72 | 73 | % Get linearized model 74 | [Ac, Bc, Ec] = obj.ode_jacobian(x0, u0); 75 | 76 | %% Discretize model 77 | % (1) 78 | % Formula from wikipedia 79 | % https://en.wikipedia.org/wiki/Discretization#cite_ref-1 80 | % expm([A B; 0 0] * dt) == [Ad Bd; 0 eye] 81 | % Cited from 82 | % Raymond DeCarlo: Linear Systems: A State Variable Approach with Numerical Implementation, Prentice Hall, NJ, 1989 83 | % page 215 84 | tmp = expm(obj.dt*[Ac Bc; zeros(size(Ac,2)+size(Bc,2)-size(Ac,1),size(Ac,2)+size(Bc,2))]); 85 | Ad = tmp(1:size(Ac,1),1:size(Ac,2)); 86 | Bd = tmp(1:size(Bc,1),[1:size(Bc,2)]+size(Ac,2)); 87 | % tmp2 = expm_new(dt*[Ac Bc; zeros(size(Ac,2)+size(Bc,2)-size(Ac,1),size(Ac,2)+size(Bc,2))]); 88 | % Ad2 = tmp2(1:size(Ac,1),1:size(Ac,2)); 89 | % Bd2 = tmp2(1:size(Bc,1),[1:size(Bc,2)]+size(Ac,2)); 90 | 91 | tmp = expm(obj.dt*[Ac Ec; zeros(size(Ac,2)+size(Ec,2)-size(Ac,1),size(Ac,2)+size(Ec,2))]); 92 | Ed = tmp(1:size(Ec,1), [1:size(Ec,2)]+size(Ac,2)); 93 | 94 | % (2) Control System Toolbox (from Alrifaee 2017) 95 | % Cc = [1 0 0 0 0 0;0 1 0 0 0 0]; 96 | % dt = model.p.dt; 97 | % 98 | % m = ss( Ac, Bc, Cc, zeros(size(Cc,1),size(Bc,2)) ); % Zero-Matrix corresponds to Dc 99 | % md = c2d( m, dt ); 100 | % Ad = md.a; 101 | % Bd = md.b; 102 | % 103 | % maff = ss( Ac,Ec,Cc,zeros(size(Cc,1),1) ); 104 | % mdaff = c2d(maff,dt); 105 | % Ed = mdaff.b; 106 | 107 | % (3) Symbolic Toolbox 108 | % dt = model.p.dt; 109 | % M = dt*[Ac Bc; zeros(size(Ac,2)+size(Bc,2)-size(Ac,1),size(Ac,2)+size(Bc,2))]; 110 | % tmp = expm(M); 111 | end 112 | end 113 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/SingleTrack.m: -------------------------------------------------------------------------------- 1 | classdef SingleTrack < model.vehicle.BaseOde 2 | % input: /delta steering angle, /tau torque rear wheels 3 | % as defined in A. Liniger, ‘Path Planning and Control for Autonomous Racing’, Dissertation, ETH Zurich, 2018. 4 | methods (Static) 5 | function p = getParamsLinigerRC_1_43_WithLinigerBounds 6 | % Liniger RC 1:43 MPCC from GitHub https://github.com/alexliniger/MPCC/blob/84cc61d628a165a424c805bbe071fe96b88da2d0/Matlab/getModelParams.m 7 | % Bounds adapted from https://github.com/alexliniger/MPCC/blob/84cc61d628a165a424c805bbe071fe96b88da2d0/Matlab/getMPC_vars.m 8 | 9 | p.paramsName = 'ST from Liniger 1:43'; 10 | 11 | % Tire 12 | p.Br = 3.3852; 13 | p.Cr = 1.2691; 14 | p.Dr = 0.1737; 15 | 16 | p.Bf = 2.579; 17 | p.Cf = 1.2; 18 | p.Df = 0.192; 19 | 20 | % Vehicle 21 | p.m = 0.041; % vehicle mass [kg] 22 | p.Iz = 27.8e-6; % vehicle inertia [kg m^2] 23 | p.l_f = 0.029; % front wheel to CoG [m] 24 | p.l_r = 0.033; % rear wheel to CoG [m] 25 | 26 | % Acceleration/Drag 27 | p.Cm1 = 0.287; 28 | p.Cm2 = 0.0545; 29 | p.Cr0 = 0.0518; 30 | p.Cr2 = 0.00035; 31 | 32 | % Bounds (first row lower, second upper bounds) 33 | % states inputs 34 | % p_x p_y v_x v_y yaw dyaw delta d 35 | % m m m/s m/s 1/s rad % 36 | % CAVE position bounds are not considered in QP creation 37 | % Liniger yaw bound was +-10, but periodicity of yaw makes no 38 | % sense with these bounds -> removed 39 | p.bounds = ... 40 | [-Inf -Inf -.1 -2 -Inf -7 -.35 -.1 ; 41 | Inf Inf 4 2 Inf 7 .35 1 ]; 42 | % CAVE delta not considered in QP creation 43 | %p.bounds_delta = ... 44 | % [-Inf -Inf -Inf -Inf -Inf -Inf -1 -1 ; 45 | % Inf Inf Inf Inf Inf Inf 1 1 ]; 46 | end 47 | 48 | function p = getParamsKloockRC_1_43_WithLinigerBounds 49 | % Liniger, acc. to M. Kloock, P. Scheffe, L. Botz, J. Maczijewski, B. Alrifaee, and S. Kowalewski, ‘Networked Model Predictive Vehicle Race Control’, in 2019 IEEE Intelligent Transportation Systems Conference (ITSC), Auckland, New Zealand, Oct. 2019, pp. 1552–1557, doi: 10.1109/ITSC.2019.8917222. 50 | 51 | % get defaults, then overwrite changes 52 | p = model.vehicle.SingleTrack.getParamsLinigerRC_1_43_WithLinigerBounds(); 53 | 54 | p.paramsName = 'ST from Kloock via Plots ST Liniger 1:43'; 55 | 56 | % Tire (CAVE: being different than Liniger GitHub's) 57 | p.Bf = 3.0323; 58 | p.Cf = 1.9669; 59 | p.Df = 0.1884; 60 | 61 | p.Br = 4.1165; 62 | p.Cr = 1.3709; 63 | p.Dr = 0.1644; 64 | 65 | % Acceleration/Drag 66 | % Kloock simplified formula to "n*T", 67 | % n beeing transmission ratio 68 | % T being the input torque 69 | % this model can be represented with the following parameters 70 | p.Cm1 = 1; 71 | p.Cm2 = 0; 72 | p.Cr0 = 0; 73 | p.Cr2 = 0; 74 | end 75 | 76 | function p = getParamsKloockRC_1_43_WithKloocksBounds 77 | % get defaults, then overwrite changes 78 | p = model.vehicle.SingleTrack.getParamsKloockRC_1_43_WithLinigerBounds(); 79 | 80 | p.paramsName = 'ST from Kloock via Plots ST Liniger 1:43, plus custom bounds'; 81 | 82 | % Bounds (first row upper, second lower bounds) 83 | % states inputs 84 | % p_x p_y v_x v_y yaw dyaw delta d 85 | % m m m/s m/s 1/s rad [CAVE] Nm (different model) 86 | % CAVE position bounds are not considered in QP creation 87 | p.bounds = ... 88 | [-Inf -Inf .05 -2 -Inf -2*pi -.4 -.08 ; 89 | Inf Inf 2 2 Inf 2*pi .4 .08]; 90 | % CAVE delta not considered in QP creation 91 | %p.bounds_delta = ... 92 | % [-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf ; 93 | % Inf Inf Inf Inf Inf Inf Inf Inf]; 94 | end 95 | end 96 | 97 | methods 98 | function obj = SingleTrack(Hp, dt, p) 99 | obj@model.vehicle.BaseOde(6, 2, Hp, dt, p) % call superclass constructor 100 | end 101 | 102 | function dX = ode(obj, x, u) 103 | % The kinetic bicycle model from Liniger et al. (2014) is used and 104 | % slightly adapted to represent the vehicle dynamics. The model used in 105 | % Liniger et al. (2014) is based on Velenis et al. (2009) and Voser et 106 | % al. (2010), tire forces are modeled based on a simplified Pacejka 107 | % Tire Model presented in Bakker et al. (1987). The longitudinal force 108 | % of the rear wheel (Fr_x) is simplified. 109 | 110 | % Inputs: p (parameter struct), X (vector of current system states), 111 | % u (vector of current system inputs) 112 | % Ouputs: dX (vector of first order differential equations) 113 | 114 | % States: 115 | % 1 x-position of vehicle CoG in intertial frame 116 | % 2 y-position of vehicle CoG in intertial frame 117 | % 3 longitudinal velocity of vehicle CoG 118 | % 4 lateral velocity of vehicle CoG 119 | % 5 vehicle angle relative to inertial frame (yaw) 120 | % 6 yaw rate 121 | 122 | % Inputs 123 | % 1 steering angle 124 | % 2 motor torque 125 | 126 | %% Readability 127 | % States 128 | v_long = x(3); 129 | v_lat = x(4); 130 | yaw = x(5); 131 | dyaw = x(6); 132 | % Inputs 133 | delta = u(1); 134 | t = u(2); 135 | 136 | %% Tire forces 137 | % front/rear tire side slip angle 138 | alpha_f = -atan2(dyaw * obj.p.l_f + v_lat, v_long) + delta; 139 | alpha_r = atan2(dyaw * obj.p.l_r - v_lat, v_long); 140 | % front/rear tire lateral force 141 | F_fy = obj.p.Df * sin(obj.p.Cf * atan(obj.p.Bf * alpha_f)); 142 | F_ry = obj.p.Dr * sin(obj.p.Cr * atan(obj.p.Br * alpha_r)); 143 | % rear tire longitudinal force 144 | F_rx = (obj.p.Cm1 - obj.p.Cm2 * v_long) * t - obj.p.Cr0 - obj.p.Cr2 * v_long^2; 145 | 146 | 147 | %% ODE 148 | dX = [ 149 | v_long * cos(yaw) - v_lat * sin(yaw); 150 | v_long * sin(yaw) + v_lat * cos(yaw); 151 | 1/obj.p.m * (F_rx - F_fy * sin(delta) + obj.p.m * v_lat * dyaw); % a_long 152 | 1/obj.p.m * (F_ry + F_fy * cos(delta) - obj.p.m * v_long * dyaw); % a_lat 153 | dyaw; 154 | 1/obj.p.Iz * (F_fy * obj.p.l_f * cos(delta) - F_ry * obj.p.l_r)]; 155 | end 156 | end 157 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/SingleTrackWAccelerationController.m: -------------------------------------------------------------------------------- 1 | classdef SingleTrackWAccelerationController < model.vehicle.SingleTrack 2 | % extending Model with Acceleration Controller 3 | % 4 | % controller on basis of SL Paper/Janis' Bachelor Thesis 5 | properties 6 | torque_I 7 | torque_error_prev 8 | phi_I 9 | phi_error_prev 10 | 11 | u_1_ST_prev 12 | end 13 | 14 | methods 15 | function obj = SingleTrackWAccelerationController(Hp, dt, p) 16 | % call superclass constructor 17 | obj@model.vehicle.SingleTrack(Hp, dt, p) 18 | 19 | obj.torque_I = 0; 20 | obj.torque_error_prev = 0; 21 | obj.phi_I = 0; 22 | obj.phi_error_prev = 0; 23 | 24 | obj.u_1_ST_prev = zeros(1, obj.nu); 25 | 26 | warning('Acceleration controller for linear model is experimental, use with caution') 27 | end 28 | 29 | 30 | function u_1_ST = controller(obj, x_0, u_1_lin) 31 | % acceleration controller: converting from desired acceleration 32 | % to single-track inputs (namely torque and steering angle) 33 | % global [a_x a_y] to [delta torque] input 34 | % 35 | % parametrization form SL paper (F1 car 1:1 scale) 36 | % 37 | % Inputs 38 | % u_1_lin is controller output (global acceleration values) 39 | % u_1_ST_prev is previous ST output (for this specific 40 | % vehicle with given x_0 and u_1_lin 41 | 42 | %% Parameters 43 | v_long = x_0(3); 44 | v_lat = x_0(4); 45 | yaw = x_0(5); 46 | 47 | % get current derivative if last inputs are applied 48 | dX = obj.ode(x_0, obj.u_1_ST_prev); 49 | a_long_prev = dX(3); 50 | a_lat_prev = dX(4); 51 | 52 | v = norm([v_long, v_lat]); 53 | % scale v > 1 so controller works properly 54 | v = 90/obj.p.bounds(2,3) * v; 55 | 56 | % convert global to vehicle reference frame acceleration 57 | a_local = model.vehicle.vector_global2local(u_1_lin, yaw); 58 | a_long_ref = a_local(1); 59 | a_lat_ref = a_local(2); 60 | 61 | %% Torque 62 | [torque, obj.torque_I, obj.torque_error_prev] = utils.PID(... 63 | 0.01, 1, 0, obj.dt, a_long_ref, a_long_prev, obj.torque_I, obj.torque_error_prev); 64 | % adapted bounds to match vehicle model 65 | %torque = max(-2, torque); % lower bound 66 | %torque = min( 1, torque); % upper bound 67 | %if torque < 0; torque = .5*torque; end % brake gain 68 | torque = max(obj.p.bounds(1, obj.nx + 2), torque); % lower bound 69 | torque = min(obj.p.bounds(2, obj.nx + 2), torque); % upper bound 70 | 71 | %% Delta 72 | % adapt a_lat_ref for low speeds 73 | % v_low scaling was: v * 0.1 => v_0 = 10 74 | % SL car v_max about 90m/s -> scale to current v_max 75 | %v_0_rescaled_for_scale = 10/90 * obj.p.bounds(2,3); 76 | v_0_rescaled_for_scale = 10; 77 | scale_v_low = v/v_0_rescaled_for_scale; 78 | scale_v_low = max(0, scale_v_low); % lower bound 79 | scale_v_low = min(1, scale_v_low); % upper bound 80 | 81 | % considering equation a_{lat} = v^2 * tan(\delta) / L, 82 | % introduce variable \phi = \delta max(v, v_0)^2 83 | % (with v > v_0 and tan(\delta) = delta) 84 | a_lat_ref_adapted = a_lat_ref * scale_v_low; 85 | [phi, obj.phi_I, obj.phi_error_prev] = utils.PID(... 86 | 2, 2000, 0, obj.dt, a_lat_ref_adapted, a_lat_prev, obj.phi_I, obj.phi_error_prev); 87 | %20, 2000, 0, obj.dt, a_lat_ref_adapted, a_lat_prev, obj.phi_I, obj.phi_error_prev); 88 | phi = max(-10000, phi); % lower bound 89 | phi = min( 10000, phi); % upper bound 90 | 91 | % NOTE thesis says using v_0 as lower bound - but in simulink 92 | % using 2 instead of 10 (as above) 93 | % scaling done as above 94 | %v_0_rescaled_for_delta = 2/90^2 * obj.p.bounds(2,3); 95 | v_0_rescaled_for_delta = 2; 96 | delta = phi/max(v_0_rescaled_for_delta, v)^2; 97 | % scale delta because of strange outputs to CarMaker [-2, 2] 98 | delta = (obj.p.bounds(2, obj.nx + 1)-obj.p.bounds(1, obj.nx + 1))/(2--2) * delta; 99 | % adapted bounds to match vehicle model 100 | delta = max(obj.p.bounds(1, obj.nx + 1), delta); % lower bound 101 | delta = min(obj.p.bounds(2, obj.nx + 1), delta); % upper bound 102 | 103 | %% Output 104 | u_1_ST = [delta, torque]; 105 | obj.u_1_ST_prev = u_1_ST; 106 | 107 | fprintf([... 108 | '\taccel_ctrl: (with v_long %.2f, v_lat %.2f, yaw %.2f)\n'... 109 | '\t\t(a_x, a_y) (%.2f, %.2f) -> (a_long, a_lat) (%.2f, %.2f)-> delta %.2f, torque %.2f\n'... 110 | '\t\t I_torque %.2f error_torque_prev %.2f I_phi %.2f error_phi_prev %.2f\n'],... 111 | v_long, v_lat, yaw,... 112 | u_1_lin(1), u_1_lin(2), a_local(1), a_local(2), u_1_ST(1), u_1_ST(2),... 113 | obj.torque_I, obj.torque_error_prev, obj.phi_I, obj.phi_error_prev); 114 | end 115 | end 116 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/state_st2lin.m: -------------------------------------------------------------------------------- 1 | function state_lin = state_st2lin(state_st) 2 | % convert single-track to linear model states 3 | % single-track 4 | % p_x 5 | % p_y 6 | % v_x 7 | % v_y 8 | % yaw 9 | % dyaw 10 | % linear 11 | % p_x 12 | % p_y 13 | % v_x 14 | % v_y 15 | state_lin = zeros(4, size(state_st,2)) * NaN; 16 | state_lin(1:2,:) = state_st(1:2,:); 17 | state_lin(3:4,:) = model.vehicle.vector_local2global(state_st(3:4,:), state_st(5,:)); -------------------------------------------------------------------------------- /code/+model/+vehicle/vector_global2local.m: -------------------------------------------------------------------------------- 1 | function vector_local = vector_global2local(vector_global, yaw) 2 | % convert global to local (vehicle) reference frame 3 | % 4 | % input and output vertical vector 5 | % 6 | % global 7 | % vector_x 8 | % vector_y 9 | % local 10 | % vector_long 11 | % vector_lat 12 | % yaw 13 | vector_local = [ cos(yaw), sin(yaw); 14 | -sin(yaw), cos(yaw)] * vector_global; -------------------------------------------------------------------------------- /code/+model/+vehicle/vector_local2global.m: -------------------------------------------------------------------------------- 1 | function vector_global = vector_local2global(vector_local, yaw) 2 | % convert global to local (vehicle) reference frame 3 | % 4 | % input and output vertical vector 5 | % 6 | % global 7 | % vector_x 8 | % vector_y 9 | % local 10 | % vector_long 11 | % vector_lat 12 | % yaw 13 | vector_global = vector_local; 14 | for i = 1:numel(yaw) 15 | vector_global(:,i) = [cos(yaw(i)), -sin(yaw(i)); 16 | sin(yaw(i)), cos(yaw(i))] * vector_local(:,1); 17 | end -------------------------------------------------------------------------------- /code/+model/+vehicle/velocity_global2local.m: -------------------------------------------------------------------------------- 1 | function velocity_local = velocity_global2local(velocity_global) 2 | % convert global velocity to local (vehicle) reference frame 3 | % transformation assumes zero side slip angle 4 | % 5 | % input and output vertical vector 6 | % 7 | % global 8 | % velocity_x 9 | % velocity_y 10 | % local 11 | % velocity_long 12 | % velocity_lat 13 | v_x = velocity_global(1); 14 | v_y = velocity_global(2); 15 | velocity_local = 1/sqrt(v_x^2 + v_y^2 + eps) .* [v_x v_y; -v_y v_x] * [v_x; v_y]; -------------------------------------------------------------------------------- /code/+plots/#README.m: -------------------------------------------------------------------------------- 1 | % all plots are in the view of the controller (plotting at discrete 2 | % controller steps, not sim steps) -------------------------------------------------------------------------------- /code/+plots/Base.m: -------------------------------------------------------------------------------- 1 | classdef Base < handle 2 | %BASE Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | figure_handle 7 | handles_tmp % handles of temporary plot objects 8 | is_background_plotted 9 | subplot_plot_handles 10 | end 11 | 12 | methods 13 | function obj = Base(figure_handle_number) 14 | %BASE Construct an instance of this class 15 | % Detailed explanation goes here 16 | 17 | % set random handle if no is given 18 | if ~exist('figure_handle_number', 'var') 19 | figure_handle_number = 100 + randi(99); 20 | end 21 | 22 | % in case plot is used as tile 23 | if ~isnan(figure_handle_number) 24 | obj.figure_handle = figure(figure_handle_number); % create or get existing figure 25 | else 26 | obj.figure_handle = NaN; 27 | end 28 | % sometimes (?) helps with performance: 29 | %set(figure_handle_number, 30 | % 'MenuBar', 'figure', 31 | % 'ToolBar', 'figure'); 32 | 33 | obj.handles_tmp = {}; 34 | obj.is_background_plotted = false; 35 | obj.subplot_plot_handles = {}; 36 | end 37 | 38 | function add_tmp_handle(obj, handle) 39 | obj.handles_tmp{end + 1} = handle; 40 | end 41 | 42 | 43 | function clear_tmp(obj) 44 | % clear old plot objects 45 | for i = 1:length(obj.handles_tmp) 46 | delete(obj.handles_tmp{i}) 47 | end 48 | obj.handles_tmp = {}; 49 | end 50 | end 51 | methods (Abstract) 52 | outputArg = plot(obj, cfg, ws) 53 | end 54 | end 55 | 56 | -------------------------------------------------------------------------------- /code/+plots/Dashboard.m: -------------------------------------------------------------------------------- 1 | classdef Dashboard < plots.Base 2 | % TODO add number of laps display (last lap time + average + cumulative, too) 3 | properties 4 | table_desc 5 | table_val 6 | end 7 | 8 | methods 9 | function obj = Dashboard(figure_handle_number) 10 | % call superclass constructor 11 | obj@plots.Base(figure_handle_number) 12 | 13 | obj.table_desc = {}; 14 | obj.table_val = {}; 15 | end 16 | 17 | function plot(obj, cfg, ws) 18 | % choose vehicle 19 | i_vh = 1; 20 | 21 | % ease variable access 22 | vh = ws.vhs{i_vh}; 23 | vh_cfg = cfg.scn.vhs{i_vh}; 24 | modelParams_controller = vh_cfg.modelParams_controller; 25 | bounds_available = isfield(modelParams_controller, 'bounds'); 26 | colors = utils.getRwthColors(100); 27 | color = colors(i_vh, :); % need to store color for later plot updates (else Matlab's gc will delete) 28 | 29 | set(groot, 'CurrentFigure', obj.figure_handle); % same as 'figure(f)' but without focusing 30 | set(obj.figure_handle, 'WindowState', 'maximized'); 31 | 32 | %% Prepare data 33 | X = vh.X_controller; 34 | X = [vh.x_0_controller X]; 35 | % if you want "vehicle" reference frame velocities, outcomment 36 | % following 37 | % NOTE due to linear model, no slip angle is assumed. thus 38 | % v_{lateral} = 0 39 | %if vh_cfg.isControlModelLinear 40 | % % convert global to vehicle frame velocities 41 | % for k = 1:length(X) 42 | % X(3:4, k) = model.vehicle.velocity_global2local(X(3:4, k)); 43 | % end 44 | %end 45 | 46 | U = vh.U_controller; 47 | U = [U U(:,end)]; % Duplicate last entry for better visibility in plot 48 | 49 | Hp = size(vh.X_controller, 2); 50 | Tx = 0:1:Hp; 51 | Tu = 1:(Hp + 1); % include Hp+1 to display the values at Hp as one stair step 52 | 53 | %% plot base track initially 54 | if ~obj.is_background_plotted 55 | clf(obj.figure_handle) 56 | set(obj.figure_handle, 'Name', 'Dashboard: States & Inputs'); 57 | hold on 58 | 59 | % create plots 60 | subplot(3,3,1); 61 | obj.subplot_plot_handles{1} = plot(Tx, X(3,:), 'Color', color); 62 | if vh_cfg.isControlModelLinear 63 | title('Controller: v_{x} (global) [m/s]') 64 | else 65 | title('Controller: v_{long} [m/s]') 66 | end 67 | if bounds_available 68 | ylim(modelParams_controller.bounds(:, 3)' .* 1.1) 69 | utils.yline_compatiblity(modelParams_controller.bounds(:, 3)', '--r') 70 | end 71 | xlim([Tx(1) Tx(end)]) 72 | grid on 73 | 74 | subplot(3,3,2); 75 | obj.subplot_plot_handles{2} = plot(Tx, X(4,:), 'Color', color); 76 | if vh_cfg.isControlModelLinear 77 | title('Controller: v_{y} (global) [m/s]') 78 | else 79 | title('Controller: v_{lateral} [m/s]') 80 | end 81 | if bounds_available 82 | ylim(modelParams_controller.bounds(:, 4)' .* 1.1) 83 | utils.yline_compatiblity(modelParams_controller.bounds(:, 4)', '--r') 84 | end 85 | xlim([Tx(1) Tx(end)]) 86 | grid on 87 | 88 | if ~vh_cfg.isControlModelLinear 89 | subplot(3,3,4); 90 | % plot multiple parallel yaw angles: 91 | %obj.subplot_plot_handles{3} = plot(Tx, [X(5,:); X(5,:) + 2*pi; X(5,:) - 2*pi], 'Color', color); 92 | obj.subplot_plot_handles{3} = plot(Tx, X(5,:), 'Color', color); 93 | title('Controller: Yaw Angle \phi [rad]') 94 | if bounds_available 95 | % if bounds of model are real 96 | if ~any(isinf(modelParams_controller.bounds(:, 5)')) 97 | ylim(modelParams_controller.bounds(:, 5)' .* 1.1) 98 | utils.yline_compatiblity(modelParams_controller.bounds(:, 5)', '--r') 99 | else % use default bounds 100 | bounds = [-pi, pi]; % use periodicity 101 | ylim(bounds .* 1.1) 102 | end 103 | end 104 | xlim([Tx(1) Tx(end)]) 105 | yticks([-3*pi -2*pi -pi 0 pi 2*pi 3*pi]) 106 | yticklabels({'-3\pi','-2\pi','-\pi','0','\pi','2\pi','3\pi'}) 107 | grid on 108 | 109 | subplot(3,3,5); 110 | obj.subplot_plot_handles{4} = plot(Tx, X(6,:), 'Color', color); 111 | title('Controller: Yaw Rate \omega [rad/sec]') 112 | if bounds_available 113 | ylim(modelParams_controller.bounds(:, 6)' .* 1.1) 114 | utils.yline_compatiblity(modelParams_controller.bounds(:, 6)', '--r') 115 | end 116 | xlim([Tx(1) Tx(end)]) 117 | yticks([-3*pi -2*pi -pi 0 pi 2*pi 3*pi]) 118 | yticklabels({'-3\pi/s','-2\pi/s','-\pi/s','0','\pi/s','2\pi/s','3\pi/s'}) 119 | grid on 120 | end 121 | 122 | subplot(3,3,7); 123 | obj.subplot_plot_handles{5} = plot(Tu, U(1,:), 'Color', color); 124 | if vh_cfg.isControlModelLinear 125 | title('Controller: a_{x} (global) [m/s^2]') 126 | else 127 | title('Controller: Input Steering Angle [rad]') 128 | end 129 | if bounds_available 130 | ylim(modelParams_controller.bounds(:, length(X(:, 1)) + 1)' .* 1.1) 131 | % if bounds are real 132 | if ~any(isinf(modelParams_controller.bounds(:, length(X(:, 1)) + 1)')) 133 | utils.yline_compatiblity(modelParams_controller.bounds(:, length(X(:, 1)) + 1)', '--r') 134 | end 135 | end 136 | xlim([Tu(1) Tu(end)]) 137 | grid on 138 | 139 | subplot(3,3,8); 140 | obj.subplot_plot_handles{6} = plot(Tu, U(2,:), 'Color', color); 141 | 142 | if vh_cfg.isControlModelLinear 143 | title('Controller: a_{y} (global) [m/s^2]') 144 | else 145 | title('Controller: Input Torque [% or Nm]') 146 | end 147 | if bounds_available 148 | ylim(modelParams_controller.bounds(:, length(X(:, 1)) + 2)' .* 1.1) 149 | % if bounds are real 150 | if ~any(isinf(modelParams_controller.bounds(:, length(X(:, 1)) + 2)')) 151 | utils.yline_compatiblity(modelParams_controller.bounds(:, length(X(:, 1)) + 2)', '--r') 152 | end 153 | end 154 | xlim([Tu(1) Tu(end)]) 155 | grid on 156 | 157 | % Text 158 | subplot(3,3,[3,6,9]); 159 | axis off 160 | 161 | %% assemble text 162 | obj.add_table_line('\bfControl: press ESC to abort, SPACE to pause\rm', ''); 163 | obj.add_table_line('', ''); 164 | obj.add_table_line(['\it' sprintf(cfg.scn.description) '\rm'], sprintf(repmat('\n', 1, length(strfind(cfg.scn.description, '\n')))), false); 165 | obj.add_table_line('\bfConfiguration\rm', ''); 166 | % get name of function handle 167 | % MATLAB version handling doesn't work due to "syntax 168 | % error" 169 | %if verLessThan('matlab', '9.7') 170 | fcn_information = functions(cfg.scn.track_handle); 171 | obj.add_table_line('Track', fcn_information.function); 172 | %else 173 | % obj.add_table_line('Track', functions(cfg.scn.track_handle).function); 174 | %end 175 | obj.add_table_line('Track Creation Scale', utils.rat2str(cfg.scn.track_creation_scale)); 176 | 177 | 178 | for i = 1:length(cfg.scn.vhs) 179 | vh = cfg.scn.vhs{i}; 180 | obj.add_table_line('', ''); 181 | obj.add_table_line(['\bfVehicle ' num2str(i) '\rm with \bf\color[rgb]{' sprintf('%f,%f,%f', colors(i, :)) '}color\rm\color{black}'], ''); 182 | obj.add_table_line(['\it' sprintf(vh.description) '\rm'], sprintf(repmat('\n', 1, length(strfind(vh.description, '\n')))), false); 183 | obj.add_table_line('Vehicle Control Model', class(vh.model_controller)); 184 | obj.add_table_line('Vehicle Control Params', vh.model_controller.p.paramsName); 185 | if ~cfg.scn.is_main_vehicle_only || i == 1 % only for vehicles !=1 when main vehicle simulation mode 186 | obj.add_table_line('Vehicle Sim Model', class(vh.model_simulation)); 187 | obj.add_table_line('Vehicle Sim Params', vh.model_simulation.p.paramsName); 188 | else 189 | obj.add_table_line('Vehicle Sim Model', 'simulation via main vehicle #1'); 190 | obj.add_table_line('Vehicle Sim Params', 'simulation via main vehicle #1'); 191 | end 192 | if vh.approximation == vh.approximationSL 193 | approx = 'SL'; 194 | elseif vh.approximation == vh.approximationSCR 195 | approx = 'SCR'; 196 | end 197 | obj.add_table_line('Track Approximation via', approx); 198 | obj.add_table_line('Blocking enabled', obj.get_logical_string(vh.p.isBlockingEnabled)); 199 | obj.add_table_line('Obstacles considered', obj.get_logical_string(vh.p.areObstaclesConsidered)); 200 | end 201 | 202 | % place text top left 203 | text(0, 1, obj.table_desc, 'VerticalAlignment', 'top') 204 | text(0.6, 1, obj.table_val, 'VerticalAlignment', 'top') 205 | 206 | obj.is_background_plotted = true; 207 | else 208 | %obj.clear_tmp() 209 | %cla; 210 | end 211 | 212 | % update plots 213 | set(obj.subplot_plot_handles{1}, 'XData', Tx); 214 | set(obj.subplot_plot_handles{1}, 'YData', X(3,:)); 215 | 216 | set(obj.subplot_plot_handles{2}, 'XData', Tx); 217 | set(obj.subplot_plot_handles{2}, 'YData', X(4,:)); 218 | 219 | if length(X(:, 1)) >=6 % TODO make state dependent 220 | set(obj.subplot_plot_handles{3}, 'XData', Tx); 221 | % plot multiple parallel yaw angles: 222 | %set(obj.subplot_plot_handles{3}, {'YData'}, num2cell([utils.removePiPeriodicity(X(5,:)); utils.removePiPeriodicity(X(5,:)) + 2*pi; utils.removePiPeriodicity(X(5,:)) - 2*pi], 2)); 223 | set(obj.subplot_plot_handles{3}, 'YData', utils.removePiPeriodicity(X(5,:))); 224 | 225 | set(obj.subplot_plot_handles{4}, 'XData', Tx); 226 | set(obj.subplot_plot_handles{4}, 'YData', X(6,:)); 227 | end 228 | 229 | 230 | set(obj.subplot_plot_handles{5}, 'XData', Tu); 231 | set(obj.subplot_plot_handles{5}, 'YData', U(1,:)); 232 | 233 | set(obj.subplot_plot_handles{6}, 'XData', Tu); 234 | set(obj.subplot_plot_handles{6}, 'YData', U(2,:)); 235 | end 236 | 237 | function add_table_line(obj, desc, val, show_colon) 238 | obj.table_desc{end + 1} = desc; 239 | if ~isempty(val) && (nargin > 3 && show_colon) 240 | obj.table_desc{end} = [obj.table_desc{end} ':']; 241 | end 242 | obj.table_val{end + 1} = val; 243 | end 244 | 245 | function logical_str = get_logical_string(~, logical) 246 | logical_str = {'false', 'true'}; 247 | % shift for matlab 1-indexing 248 | logical_str = logical_str{logical + 1}; 249 | end 250 | end 251 | end 252 | 253 | -------------------------------------------------------------------------------- /code/+plots/DashboardAcceleration.m: -------------------------------------------------------------------------------- 1 | classdef DashboardAcceleration < plots.Base 2 | methods 3 | function plot(obj, cfg, ws) 4 | i_vehicle = 1; 5 | colors = utils.getRwthColors(100); 6 | color = colors(i_vehicle, :); % need to store color for later plot updates (else Matlab's gc will delete) 7 | 8 | 9 | 10 | % only plot for linear models (having accelerations as input) 11 | if ~cfg.scn.vhs{i_vehicle}.isControlModelLinear; return; end 12 | 13 | set(groot,'CurrentFigure', obj.figure_handle); % same as 'figure(f)' but without focusing 14 | 15 | U_opt = ws.vhs{i_vehicle}.U_controller; 16 | 17 | % plot base track initially 18 | if ~obj.is_background_plotted 19 | clf(obj.figure_handle) 20 | set(obj.figure_handle,'color',[1 1 1]); 21 | set(obj.figure_handle,'Name', ['Dashboard: Acceleration of Vehicle ' num2str(i_vehicle)]); 22 | hold on 23 | box on 24 | 25 | modelParams_controller = cfg.scn.vhs{i_vehicle}.modelParams_controller; 26 | u_max = max([max(modelParams_controller.a_backward_max) max(modelParams_controller.a_forward_max) max(modelParams_controller.a_lateral_max)]); 27 | 28 | a = linspace(0,2*pi,50); 29 | c = cos(a); 30 | s = sin(a); 31 | 32 | for k = 5:5:u_max 33 | plot(c*k,s*k,'-.k'); 34 | end 35 | 36 | plot(u_max*[-1 1],[0 0],'k') 37 | plot([0 0],u_max*[-1 1],'k') 38 | 39 | daspect([1 1 1]) 40 | xlim(u_max*[-1 1]) 41 | ylim(u_max*[-1 1]) 42 | xlabel('$a_x [m s^{-2}]$','interpreter','LaTeX'); 43 | ylabel('$a_y [m s^{-2}]$','interpreter','LaTeX'); 44 | 45 | obj.is_background_plotted = true; 46 | else 47 | obj.clear_tmp() 48 | end 49 | 50 | obj.add_tmp_handle(plot(U_opt(1,:),U_opt(2,:),'*-', 'Color', color)); 51 | obj.add_tmp_handle(plot(U_opt(1,1),U_opt(2,1),'o', 'MarkerSize',10, 'MarkerEdgeColor','k', 'MarkerFaceColor',color)); 52 | end 53 | end 54 | end -------------------------------------------------------------------------------- /code/+plots/TrackCheckpoints.m: -------------------------------------------------------------------------------- 1 | classdef TrackCheckpoints < plots.Base 2 | % plot discretized track's checkpoints 3 | % 4 | % usage example 5 | % ``` 6 | % [checkpoints, track_creation_scale] = model.track.HockenheimShort() 7 | % plots.TrackCheckpoints().plot(checkpoints, track_creation_scale) 8 | % ``` 9 | 10 | methods 11 | function plot(obj, checkpoints, track_creation_scale) 12 | % in case is distinct plot (need to check object as `isnan` 13 | % doesn't work with objects) 14 | if isobject(obj.figure_handle) || ~isnan(obj.figure_handle) 15 | set(groot, 'CurrentFigure', obj.figure_handle); % same as 'figure(f)' but without focusing 16 | set(obj.figure_handle, 'Name', sprintf('Track - Discretized & Checkpoints (scale %s)', utils.rat2str(track_creation_scale))); 17 | end 18 | 19 | hold on 20 | n = length(checkpoints); 21 | center_points = [checkpoints.center]; 22 | left_points = [checkpoints.left]; 23 | right_points = [checkpoints.right]; 24 | 25 | % center line points 26 | scatter(center_points(1, :), center_points(2, :), 'm', '.') 27 | 28 | % normal vector 29 | plot(... 30 | reshape([left_points(1,:);right_points(1,:);nan(1,n)],3*n,1),... 31 | reshape([left_points(2,:);right_points(2,:);nan(1,n)],3*n,1),... 32 | 'Color',[1 1 1]*0.7); 33 | 34 | % left points 35 | plot([left_points(1,:) left_points(1,1)],[left_points(2,:) left_points(2,1)], 'b'); 36 | 37 | % right points 38 | plot([right_points(1,:) right_points(1,1)],[right_points(2,:) right_points(2,1)], 'r'); 39 | 40 | 41 | if isobject(obj.figure_handle) || ~isnan(obj.figure_handle) 42 | xlabel('x [m]'); ylabel('y [m]') 43 | end 44 | legend(... 45 | 'Center Line Points',... 46 | 'Normal Vectors',... 47 | 'Right Track Points (Connected)',... 48 | 'Left Track Points (Connected)') 49 | axis equal 50 | end 51 | end 52 | end -------------------------------------------------------------------------------- /code/+plots/TrackPolygons.m: -------------------------------------------------------------------------------- 1 | classdef TrackPolygons < plots.Base 2 | % plot discretized track's polygon creation process 3 | % 4 | % usage example 5 | % ``` 6 | % [checkpoints, track_creation_scale] = model.track.HockenheimShort() 7 | % plots.TrackPolygons().plot(checkpoints, track_creation_scale, 0.5) 8 | % ``` 9 | % 10 | % FIXME sync with controller.track_SCR's functions (instead of calling 11 | % the 12 | 13 | methods 14 | function plot(obj, checkpoints, track_tesselated, track_merged, track_overlapped, track_creation_scale, track_SCR_epsilon_area_tolerance) 15 | % epsilon_area_tolerance [m^2]: maximal poylgon differnce for 16 | % merging 17 | set(groot, 'CurrentFigure', obj.figure_handle); % same as 'figure(f)' but without focusing 18 | set(obj.figure_handle, 'Name', sprintf('Track - SCR (scale %s)', utils.rat2str(track_creation_scale)), 'WindowState', 'maximized'); 19 | 20 | if verLessThan('matlab', '9.7') 21 | warning('Unlinked figures for track generation plots due to old MATLAB version') 22 | else 23 | t = tiledlayout(2,2); 24 | if verLessThan('matlab', '9.10') 25 | t.Padding = 'compact'; 26 | t.TileSpacing = 'compact'; 27 | else 28 | t.Padding = 'tight'; 29 | t.TileSpacing = 'tight'; 30 | end 31 | end 32 | 33 | % 0) original 34 | if verLessThan('matlab', '9.7') 35 | figure 36 | xlabel('x [m]'); ylabel('y [m]') 37 | else 38 | ax1 = nexttile; 39 | end 40 | title(sprintf('0) Original Track - Discretized with %i Checkpoints', length(checkpoints))) 41 | plots.TrackCheckpoints(NaN).plot(checkpoints) 42 | 43 | % 1) tesselation (based on discret track checkpoints) 44 | if verLessThan('matlab', '9.7') 45 | figure 46 | xlabel('x [m]'); ylabel('y [m]') 47 | else 48 | ax2 = nexttile; 49 | end 50 | plotTrackPolygons_(track_tesselated, sprintf('1) Tesselation to %i Polygons', length(track_tesselated.polygons))) 51 | 52 | % 2) merge polygons 53 | if verLessThan('matlab', '9.7') 54 | figure 55 | xlabel('x [m]'); ylabel('y [m]') 56 | else 57 | ax3 = nexttile; 58 | end 59 | plotTrackPolygons_(track_merged, sprintf('2) Merged to %i Polygons - Relaxation Tolerance \\epsilon_A = %.4f m^2', length(track_merged.polygons), track_creation_scale*track_SCR_epsilon_area_tolerance)) 60 | 61 | % 3) add overlaps 62 | if verLessThan('matlab', '9.7') 63 | figure 64 | xlabel('x [m]'); ylabel('y [m]') 65 | else 66 | ax4 = nexttile; 67 | end 68 | plotTrackPolygons_(track_overlapped, sprintf('3) Overlapped %i Polygons', length(track_overlapped.polygons))) 69 | 70 | if ~verLessThan('matlab', '9.7') 71 | linkaxes([ax1 ax2 ax3 ax4],'xy') 72 | xlabel(t, 'x [m]'); ylabel(t, 'y [m]') 73 | end 74 | 75 | function plotTrackPolygons_(track, name) 76 | title(name) 77 | hold on 78 | 79 | % plot every polygon hull 80 | for i = 1:length(track.polygons) 81 | vertices = track.vertices(:, track.polygons(i).vertex_indices); 82 | [K, ~] = convhull(vertices'); 83 | vertices = vertices(:, K(:,1)); 84 | plot(vertices(1,:), vertices(2,:), '-'); 85 | end 86 | 87 | axis equal 88 | end 89 | end 90 | end 91 | end -------------------------------------------------------------------------------- /code/+sim/#README.m: -------------------------------------------------------------------------------- 1 | % contains all helper functions for simulation loop -------------------------------------------------------------------------------- /code/+sim/init_log.m: -------------------------------------------------------------------------------- 1 | function log = init_log(ws, cfg) 2 | % initialize logging structure on basis of given working set 3 | 4 | % Initialize logging structure 5 | fields = fieldnames(ws)'; 6 | fields(ismember(fields, 'vhs')) = []; 7 | fields{2,1} = {}; 8 | log.lap = struct(fields{:}); 9 | 10 | % copy structure from ws 11 | fields = fieldnames(ws.vhs{1})'; 12 | fields{2,1} = {}; 13 | for i = 1:length(cfg.scn.vhs) 14 | log.vehicles{i} = struct(fields{:}); % Struct to save all data for each lap 15 | end -------------------------------------------------------------------------------- /code/+sim/init_ws.m: -------------------------------------------------------------------------------- 1 | function ws = init_ws(cfg) 2 | % initialize working structure 3 | 4 | ws = struct; 5 | % vehicle-specfic working set 6 | for i = 1:length(cfg.scn.vhs) 7 | % controller-specifics 8 | ws.vhs{i}.controller_output = NaN; 9 | ws.vhs{i}.x_0 = cfg.scn.vhs{i}.x_start; 10 | ws.vhs{i}.x_0_next = NaN; 11 | ws.vhs{i}.x_sim = NaN; 12 | if cfg.scn.vhs{i}.isControlModelLinear && ~cfg.scn.vhs{i}.isSimulationModelLinear 13 | % convert states from Single Track to Linear 14 | ws.vhs{i}.x_0_controller = model.vehicle.state_st2lin(ws.vhs{i}.x_0); 15 | else 16 | ws.vhs{i}.x_0_controller = ws.vhs{i}.x_0; 17 | end 18 | 19 | try 20 | ws.vhs{i}.X_controller = cfg.scn.vhs{i}.X_controller_start; 21 | catch 22 | ws.vhs{i}.X_controller = repmat(ws.vhs{i}.x_0_controller, 1, cfg.scn.vhs{i}.p.Hp); 23 | end 24 | 25 | ws.vhs{i}.X_controller_prev = ws.vhs{i}.X_controller; 26 | ws.vhs{i}.U_controller = repmat([0;0], 1, cfg.scn.vhs{i}.p.Hp); 27 | ws.vhs{i}.u_1 = ws.vhs{i}.U_controller(:, 1); 28 | 29 | 30 | % lap-specific 31 | cp_x_0 = controller.track_SL.find_closest_checkpoint_index(... 32 | ws.vhs{i}.x_0(cfg.scn.vhs{i}.model_controller.idx_pos), cfg.scn.track_center); 33 | ws.vhs{i}.cp_prev = cp_x_0; 34 | ws.vhs{i}.cp_curr = cp_x_0; 35 | ws.vhs{i}.lap_count = 0; % start with 0 finished laps (-1 in case of vehicles start just short of finish line) 36 | ws.vhs{i}.pos = 0; % ego vehicle position relative to all other vehicles 37 | end 38 | 39 | % inter-vehicle/obstacle data 40 | % Initialize tables with indications for each vehicle (rows) which other 41 | % vehicle has to be considered as an obstacle (colums) or wich other 42 | % vehicle has to be blocked (columns) 43 | ws.obstacleTable = zeros(length(cfg.scn.vhs), length(cfg.scn.vhs)); 44 | ws.blockingTable = zeros(length(cfg.scn.vhs), length(cfg.scn.vhs)); 45 | 46 | % init data (in simulation: executed every round) 47 | ws = sim.update_administrative_data(cfg, ws); -------------------------------------------------------------------------------- /code/+sim/run.m: -------------------------------------------------------------------------------- 1 | function output_file = run(cfg) 2 | % Main file to run the scenario with any given configuration 3 | % 4 | % debug despite try/catch block: `dbstop if caught error`, disable with 5 | % `dbclear if error` 6 | % 7 | % Used Abbreviations 8 | % ws: abbreviation of working set, the struct which acts as "RAM". 9 | % Contains only current status; every step is saved in log 10 | % all variables are assumed to be simulation data, exceptions 11 | % are declared (e.g. controller internal data with postfix 12 | % "_controller") 13 | % cp: abbreviation of checkpoint, the points which define the track path 14 | % cfg: configuration 15 | % scn: scenario 16 | % vhs: vehicles 17 | 18 | %% Initialization 19 | timer_overall = tic; 20 | 21 | if verLessThan('matlab', '9.10') 22 | warning('This is an old MATLAB version. Not all features of the software will work') 23 | end 24 | 25 | % initialization of objects etc. 26 | cfg = config.init_config(cfg); 27 | 28 | % initialize working and logging structures 29 | step_sim = 0; 30 | ws = sim.init_ws(cfg); 31 | log = sim.init_log(ws, cfg); 32 | 33 | % init controls 34 | ctl_race_ongoing = true; % true until user ends or first vehicle finishes 35 | global ctl_abort ctl_pause 36 | init_control_keys(cfg.plot.plots_to_draw); 37 | 38 | %% Execute Control & Simulation Loop 39 | disp('########################') 40 | disp('#- Starting race loop -#') 41 | 42 | % try 43 | while ctl_race_ongoing 44 | fprintf('------------------------- Step %i -------------------------\n', step_sim); 45 | step_sim = step_sim + 1; 46 | timer_loop = tic; 47 | 48 | 49 | %% Controller Execution 50 | for i = 1:length(cfg.scn.vhs) 51 | %% Measuring 52 | % "measure reality" (simulation) to models 53 | % only required for linear control combined with single-track 54 | % simulation models 55 | if cfg.scn.vhs{i}.isControlModelLinear && ~cfg.scn.vhs{i}.isSimulationModelLinear 56 | % convert states from Single Track to Linear 57 | ws.vhs{i}.x_0_controller = model.vehicle.state_st2lin(ws.vhs{i}.x_0); 58 | else 59 | ws.vhs{i}.x_0_controller = ws.vhs{i}.x_0; 60 | end 61 | 62 | %% Controller Execution 63 | % prepare vehicle working set 64 | vhs = cell(1, length(ws.vhs)); 65 | for k = 1:length(ws.vhs) 66 | vhs{k}.x_0 = ws.vhs{k}.x_0_controller; 67 | vhs{k}.X_opt = ws.vhs{k}.X_controller; 68 | if k == i 69 | vhs{k}.U_opt = ws.vhs{k}.U_controller; 70 | end 71 | vhs{k}.cp_curr = ws.vhs{k}.cp_curr; 72 | end 73 | 74 | % save *raw* output 75 | ws.vhs{i}.controller_output = controller.run_SCP(cfg,... 76 | vhs, ws.obstacleTable, ws.blockingTable, i); 77 | 78 | % save payload (predicted trajectories) for easier access 79 | i_scp = find([ws.vhs{i}.controller_output.t_opt], 1, 'last'); 80 | ws.vhs{i}.X_controller = ws.vhs{i}.controller_output(i_scp).X_opt; 81 | ws.vhs{i}.U_controller = ws.vhs{i}.controller_output(i_scp).U_opt; 82 | ws.vhs{i}.u_1 = ws.vhs{i}.U_controller(:, 1); 83 | end 84 | 85 | 86 | %% Visualization 87 | % Visualization plots x_0 (current state) of last time step and 88 | % predictions of Hp states x calculated in the current time step. 89 | % In other words - the visualization takes place before the 90 | % simulation/application of the control inputs. 91 | if cfg.plot.is_enabled 92 | for i = 1:length(cfg.plot.plots_to_draw) 93 | cfg.plot.plots_to_draw{i}.plot(cfg, ws); 94 | end 95 | drawnow 96 | end 97 | 98 | %% Simulation 99 | % compute input response / advance to next state/x_0 100 | for i = 1:length(cfg.scn.vhs) 101 | % if only simulation for main vehicle ("equipped" with 102 | % multiple controllers), copy it's x_0 to other vehicles 103 | % (which are only to compare controllers at each step) 104 | if cfg.scn.is_main_vehicle_only && i > 1 105 | % don't simulate 106 | continue 107 | end 108 | 109 | isCtrLin = cfg.scn.vhs{i}.isControlModelLinear; 110 | isSimLin = cfg.scn.vhs{i}.isSimulationModelLinear; 111 | % three cases: 112 | % Controller Model Simulation Model 113 | % Linear Linear 114 | % Linear Single Track 115 | % Single Track Single Track 116 | if isSimLin && isCtrLin 117 | % equals to controller output `ws.vhs{i}.x_0 = ws.vhs{i}.controller_output.x(:,1)`; 118 | ws.vhs{i}.x_0_next = ws.vhs{i}.x_0 + cfg.scn.vhs{i}.model_simulation.ode(ws.vhs{i}.x_0, ws.vhs{i}.u_1); 119 | ws.vhs{i}.x_sim = nan; 120 | elseif ~isSimLin && isCtrLin 121 | [ws.vhs{i}.x_0_next,ws.vhs{i}.x_sim] = sim.simulate_ode(ws.vhs{i}.x_0, ws.vhs{i}.u_1, cfg.scn.vhs{i}); 122 | elseif ~isSimLin && ~isCtrLin 123 | [ws.vhs{i}.x_0_next,ws.vhs{i}.x_sim] = sim.simulate_ode(ws.vhs{i}.x_0, ws.vhs{i}.u_1, cfg.scn.vhs{i}); 124 | else 125 | % should never happen: why choose worse simulation model than controller? 126 | error('Combination of controller and simulation vehicle model types not supported') 127 | end 128 | end 129 | 130 | 131 | 132 | %% Log 133 | if cfg.log.level >= cfg.log.LOG 134 | % saving ws for vehicles seperately for easier access 135 | log.lap{end + 1} = rmfield(ws, 'vhs'); 136 | 137 | for i = 1:length(cfg.scn.vhs) 138 | log.vehicles{i}(end + 1) = ws.vhs{i}; 139 | end 140 | end 141 | 142 | 143 | %% Advance 144 | for i = 1:length(cfg.scn.vhs) 145 | % if only simulation for main vehicle ("equipped" with 146 | % multiple controllers), copy it's x_0 to other vehicles 147 | % (which are only to compare controllers at each step) 148 | if cfg.scn.is_main_vehicle_only && i > 1 149 | % copy trajectory of main vehicle to other controllers 150 | if ~cfg.scn.vhs{i}.isSimulationModelLinear 151 | ws.vhs{i}.x_0_next = ws.vhs{1}.x_0_next; 152 | ws.vhs{i}.X_controller = ws.vhs{1}.X_controller; 153 | else 154 | ws.vhs{i}.x_0_next = model.vehicle.state_st2lin(ws.vhs{1}.x_0_next); 155 | ws.vhs{i}.X_controller = model.vehicle.state_st2lin(ws.vhs{1}.X_controller); 156 | end 157 | end 158 | end 159 | 160 | % shift controller data acc. to simulation advance 161 | ws = controller.shift_prev_data(length(cfg.scn.vhs), ws); 162 | 163 | % updates adminstrative data of working set to current simulation 164 | % state 165 | ws = sim.update_administrative_data(cfg, ws); 166 | 167 | %% Execution control 168 | % Check if race finished (every vehicle reached n_laps finish line 169 | ctl_race_ongoing = false; 170 | for i = 1:length(cfg.scn.vhs) 171 | if ws.vhs{i}.lap_count < cfg.race.n_laps 172 | ctl_race_ongoing = true; 173 | break; 174 | end 175 | end 176 | 177 | %% enact user inputs 178 | if cfg.plot.is_enabled 179 | if ctl_pause; disp('Pause...'); end 180 | while true % pseudo do..while loop 181 | if ctl_abort % ..stop race 182 | disp('Aborted'); 183 | ctl_race_ongoing = false; 184 | break; 185 | end 186 | if ~ctl_pause; break; end 187 | pause(0.1) 188 | end 189 | end 190 | fprintf('Loop time %4.0fms\n', toc(timer_loop) * 1000) 191 | end 192 | % catch ME 193 | % warning('#- Error in race loop -#') 194 | % end 195 | disp('#- Ending race loop -#') 196 | disp('########################') 197 | fprintf('Overall loop time %.2fs\n', toc(timer_overall)) 198 | 199 | %% Save 200 | if cfg.log.level >= cfg.log.LOG 201 | % save all workspace variables, but not figures 202 | disp('Saving workspace') 203 | 204 | % remove figure handles, so they won't get saved 205 | cfg.plot.plots_to_draw = NaN; 206 | output_file = [cfg.outputPath '/log.mat']; 207 | save(output_file) 208 | end 209 | 210 | % if error occured in race loop 211 | if exist('ME', 'var') 212 | warning('Error in race loop ocurred, rethrowing:') 213 | rethrow(ME) 214 | end 215 | 216 | %% Evaluation 217 | % % Run example evaluation scipts 218 | % evaluation.plot_t_opts() 219 | % evaluation.plot_track_with_speed() 220 | 221 | % % export all currently open figures (especially for CodeOcean) 222 | % utils.exportAllFigures(cfg); 223 | % fprintf(2, 'Result files (graphics, figures, logs) were saved in "%s"\n', cfg.outputPath) 224 | end 225 | 226 | 227 | function init_control_keys(plots_to_draw) 228 | % initalize control keys on given plots 229 | global ctl_abort ctl_pause 230 | ctl_pause = false; 231 | ctl_abort = false; 232 | 233 | % Plot control: SPACE to pause, ESC to abort 234 | function key_press_callback(~,eventdata) 235 | if strcmp(eventdata.Key, 'escape') 236 | ctl_abort = true; 237 | elseif strcmp(eventdata.Key, 'space') 238 | ctl_pause = ~ctl_pause; 239 | end 240 | end 241 | 242 | % enable callbacks on every plot 243 | for i = 1:length(plots_to_draw) 244 | set(plots_to_draw{i}.figure_handle, ... 245 | 'WindowKeyPressFcn', @key_press_callback); 246 | end 247 | end -------------------------------------------------------------------------------- /code/+sim/simulate_ode.m: -------------------------------------------------------------------------------- 1 | function [x_0,x_sim] = simulate_ode(x_0, u_1, veh_cfg) 2 | % solve ode for individual vehicle with calculated inputs 3 | % constant control input across simulation step 4 | % 5 | % Inputs 6 | % x_0 current simulation's states 7 | % u_1 current control inputs 8 | % veh_cfg (struct) vehicle configuration 9 | % 10 | % Outputs 11 | % x_0 next simulation's states, based on input and last state 12 | 13 | % prevent division errors if low v_{long} 14 | if x_0(3) == 0 15 | x_0(3) = eps; 16 | end 17 | 18 | % time span [s] -> n+ time points including n time steps 19 | % CAVE actually, MATLAB's ODE solvers don't care about these steps 20 | dt_sim = 0:veh_cfg.p.dt_simulation:veh_cfg.p.dt_controller; 21 | 22 | % create function handle: varying states in sim, but keeping inputs constant 23 | % if simulation modell has controller: 24 | % use controller to translate between control output and sim input 25 | if ~ismethod(veh_cfg.model_simulation, 'controller') 26 | u_1_for_sim = u_1; 27 | function_sim = @(t, x_in_sim) veh_cfg.model_simulation.ode(x_in_sim, u_1_for_sim); 28 | 29 | % simulate 30 | [~, x_sim] = ode45(function_sim, dt_sim, x_0);%,... 31 | %odeset('RelTol',1e-8,'AbsTol',1e-8)); 32 | else % if model has controller 33 | % execute controller at dt's of simulation 34 | for i = 2:length(dt_sim) 35 | u_1_for_sim = veh_cfg.model_simulation.controller(x_0, u_1); 36 | function_sim = @(t, x_in_sim) veh_cfg.model_simulation.ode(x_in_sim, u_1_for_sim); 37 | 38 | % simulate 39 | % NOTE reduced accuracy due to fluctuations of model 40 | % to debug: 41 | % 1. call `figure` 42 | % 2. expand `odeset` with `'Stats','on','OutputFcn',@odeplot` 43 | [~, x_sim] = ode45(function_sim, [dt_sim(i - 1) dt_sim(i)], x_0);%,... 44 | %odeset('RelTol',1e-4,'AbsTol',1e-4)); 45 | end 46 | end 47 | 48 | % extract new x_0 49 | x_0 = x_sim(end,:)'; 50 | x_sim = x_sim(1:end-1,:)'; -------------------------------------------------------------------------------- /code/+sim/update_administrative_data.m: -------------------------------------------------------------------------------- 1 | function ws = update_administrative_data(cfg, ws) 2 | % updates administrative data of working set to current status of 3 | % simulation data 4 | % - current checkpoints, position, lap 5 | % - update blocking & obstacles tables 6 | 7 | %% Current CP, position and lap 8 | % Get current checkpoints (corresponding to x_0) and current laps 9 | for i = 1:length(cfg.scn.vhs) 10 | % update checkpoints 11 | cp_curr = controller.track_SL.find_closest_checkpoint_index(... 12 | ws.vhs{i}.x_0(cfg.scn.vhs{i}.model_controller.idx_pos), cfg.scn.track_center); 13 | ws.vhs{i}.cp_prev = ws.vhs{i}.cp_curr; 14 | ws.vhs{i}.cp_curr = cp_curr; 15 | 16 | % if new lap 17 | % CAVE may not be the most robust lap detection - depending on 18 | % vehicle speed & discretization 19 | if (ws.vhs{i}.cp_prev ~= ws.vhs{i}.cp_curr) && ... 20 | (ws.vhs{i}.cp_prev > 0.9 * length(cfg.scn.track)) && ... % was in last 10% of lap 21 | (ws.vhs{i}.cp_curr < 0.1 * length(cfg.scn.track)) % now is in first 10% of lap 22 | % advance lap counter 23 | ws.vhs{i}.lap_count = ws.vhs{i}.lap_count + 1; 24 | % printing in red 25 | fprintf(2, '########## Vehicle %i has finished lap %i ##########\n', i, ws.vhs{i}.lap_count) 26 | end 27 | end 28 | 29 | % Determine relative positions of racing vehicles corresponding to 30 | % current checkpoints and current laps 31 | for i = 1:length(cfg.scn.vhs) 32 | ws.vhs{i}.pos = 1; 33 | for j = 1:length(cfg.scn.vhs) 34 | if (ws.vhs{i}.cp_curr < ws.vhs{1,j}.cp_curr) && ... 35 | (ws.vhs{i}.lap_count == ws.vhs{1,j}.lap_count) || ... 36 | (ws.vhs{i}.lap_count < ws.vhs{1,j}.lap_count) 37 | ws.vhs{i}.pos = ws.vhs{i}.pos + 1; 38 | end 39 | end 40 | end 41 | 42 | %% Determine obstacle relationship 43 | % Set obstacle-matrix entry to 1 if vehicle of row has to respect 44 | % vehicle in column as an obstacle, depending on the relativ 45 | % positioning on the race track. 46 | CP_halfTrack = length(cfg.scn.track)/2; % get checkpoint index at half of the track 47 | for i = 1:length(cfg.scn.vhs) % ego vehicle 48 | for j = 1:length(cfg.scn.vhs) % opposing vehicles 49 | if (i ~= j) && (... 50 | ( (ws.vhs{i}.cp_curr < ws.vhs{1,j}.cp_curr) && ... 51 | ( (ws.vhs{1,j}.cp_curr - ws.vhs{i}.cp_curr) < CP_halfTrack ) ) || ... 52 | ( (ws.vhs{i}.cp_curr >= ws.vhs{1,j}.cp_curr) && ... 53 | ( (ws.vhs{i}.cp_curr - ws.vhs{1,j}.cp_curr) > CP_halfTrack ) ) || ... 54 | (ws.vhs{i}.cp_curr == ws.vhs{1,j}.cp_curr) ) 55 | ws.obstacleTable(i,j) = 1; 56 | else 57 | ws.obstacleTable(i,j) = 0; 58 | end 59 | end 60 | end 61 | 62 | %% Determine Defending Relationship -> Blocking 63 | for i = 1:length(cfg.scn.vhs) % ego vehicle 64 | for j = 1:length(cfg.scn.vhs) % opposing vehicles 65 | if (i ~= j) && ... 66 | ( norm(ws.vhs{i}.x_0(3:4)) < norm(ws.vhs{1,j}.x_0(3:4)) ) && ... 67 | ( ws.obstacleTable(i,j) == 0 ) && ... 68 | ( ( norm(ws.vhs{i}.x_0(1:2) - ws.vhs{1,j}.X_controller(1:2,1)) <= 0.1 ) )%|| ... 69 | % outcomment if future trajectory points should be considered 70 | %( norm(ws.vhs{i}.x_0(1:2) - ws.vhs{j}.X_controller(1:2, 2)) <= 0.3 ) || ... 71 | %( norm(ws.vhs{i}.x_0(1:2) - ws.vhs{j}.X_controller(1:2, 3)) <= 0.3 ) ) 72 | ws.blockingTable(i,j) = 1; 73 | else 74 | ws.blockingTable(i,j) = 0; 75 | end 76 | end 77 | end -------------------------------------------------------------------------------- /code/+utils/+poly/cleanse_convex_polygon.m: -------------------------------------------------------------------------------- 1 | function vertices = cleanse_convex_polygon(vertices) 2 | % order vertices, remove unneccesary vertices 3 | select_and_reorder_indices = convhull(vertices); 4 | select_and_reorder_indices = select_and_reorder_indices(1:end-1); 5 | vertices = vertices(select_and_reorder_indices, :); 6 | end -------------------------------------------------------------------------------- /code/+utils/+poly/con2vert.m: -------------------------------------------------------------------------------- 1 | function [V] = con2vert(A,b) 2 | % Convert 2D convex polygon from half-space representation to vertex 3 | % representation. 4 | % 5 | % inspired by 'poly.con2vert' from Matlab File Exchange by Michael Kleder 6 | 7 | assert(size(A,1) == size(b,1)); 8 | assert(size(A,2) == 2); 9 | assert(size(b,2) == 1); 10 | 11 | V = zeros(0,2); 12 | for C = nchoosek(1:length(b),2)' 13 | if abs(det(A(C,:))) > 1e-10 14 | x = A(C,:)\b(C); 15 | if all(A*x-b < 1e-5) 16 | V = [V; x']; 17 | end 18 | end 19 | end 20 | 21 | end -------------------------------------------------------------------------------- /code/+utils/+poly/enlarge_polygon_into_forward_direction.m: -------------------------------------------------------------------------------- 1 | function p_enlarged = enlarge_polygon_into_forward_direction(i_p, track, track_scale, direction_is_forward, debug_) 2 | % enlarge polygon into direction of shared edge with forward neighbour 3 | % 4 | % Inputs 5 | % i_p: index of polygon to be expanded 6 | n_vertices_p = length(track.polygons(i_p).vertex_indices); 7 | 8 | % shared vertices with forward neighbour 9 | if direction_is_forward 10 | i_f = utils.mod1(i_p + 1, length(track.polygons)); % next forward polygon 11 | else 12 | i_f = utils.mod1(i_p - 1, length(track.polygons)); % next forward polygon 13 | end 14 | n_vertices_f = length(track.polygons(i_f).vertex_indices); 15 | [i_shared_vertices_f_index, ~] = find(repmat(track.polygons(i_p).vertex_indices, n_vertices_f, 1) == repmat(track.polygons(i_f).vertex_indices', 1, n_vertices_p)); 16 | i_shared_vertices_f = track.polygons(i_f).vertex_indices(i_shared_vertices_f_index); 17 | assert(numel(i_shared_vertices_f) == 2, "number of shared vertices don't match, check your track creation!") 18 | shared_point_f_1 = track.vertices(:, i_shared_vertices_f(1)); 19 | shared_point_f_2 = track.vertices(:, i_shared_vertices_f(2)); 20 | if debug_; utils.poly.plot_vertices(utils.poly.get_track_polygon_vertices(i_p, track)'); utils.poly.plot_vertices(utils.poly.get_track_polygon_vertices(i_f, track)'); end 21 | 22 | %% find shared (opposite) constraints 23 | % forward 24 | b_p_f = create_b_of_edge(shared_point_f_1, shared_point_f_2); 25 | i_shared_constraints_p_f = find(abs(abs(track.polygons(i_p).b) - abs(b_p_f)) < 1e-10 * track_scale); 26 | assert(numel(i_shared_constraints_p_f) == 1, "number of vertices in constraint representation don't match! Please investigate"); 27 | 28 | % just double check that neighbour has same amount of shared constraints 29 | i_shared_constraints_f_p = find(abs(abs(track.polygons(i_f).b) - abs(b_p_f)) < 1e-10 * track_scale); 30 | assert(numel(i_shared_constraints_f_p) == 1, "number of vertices in constraint representation don't match! Please investigate") 31 | 32 | %% expand current polygon into direction of shared edge 33 | % creating pseudo polygon with effectively shared edges expanded 34 | % that far that they become ineffective 35 | % we can use the same index as polygon was converted to 36 | % constraint representation with same function --> must be 37 | % determinstically the same index order 38 | b_enlarged_p = track.polygons(i_p).b; 39 | % FIXME change to max track diagonal dimension 40 | b_enlarged_p(i_shared_constraints_p_f) = 5000*track_scale; 41 | 42 | %% convert back to vertices representation 43 | p_enlarged = utils.poly.con2vert(track.polygons(i_p).A, b_enlarged_p); 44 | %if debug_; plot_polygon(p_enlarged, ':'); end 45 | assert(~isempty(p_enlarged), 'Some error in polygon enlarging happened! Please investigate') 46 | end 47 | 48 | function b = create_b_of_edge(shared_point_1, shared_point_2) 49 | % create b of given line segment 50 | n = [0 -1; 1 0] * (shared_point_1 - shared_point_2); 51 | n = n ./ norm(n); 52 | b = n' * shared_point_1; 53 | end -------------------------------------------------------------------------------- /code/+utils/+poly/get_track_polygon_vertices.m: -------------------------------------------------------------------------------- 1 | function p_vert = get_track_polygon_vertices(i, track) 2 | % get vertices by index 3 | p_vert = track.vertices(:, track.polygons(i).vertex_indices)'; 4 | end -------------------------------------------------------------------------------- /code/+utils/+poly/plot_vertices.m: -------------------------------------------------------------------------------- 1 | function handle = plot_vertices(vertices, LineSpec, varargin) 2 | % plot vertices as polygon 3 | if nargin < 2; LineSpec = '-'; end 4 | %plot([vertices(1, :) vertices(1, 1)], [vertices(2, :) vertices(2, 1)], LineSpec) 5 | handle = plot(vertices(1, :), vertices(2, :), LineSpec, varargin{:}); 6 | end -------------------------------------------------------------------------------- /code/+utils/+poly/poly2vert.m: -------------------------------------------------------------------------------- 1 | function p_vert = poly2vert(p_poly) 2 | % convert MATLAB's polyshape to vertices 3 | p_vert = p_poly.Vertices'; 4 | end -------------------------------------------------------------------------------- /code/+utils/+poly/vert2con.m: -------------------------------------------------------------------------------- 1 | function [A,b] = vert2con(V) 2 | % Convert 2D convex polygon from vertex representation to half-space 3 | % representation. 4 | % 5 | % inspired by 'poly.vert2con' from Matlab File Exchange by Michael Kleder 6 | 7 | assert(size(V,2) == 2); 8 | 9 | K = convhull(V); 10 | A = nan(length(K)-1,2); 11 | b = nan(length(K)-1,1); 12 | for i = 1:(length(K)-1) 13 | n = [0 1;-1 0]*(V(K(i+1),:) - V(K(i),:))'; 14 | x0 = V(K(i),:); 15 | b(i) = x0*n; 16 | A(i,:) = n'; 17 | end 18 | 19 | % normalize 20 | L = sqrt(sum(A.^2,2)); 21 | A = A ./ repmat(L,1,size(A,2)); 22 | b = b ./ L; 23 | end -------------------------------------------------------------------------------- /code/+utils/+poly/vert2poly.m: -------------------------------------------------------------------------------- 1 | function p_poly = vert2poly(p_vert) 2 | % convert from vertice representation to MATLAB's polyshape 3 | 4 | % convert to correctly ordered polygon (current vertices are unordered) 5 | p_vert = utils.poly.cleanse_convex_polygon(p_vert); 6 | %if debug_; plot_polygon(p_vert); end 7 | % ... so that we can convert to MATLAB's poly 8 | p_poly = polyshape(p_vert(:, 1), p_vert(:, 2)); 9 | %if debug_; plot(p_poly); end 10 | end -------------------------------------------------------------------------------- /code/+utils/PID.m: -------------------------------------------------------------------------------- 1 | function [u_out, I, error] = PID(K_p, K_i, K_d, dt, u_ref, u_actual, I, error_prev) 2 | % stateless PID controller in parallel form 3 | 4 | error = u_ref - u_actual; 5 | 6 | % integrate & differntiate 7 | I = I + error * dt; 8 | derivative = (error - error_prev) / dt; 9 | 10 | % PID Equation 11 | u_out = K_p * error + K_i * I + K_d * derivative; 12 | end -------------------------------------------------------------------------------- /code/+utils/exportAllFigures.m: -------------------------------------------------------------------------------- 1 | function exportAllFigures(cfg) 2 | % save all figures as PDF to results folder 3 | all_figures = findall(groot,'Type','figure'); 4 | for i = 1:length(all_figures) 5 | filename = ['Figure ' num2str(all_figures(i).Number) ' - ' all_figures(i).Name]; 6 | filename = utils.sanitizeFilename(filename); 7 | 8 | % save figure 9 | utils.exportFigure(all_figures(i).Number, [cfg.outputPath filename]); 10 | end -------------------------------------------------------------------------------- /code/+utils/exportFigure.m: -------------------------------------------------------------------------------- 1 | function exportFigure(figure_number, filename) 2 | % save given figure under given path (without file extension) as landscape 3 | % PDF and MATLAB fig 4 | set(figure_number, 'PaperOrientation', 'landscape'); 5 | print(['-f' num2str(figure_number)], [filename '.pdf'], '-dpdf', '-bestfit'); 6 | 7 | warning off % gives redundant warning about using savefig 8 | savefig(figure_number, [filename '.fig'], 'compact'); 9 | warning on -------------------------------------------------------------------------------- /code/+utils/getRwthColors.m: -------------------------------------------------------------------------------- 1 | function colors = getRwthColors(percent) 2 | %COLOR SELECTION: this function defines a selection of colors for the 3 | % plotting of multiple vehicles corresponding to the corporate design of 4 | % RWTH Aachen University. The colors are defined as RGB-triplets. 5 | colors = zeros(8,3); 6 | 7 | if percent == 100 % 100% colors 8 | colors(1,:) = [246 168 0]; % orange 9 | colors(2,:) = [87 171 39]; % green 10 | colors(3,:) = [204 7 30]; % red 11 | colors(4,:) = [0 97 101]; % petrol 12 | colors(5,:) = [97 33 88]; % violet 13 | colors(6,:) = [87 171 39]; % turquoise 14 | colors(7,:) = [0 84 159]; % blue 15 | elseif percent == 50 % 50% colors 16 | colors(1,:) = [253 212 143]; % orange 17 | colors(2,:) = [184 214 152]; % green 18 | colors(3,:) = [230 150 121]; % red 19 | colors(4,:) = [125 164 167]; % petrol 20 | colors(5,:) = [168 133 158]; % violet 21 | colors(6,:) = [137 204 207]; % turquoise 22 | colors(7,:) = [142 186 229]; % blue 23 | end 24 | 25 | colors = colors / 255; % Get RGB triplet in values from 0 to 1 26 | end -------------------------------------------------------------------------------- /code/+utils/isToolboxAvailable.m: -------------------------------------------------------------------------------- 1 | function bool = isToolboxAvailable(toolbox_name) 2 | % check if MATLAB toolbox with given name is available 3 | bool = any(any(contains(struct2cell(ver), toolbox_name))); -------------------------------------------------------------------------------- /code/+utils/mod1.m: -------------------------------------------------------------------------------- 1 | function y = mod1(i, N) 2 | % Modulo for one-based indices: offsetting modulo to work with Matlab's 3 | % indexing (start is 1, not 0) 4 | y = mod(i-1, N) + 1; 5 | end -------------------------------------------------------------------------------- /code/+utils/rat2str.m: -------------------------------------------------------------------------------- 1 | function str = rat2str(rat_) 2 | % create string version of rational 3 | str = regexprep(rats(rat_), '\s+', ''); 4 | end -------------------------------------------------------------------------------- /code/+utils/removePiPeriodicity.m: -------------------------------------------------------------------------------- 1 | function pis = removePiPeriodicity(pis) 2 | % removes periodicity of given vector of pis and moves into range [-pi, pi] 3 | 4 | % remove periodicity 5 | pis = mod(pis, 2*pi); 6 | % move into range [-pi, pi] 7 | pis(pis > pi) = pis(pis > pi) - 2*pi; -------------------------------------------------------------------------------- /code/+utils/sanitizeFilename.m: -------------------------------------------------------------------------------- 1 | function filename = sanitizeFilename(filename) 2 | % allow only alphanumeric characters, replace others with underscore 3 | indices_invalid = regexp(filename, '[^a-zA-Z0-9 -]'); 4 | filename(indices_invalid) = '_'; -------------------------------------------------------------------------------- /code/+utils/set_figure_properties.m: -------------------------------------------------------------------------------- 1 | function set_figure_properties(figHandle, preset, paperheight_in) 2 | % SET_FIGURE_PROPERTIES Set Properties used for figures based on the type of export. 3 | 4 | switch lower(preset) 5 | case 'paper' % \the\linewidth=252.0pt, 1pt=0.3515mm --> 88.578mm 6 | fontsize = 6; 7 | paperwidth = 8; % picture width in cm 8 | paperheight = 4; % picture height in cm 9 | % paperwidth = 8.8; % picture width in cm 10 | % paperheight = 4.4; % picture height in cm 11 | linewidth=0.5; 12 | fontname = 'CMU Serif'; 13 | units = 'centimeters'; 14 | 15 | case 'presentation' 16 | fontsize = 18; 17 | paperwidth = 31.77; % picture width in cm 18 | paperheight = 14.01; % picture height in cm 19 | linewidth=1; 20 | fontname = 'Arial'; 21 | units = 'centimeters'; 22 | 23 | case 'document' 24 | fontsize = 9; 25 | paperwidth = 15.7; % picture width in cm 26 | paperheight = 7.85; % picture height in cm 27 | linewidth=0.5; 28 | fontname = 'CMU Serif'; 29 | units = 'centimeters'; 30 | 31 | case 'video' 32 | fontsize = 20; 33 | paperwidth = 1920; 34 | paperheight = 1080; 35 | linewidth = 1; 36 | fontname = 'Arial'; 37 | units = 'pixels'; 38 | 39 | otherwise % default 40 | error('No valid preset selected.') 41 | end 42 | if nargin == 3 43 | paperheight = paperheight_in; 44 | end 45 | 46 | % beauty corrections 47 | allchildren = get(figHandle, 'Children'); % get handle figure 48 | for a=1:length(allchildren) 49 | try % set title handle 50 | h_title=get(allchildren(a),'Title'); 51 | set(h_title,... 52 | 'FontWeight','normal',... 53 | 'FontSize',fontsize+1,... 54 | 'FontName',fontname,... 55 | 'Interpreter','latex'); 56 | catch 57 | % continue 58 | end 59 | try % redefine x- and y-labels 60 | h_xlabel = get(allchildren(a), 'xlabel'); 61 | h_ylabel = get(allchildren(a), 'ylabel'); 62 | set(h_xlabel,... 63 | 'FontSize',fontsize,... 64 | 'FontName',fontname,... 65 | 'Interpreter','latex') 66 | set(h_ylabel,... 67 | 'FontSize',fontsize,... 68 | 'FontName',fontname,... 69 | 'Interpreter','latex') 70 | catch 71 | % continue 72 | end 73 | % set axes 74 | try 75 | h_axes=get(allchildren(a),'Axes'); 76 | set(h_axes,... 77 | 'FontSize',fontsize,... 78 | 'FontName',fontname,... 79 | 'LineWidth',linewidth, ... 80 | 'Box','on'); 81 | catch 82 | % continue 83 | end 84 | % set subplotaxes 85 | try 86 | set(allchildren(a)... 87 | ,'FontSize',fontsize... 88 | ,'FontName',fontname... 89 | ,'LineWidth',linewidth... 90 | ,'Box','on'... 91 | ); 92 | % ,'XAxisLocation','origin'... 93 | % ,'YAxisLocation','origin'... 94 | catch 95 | % continue 96 | end 97 | % set legend 98 | if strcmpi(get(allchildren(a),'Tag'),'legend') 99 | h_legend=allchildren(a); 100 | if isequal(get(h_legend,'Interpreter'),'none') 101 | set(h_legend,'FontSize',fontsize+1) 102 | else 103 | set(h_legend,'FontSize',fontsize) 104 | end 105 | set(h_legend,... 106 | 'LineWidth',linewidth,... 107 | 'FontName',fontname,... 108 | 'Interpreter','latex',... 109 | 'Box','on'); 110 | end 111 | % Set graphic objects 112 | 113 | h_graphics = get(allchildren(a),'Children'); 114 | for h_graphic = h_graphics' 115 | try 116 | set(h_graphic ... 117 | ,'LineWidth', linewidth ... 118 | ); 119 | catch 120 | % continue 121 | end 122 | end 123 | end 124 | 125 | % background color 126 | set(figHandle, 'Color', 'w'); 127 | % format 128 | set(figHandle,'Units',units); 129 | screenpos = get(figHandle,'Position'); 130 | set(figHandle ... 131 | ,'Position',[screenpos(1:2), paperwidth, paperheight]... % px, py, w, h, of figure on screen 132 | ); 133 | % 134 | % if ~strcmp(preset, 'video') 135 | % % Make axes span whole window 136 | % ax = get(figHandle,'CurrentAxes'); 137 | % outerpos = ax.OuterPosition; 138 | % ti = ax.TightInset; 139 | % left = outerpos(1) + ti(1); 140 | % bottom = outerpos(2) + ti(2); 141 | % ax_width = outerpos(3) - ti(1) - ti(3) - 2e-3; %box was sometimes cut off 142 | % ax_height = outerpos(4) - ti(2) - ti(4); 143 | % ax.Position = [left bottom ax_width ax_height]; 144 | % set(figHandle,'PaperUnits',units) 145 | % set(figHandle ... 146 | % ,'PaperSize',[paperwidth, paperheight] ... 147 | % ,'PaperPosition',[0, 0, paperwidth, paperheight] ... 148 | % ); 149 | % end 150 | 151 | end -------------------------------------------------------------------------------- /code/+utils/yline_compatiblity.m: -------------------------------------------------------------------------------- 1 | function yline_compatiblity(yvalues, LineSpec) 2 | % compatibility abstraction for old MATLAB versions 3 | if verLessThan('matlab', '9.10') 4 | % seperate eacht data point 5 | for yvalue = yvalues 6 | yline(yvalue, LineSpec); 7 | end 8 | else 9 | yline(yvalues, LineSpec); 10 | end -------------------------------------------------------------------------------- /code/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016-2021 Bassam Alrifaee 4 | Copyright (c) 2017 Janis Maczijewski 5 | Copyright (c) 2021 Theodor Mario Henneken 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. -------------------------------------------------------------------------------- /code/run: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # 3 | # Run file for CodeOcean.com 4 | set -ex 5 | 6 | # This is the master script for the capsule. When you click "Reproducible Run", the code in this file will execute. 7 | matlab -nodisplay -r "addpath(genpath('.')); evaluation.paper()" -------------------------------------------------------------------------------- /code/run.m: -------------------------------------------------------------------------------- 1 | function scenarios = run(~) 2 | % main run file & examples how to use this software 3 | 4 | %% Initial Remarks 5 | % when running this software the first time, some simulation data needs to 6 | % be prepared -> don't be confused by the outputs and warnings 7 | clc 8 | clear 9 | % get scenario independent cfg 10 | cfg = config.config(); 11 | % default scenario (not runable) 12 | scenarios(1) = config.base_scenario(cfg); 13 | 14 | %% Scenarios 15 | % some preconfigured scenarios are provided in the following. 16 | % just cascade scenario options to your like, as done below 17 | 18 | % SCR 19 | % one vehicle only 20 | scenarios(4) = config.scenario_1_vehicle(scenarios(1), @(cfg_veh) config.vehicle_SCR(config.vehicle_ST_Liniger(cfg_veh))); 21 | scenarios(6) = config.scenario_1_vehicle(scenarios(1), @(cfg_veh) config.vehicle_SCR(config.vehicle_lin_Liniger(cfg_veh))); 22 | % ... with some (2) laps 23 | scenarios(7) = config.scenario_n_laps_race(scenarios(4), 2); 24 | scenarios(9) = config.scenario_n_laps_race(scenarios(6), 2); 25 | 26 | % SL 27 | % one vehicle only 28 | scenarios(10) = config.scenario_1_vehicle(scenarios(1), @config.vehicle_ST_Liniger); 29 | scenarios(12) = config.scenario_1_vehicle(scenarios(1), @config.vehicle_lin_Liniger); 30 | % ... and with endless race 31 | scenarios(13) = config.scenario_endless_race(scenarios(10)); 32 | scenarios(15) = config.scenario_endless_race(scenarios(12)); 33 | % ... and with reduced checkpoints 34 | scenarios(16) = config.scenario_SL_reduce_checkpoints(scenarios(13)); 35 | scenarios(18) = config.scenario_SL_reduce_checkpoints(scenarios(15)); 36 | % actual race situations 37 | scenarios(19) = config.scenario_race_various_vehicles(scenarios(1)); 38 | 39 | % different controller per vehicle 40 | scenarios(21) = config.scenario_lin_vs_ST(scenarios(1)); 41 | scenarios(22) = config.scenario_SL_vs_SCR(scenarios(1)); 42 | scenarios(23) = config.scenario_SCR_vs_SL(scenarios(1)); 43 | % ... as endless race 44 | scenarios(26) = config.scenario_endless_race(scenarios(21)); 45 | scenarios(27) = config.scenario_endless_race(scenarios(22)); 46 | 47 | % only one main vehicle, of which state various controllers are calculated 48 | % and displayed overlayed 49 | scenarios(30) = config.scenario_main_vehicle_lin_vs_ST(scenarios(1)); 50 | scenarios(31) = config.scenario_main_vehicle_SL_vs_SCR(scenarios(1)); 51 | 52 | % previous paper comparion 53 | scenarios(40) = config.scenario_paper_SL(scenarios(1)); 54 | 55 | %% Run Selected Scenario 56 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 57 | % select scneario by changing the indexnumber % 58 | % % 59 | % here || % 60 | % vv % 61 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 62 | if nargin == 0 % default: run only selected scenario 63 | output_file = sim.run(scenarios(22)); 64 | 65 | % Loding saved file after finished scenario 66 | fprintf(2, 'Loaded output file of sim, so you can use individual evaluation scripts or replay, if desired\n') 67 | evalin('base', sprintf('load(''%s'')', output_file)); 68 | end -------------------------------------------------------------------------------- /code/run_all.m: -------------------------------------------------------------------------------- 1 | % running all pre-configured scnearios from main `run`-file 2 | 3 | scenarios = run(true); 4 | 5 | for i = 10:length(scenarios) 6 | fprintf('Executing scenario %i', i); 7 | 8 | % recreate figure handles 9 | cfg.plot.plots_to_draw = config.config().plot.plots_to_draw; 10 | 11 | scenario = scenarios(i); 12 | if ~isempty(scenario.scn) 13 | if scenario.race.n_laps > 10 % limit race length 14 | scenario.race.n_laps = 2; 15 | end 16 | output_file = sim.run(scenario); 17 | end 18 | end -------------------------------------------------------------------------------- /code/run_parameter_study.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | 4 | %% [CONFIG] Parameter to Change, Vehicle & Scenario 5 | cfg_default = config.scenario_1_vehicle(... 6 | config.base_scenario(config.config()),... 7 | @config.vehicle_ST_Liniger); 8 | % SCR lap time: 10.7, 2 laps 20.5 -> delta = 9.8 9 | % mit korrigierter polygon findung: lap time: 10.5 10 | % SL lap time: 11.8, 2 laps 21.5 -> delta = 9.7 11 | 12 | % choose SCR 13 | %cfg_default.scn.vhs{1}.approximation = 20; 14 | 15 | % current time-lap-optimal params 16 | cfg_vh.p.Q = 1.5; % or even more?2.6 for maximum aggressive but close to instable; % weight for maximization of position on track 17 | cfg_vh.p.R = diag([90 90]); % 41 for maximum aggrasion: 14.6s. 90: 15.1 % weight for control changes over time 18 | cfg_vh.p.trust_region_size = 1.6; % [m] adds/subtracts to position (SL only) 19 | 20 | % e.g. Q R, trust_region_size 21 | parameter_name = 'trust_region_size'; % field in `scenario_default.scn.vhs{1}.p` 22 | % adapt below in code in case of arrays to process arbitrary values like arrays 23 | % create parameter variation 24 | parameter_variation_factors = 0.5:0.1:1.8; 25 | for i = 1:length(parameter_variation_factors) 26 | if strcmp(parameter_name, 'R') % for arrays 27 | % first tune whole R 28 | parameter_variation(i).parameter = parameter_variation_factors(i) .* diag([1 1]); %#ok 29 | % then tune individual entries 30 | % parameter_variation(i).parameter = diag([30 parameter_variation_factors(i)]); %#ok 31 | else % for scalar 32 | parameter_variation(i).parameter = parameter_variation_factors(i); %#ok 33 | end 34 | end 35 | 36 | %% Parameter Iteration Loop 37 | 38 | % make sure only running one lap 39 | cfg_default.race.n_laps = 1; 40 | 41 | % preallocate 42 | results(length(parameter_variation)).lap_time = []; 43 | results(length(parameter_variation)).parameter = []; 44 | 45 | for i = 1:length(parameter_variation) 46 | % change param, store, and run sim 47 | results(i).parameter = parameter_variation(i).parameter; 48 | cfg = cfg_default; 49 | cfg.scn.vhs{1}.p = setfield(cfg_default.scn.vhs{1}.p, parameter_name, parameter_variation(i).parameter); %#ok 50 | try 51 | output_file = sim.run(cfg); 52 | 53 | %% store current lap time for parameter 54 | % load results of current simulation 55 | sim_result = load(output_file) %#ok 56 | fprintf('Current parameter iteration %i with parameter %.2f\n', i, parameter_variation(i).parameter) 57 | 58 | % calculate lap_time (depending on x_start and track's finish line): 59 | % actually 1 lap + a fraction of a lap 60 | % just using length of indices, as made sure that only 1 lap is run 61 | % in scenario 62 | results(i).lap_time = length(sim_result.log.vehicles{1}) * cfg.scn.vhs{1}.p.dt_controller; 63 | catch me % in case of error in simulation due to bad parameter choice 64 | results(i).lap_time = 0; 65 | disp(me) 66 | warning('Error in simulation, setting lap time to 0') 67 | end 68 | 69 | %% Plot Results 70 | figure_number = 1001; 71 | figure(figure_number) 72 | clf(figure_number) 73 | hold on 74 | set(gcf, 'Name', ['Parameter Study: Parameter ' parameter_name ' vs. Lap Time']) 75 | if isscalar(results(1).parameter) % if scalar 76 | scatter([results(1:i).parameter], [results(1:i).lap_time], 'filled') 77 | plot([results(1:i).parameter], [results(1:i).lap_time]) 78 | xlabel(['Parameter ' parameter_name], 'Interpreter', 'none'); ylabel('Lap Time [s]'); 79 | else % if arrays etc.: show index 80 | scatter(1:i, [results(1:i).lap_time], 'filled') 81 | plot(1:i, [results(1:i).lap_time]) 82 | xlabel(['Index of Parameter ' parameter_name], 'Interpreter', 'none'); ylabel('Lap Time [s]'); 83 | end 84 | 85 | drawnow 86 | end 87 | 88 | 89 | % export figure (in parameter study folder) 90 | outputPathStudy = [cfg_default.outputPath '../Parameter Study/']; 91 | if ~isfolder(outputPathStudy); mkdir(outputPathStudy); end 92 | utils.exportFigure(figure_number, [outputPathStudy cfg_default.startTimeStr]); -------------------------------------------------------------------------------- /code/run_replay.m: -------------------------------------------------------------------------------- 1 | %% Replay Simulation 2 | %% How to 3 | % 1. load a "log.mat" from the output directory into workspace 4 | % 2. run script 5 | 6 | % recreate figure handles 7 | cfg.plot.plots_to_draw = config.config().plot.plots_to_draw; 8 | % re-init config (so that objects & figure handles are present again) 9 | cfg = config.init_config(cfg); 10 | 11 | %% Visualization 12 | disp('run plotting') 13 | warning('replaying at real-time speed of vehicle 1 (other vehicles may have other discretization time dt and thus a woring speed)') 14 | 15 | for j = 1:length(log.lap) 16 | tic 17 | % reconstruct ws from log 18 | ws = log.lap{j}; 19 | for i = 1:length(cfg.scn.vhs) 20 | ws.vhs{i} = log.vehicles{i}(j); 21 | end 22 | 23 | for i = 1:length(cfg.plot.plots_to_draw) 24 | cfg.plot.plots_to_draw{i}.plot(cfg, ws); 25 | end 26 | drawnow 27 | 28 | if j == 1 29 | fprintf(2, 'start replay with press of button\n') 30 | pause 31 | end 32 | 33 | % pause to simulate real time 34 | % CAVE replaying at real-time of vehicle 1 35 | delta = cfg.scn.vhs{1}.p.dt_controller - toc; 36 | if delta > 0 37 | pause(delta); 38 | elseif j > 1 39 | warning('Replay is currently not real-time') 40 | end 41 | end 42 | 43 | disp('replay finished') -------------------------------------------------------------------------------- /data/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016-2021 Bassam Alrifaee 4 | Copyright (c) 2017 Janis Maczijewski 5 | Copyright (c) 2021 Theodor Mario Henneken 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. -------------------------------------------------------------------------------- /data/singleTrackAMax.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/data/singleTrackAMax.mat -------------------------------------------------------------------------------- /environment/Dockerfile: -------------------------------------------------------------------------------- 1 | # hash:sha256:eb133fa6ef390c8a024c045858d448a3c7a34f140ef89a10c693182c8e4e31c7 2 | FROM registry.codeocean.com/codeocean/matlab:2020b-ubuntu20.04 3 | 4 | ARG DEBIAN_FRONTEND=noninteractive 5 | ARG MLM_LICENSE_FILE 6 | -------------------------------------------------------------------------------- /environment/README.txt: -------------------------------------------------------------------------------- 1 | this folders contains configuration files for CodeOcean.com -------------------------------------------------------------------------------- /metadata/README.txt: -------------------------------------------------------------------------------- 1 | this folder contains metadata for CodeOcean -------------------------------------------------------------------------------- /metadata/metadata.yml: -------------------------------------------------------------------------------- 1 | metadata_version: 1 2 | name: Sequential Convex Programming Methods for Real-time Optimal Trajectory Planning 3 | in Autonomous Vehicle Racing 4 | description: Optimization problems for trajectory planning in autonomous vehicle racing 5 | are characterized by their nonlinearity and nonconvexity. Instead of solving these 6 | optimization problems, usually a convex approximation is solved instead to achieve 7 | a high update rate. We present a real-time-capable model predictive control (MPC) 8 | trajectory planner based on a nonlinear single-track vehicle model and Pacejka's 9 | magic tire formula for autonomous vehicle racing. After formulating the general 10 | nonconvex trajectory optimization problem, we form a convex approximation using 11 | sequential convex programming (SCP). The state of the art convexifies track constraints 12 | using sequential linearization (SL), which is a method of relaxing the constraints. 13 | Solutions to the relaxed optimization problem are not guaranteed to be feasible 14 | in the nonconvex optimization problem. We propose sequential convex restriction 15 | (SCR) as a method to convexify track constraints. SCR guarantees that resulting 16 | solutions are feasible in the nonconvex optimization problem. We show recursive 17 | feasibility of solutions to the restricted optimization problem. The MPC is evaluated 18 | on a scaled version of the Hockenheimring racing track in simulation. The results 19 | show that an MPC using SCR yields faster lap times than an MPC using SL, while still 20 | being real-time capable. 21 | tags: 22 | - autonomous-vehicle 23 | - autonomous-vehicle-racing 24 | - discretization 25 | - scp 26 | - sequential-convex-programming 27 | - sqp 28 | - sequential-quadratic-programming 29 | authors: 30 | - name: Patrick Scheffe 31 | affiliations: 32 | - name: Chair of Embedded Software, RWTH Aachen University, Germany 33 | - name: Theodor Mario Henneken 34 | affiliations: 35 | - name: Chair of Embedded Software, RWTH Aachen University, Germany 36 | - name: Maximilian Kloock 37 | affiliations: 38 | - name: Chair of Embedded Software, RWTH Aachen University, Germany 39 | - name: Bassam Alrifaee 40 | affiliations: 41 | - name: Chair of Embedded Software, RWTH Aachen University, Germany 42 | corresponding_contributor: 43 | name: Patrick Scheffe 44 | email: scheffe@embedded.rwth-aachen.de 45 | -------------------------------------------------------------------------------- /results/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/embedded-software-laboratory/sequential-convex-programming/468314265ec4ca1e3e77777511abef66fd94901d/results/.gitkeep --------------------------------------------------------------------------------