full code release

README.md

@@ -12,18 +12,14 @@ This repository is the official implementation of the [paper](https://scholar.go
 > [Manabu Tsukada](https://scholar.google.com/citations?user=GSfYxPIAAAAJ&hl=en),
 > and [Marco Caccamo](https://scholar.google.com/citations?user=Jbo1MqwAAAAJ&hl=en&oi=ao).
 
-The paper is under review for possible publication. If you find our work useful, please consider [citing](#reference) it.
-
-**Note**: The <u>full code</u> will be made available <u>after publication</u>.
+Accepted at: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2024.
 
 <p align="center">
   <img src="docs/anim_1.gif" alt="Replicated real-world racetracks." width="200" />
-  <img src="docs/anim_2.gif" alt="Replicated real-world racetracks." width="200" />
+  <img src="docs/anim_2.gif" [main.py](../icra_2023/main.py)alt="Replicated real-world racetracks." width="200" />
   <img src="docs/anim_3.gif" alt="Replicated real-world racetracks." width="200" />
 </p>
 
-
-
 ## Table of contents
 - [Background](#background)
 - [Install](#install)
@@ -84,7 +80,7 @@ python main.py
 ```
 The use of your GPU can be avoided by running:
 ```bash
-python main.py cuda=cpu
+python main.py device=cpu
 ```
 ### Rendering
 Rendering can be enabled by setting running:

configs/config.yaml

@@ -0,0 +1,40 @@
+# Standard Config
+defaults:
+  - maps@maps
+  - rl@rl
+  - env@env
+  - planner: planner
+  - sim: sim
+  - _self_
+# Run settings
+mode: training  # [ training, inference, baseline ]
+benchmark: false
+async_env: true
+render: false
+wandb: true
+# Cuda settings
+compile_torch: true
+cuda_allow_tf32: true
+torch_deterministic: false
+device: cuda:0
+# Evaluation settings
+eval_freq: 50
+eval_n_eps: 10
+eval_n_parallel_envs: 2
+sample_mode: false
+safety_controller: false
+# Inference loading model from wandb
+wandb_model:
+  alias: latest
+  name: race_car-649yfd1r
+# Others
+exp_name: raceMOP
+seed: 1
+notes:
+hydra:
+  job_logging:
+    formatters:
+      simple:
+        format: '%(message)s'
+
+

configs/env.yaml

@@ -0,0 +1,13 @@
+frame_cat:
+  use: true
+  n: 35
+  skip_slicing: 4  # n = ceil(n / (skip_slicing + 1)), e.g., ceil(5 / (1 + 1)) = 3, ceil(5 / (2 + 1)) = 2
+wrapper:
+  normalize_obs: true
+  lidar_downsample_filter: true
+  clip_obs: [ -100, 100 ]
+  normalize_rew: true
+  clip_rew: [ -100, 100 ]
+  block_updates: true
+  block_after_n_updates: 0.2 # block after x * total_updates
+reduce_scan_freq_by_2: false

configs/maps.yaml

@@ -0,0 +1,15 @@
+maps_train: [
+  Catalunya #, Nuerburgring, MoscowRaceway, Hockenheim, Sepang, Budapest, Spielberg, Sakhir
+]
+maps_test: [
+  # SaoPaulo #, BrandsHatch, Melbourne, MexicoCity
+]
+map_path: './maps/'
+map_ext: '.png'
+wpt_delim: ';'
+wpt_rowskip: 3
+wpt_xind: 1
+wpt_yind: 2
+wpt_thind: 3
+wpt_vind: 5
+wpt_kappa: 4

configs/planner/ego.yaml

@@ -0,0 +1,8 @@
+defaults:
+  - params: apf  # pure_pursuit / ftg_plus
+  - _self_
+params:
+  do_plot: false
+  wpts_type: center  # 'race' or 'center'
+
+

configs/planner/opponents.yaml

@@ -0,0 +1,6 @@
+defaults:
+  - params: pure_pursuit  # pure_pursuit / ftg_plus
+  - _self_
+params:
+  vgain: [ 0.8, 0.75, 0.7 ]
+  wpts_type: race

configs/planner/params/apf.yaml

@@ -0,0 +1,19 @@
+type: apf
+vgain: [ 1.0 ]
+tlad: 1.2
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: center  # 'race' or 'center'
+# Specific params for each planner
+rr: 8.0
+k_att: 1000.0
+k_rep: 50.0
+max_iters: 20
+step_size: 0.1
+goal_threshold: 0.2
+target_l: 10.0
+lookahead_distance: 1.0
+do_plot: false
+#angle_incre: 0.00435
+#angle_min: -2.35

configs/planner/params/disparity.yaml

@@ -0,0 +1,17 @@
+type: disparity  # 'mpc' / 'pure_pursuit' / 'apf' / 'lattice'
+vgain: [ 1.0 ]  # 1.3, 0.6
+tlad: 1.2 # 0.82461887897713965
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: center  # 'race' or 'center'
+do_plot: false
+# params
+window_size: 10
+safe_thres: 3.0  # 2.0
+danger_thres: 3.0  # 1.5  #0.8
+rb: 100
+P: 0.6  # P gain for steering
+max_gap_length: 600
+min_gap_length: 20
+safe_vgain: 1.2

configs/planner/params/dummy.yaml

@@ -0,0 +1,19 @@
+type: dummy
+vgain: [ 1.0 ]
+#tlad: 1.2
+#n_next_points: 150
+#skip_next_points: 5
+#vmax: None
+#wpts_type: center  # 'race' or 'center'
+## Specific params for each planner
+#rr: 8.0
+#k_att: 1000.0
+#k_rep: 50.0
+#max_iters: 20
+#step_size: 0.1
+#goal_threshold: 0.2
+#target_l: 10.0
+#lookahead_distance: 1.0
+#do_plot: false
+##angle_incre: 0.00435
+##angle_min: -2.35

configs/planner/params/ftg.yaml

@@ -0,0 +1,17 @@
+type: ftg  # 'mpc' / 'pure_pursuit' / 'apf' / 'lattice'
+vgain: [ 1.0 ]  # 1.3, 0.6
+tlad: 1.2 # 0.82461887897713965
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: center  # 'race' or 'center'
+do_plot: false
+# params
+window_size: 10
+safe_thres: 3.0  # 2.0
+danger_thres: 2.0  # 1.5  #0.8
+rb: 100
+P: 0.3  # P gain for steering
+max_gap_length: 500
+min_gap_length: 30
+safe_vgain: 1.0

configs/planner/params/ftg_plus.yaml

@@ -0,0 +1,17 @@
+type: ftg_plus  # 'mpc' / 'pure_pursuit' / 'apf' / 'lattice'
+vgain: [ 1.0 ]  # 1.3, 0.6
+tlad: 1.2 # 0.82461887897713965
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: center  # 'race' or 'center'
+do_plot: false
+# params
+window_size: 10
+safe_thres: 2.0  # 2.0  # Must be meet for finding gaps
+danger_thres: 0.5  # 1.5  #0.8  # Set to zero
+rb: 100
+P: 0.3 #6 #0.6  # P gain for steering
+max_gap_length: 290
+min_gap_length: 30
+safe_vgain: 1.2

configs/planner/params/lattice.yaml

@@ -0,0 +1,7 @@
+type: lattice  # 'mpc' / 'pure_pursuit' / 'apf' / 'lattice'
+vgain: [ 1.0 ]  # 1.3, 0.6
+tlad: 1.2  # 0.82461887897713965 #  1.2 #
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: race  # 'race' or 'center'

configs/planner/params/pure_pursuit.yaml

@@ -0,0 +1,7 @@
+type: pure_pursuit
+vgain: [ 1.0 ]  # 1.3, 0.6
+tlad: 0.82461887897713965  # 1.2
+n_next_points: 150
+skip_next_points: 5
+vmax: None
+wpts_type: center  # 'race' or 'center'

configs/planner/planner.yaml

@@ -0,0 +1,7 @@
+defaults:
+  - ego@ego
+  - opponents@opponents
+  - _self_
+
+
+

configs/rl.yaml

@@ -0,0 +1,54 @@
+total_timesteps: 30e6  # total timesteps of the experiments
+learning_rate: 1e-4  # the learning rate of the optimizer
+warm_up_learning_rate: 3e-4
+num_envs: 128  # the number of parallel game environments
+num_steps: 4096 # the number of steps to run in each environment per policy rollout
+anneal_lr: true  # Toggle learning rate annealing for policy and value networks
+anneal_lr_factor: 0.0 # eta_min = lr * factor
+gamma: 0.99 # the discount factor gamma
+gae_lambda: 0.95  # the lambda for the general advantage estimation
+num_minibatches: 1024  # the number of mini-batches
+update_epochs: 7 # the K epochs to update the policy
+norm_adv: True  # Toggles advantages normalization
+clip_coef: 0.1  # the surrogate clipping coefficient
+clip_vloss: true  # Toggles whether to use a clipped loss for the value function, as per the paper.
+ent_coef: 0.0  # coefficient of the entropy
+vf_coef: 0.5 # 0.5  # coefficient of the value function
+max_grad_norm: 1.0  # 0.5  # the maximum norm for the gradient clipping
+target_kl: null  #  0.01 the target KL divergence threshold
+distribution: TruncatedNormal  # ['TanhNormal', 'TruncatedNormal', 'Normal'] distribution to use for the actor's head -> TanhNormal is bounded [-1, 1]
+combine_before_dist: true
+init_logstd: [ -0.7, -0.7 ]
+upscale_tanh: 5.0
+tanh_loc: false
+pre_tanh_loc: false
+scale_action: [ 0.5, 0.5 ]
+hidden_out_dim: 64
+state_dim: 8
+weight_decay: 0
+rpo_alpha: 0.05
+pretrain: false
+critic_pretrain: 0.0
+critic_safe_exploration: false
+all_frames_history: true  # use also the stacked frames for the feature vector and not only for the lidar
+lstm:
+  use: false
+  hidden_size: 64
+  num_layers: 1
+  num_sub_minibatches: 8
+obs_keys: [
+  aaa_scans,
+  prev_action,
+  linear_vels_x,
+  linear_vels_y,
+  slip_angle,
+  action_planner,
+  yaw_rate,
+  acc_x,
+  steering_angle
+  #  'ang_vels_z',
+  #  'acc_y',
+  #  'progress',
+  #  'safe_velocity',
+  #  'lookahead_points_relative',
+]

configs/sim/reward.yaml

@@ -0,0 +1,14 @@
+reward:
+  step: 0.0
+  collision: -5.0
+  long_vel: 0.0
+  lat_vel: -0.0
+  scaling: 1.0
+  action_change: -0.002
+  yaw_change: -0.0
+  overtaking: 0.0
+  safe_distance_to_obstacles: -0.0
+  distance_threshold: 0.0
+  safe_velocity: 0.0
+  progress: 0.1
+  traveled_distance: 0.0

configs/sim/sim.yaml

@@ -0,0 +1,14 @@
+defaults:
+  - vehicle_params
+  - reward
+render_mode: human_fast  # [ human, human_fast, None ]
+controller_dt: 0.02
+dt: 0.01
+start_pose_mode: random # [ first, sequential, random ]
+n_agents: 3
+start_positions:
+  n_clusters: 30
+  line_type: race  # 'race' / 'center'
+  kappa_threshold: 10.0 #0.5
+  dynamic_reset: true
+  n_split: 2

configs/sim/vehicle_params.yaml

@@ -0,0 +1,21 @@
+vehicle_params:
+  mu: 0.8
+  C_Sf: 4.718
+  C_Sr: 5.4562
+  lf: 0.15875
+  lr: 0.17145
+  h: 0.074
+  m: 3.47
+  I: 0.04712
+  s_min: -0.4189
+  s_max: 0.4189
+  sv_min: -3.2
+  sv_max: 3.2
+  v_switch: 7.319
+  a_max: 7.51
+  v_min: 0.0 # -5.0
+  v_max: 8.0
+  width: 0.27
+  length: 0.51
+  fov_deg: 270
+  lidar_points: 1080

main.py

@@ -0,0 +1,25 @@
+# Copyright (C) 2023  Raphael Trumpp
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <https://www.gnu.org/licenses/>.
+import logging
+import warnings
+import matplotlib.style as mplstyle
+from racemop import main
+
+logging.getLogger("jax._src.xla_bridge").addFilter(logging.Filter("No GPU/TPU found, falling back to CPU."))
+warnings.filterwarnings('ignore', message='')
+mplstyle.use('fast')
+
+if __name__ == "__main__":
+    main()