Remove unused method

bc-experiment.py

@@ -142,7 +142,7 @@ if __name__ == '__main__':
     import argparse
     parser = argparse.ArgumentParser()
     parser.add_argument('--train', choices=['A', 'B'])
-    parser.add_argument('--epochs', type=int, default=1000)
+    parser.add_argument('--epochs', type=int, default=500)
     parser.add_argument('--test', type=str, help='path to config file to run final training on')
     parser.add_argument('--test_seeds', type=int, default=5)
     parser.add_argument('--test_cpus', type=int, help='number of cpus available to split test seed training over')
@@ -162,10 +162,10 @@ if __name__ == '__main__':
                 },
                 'policy': {
                     'learning_rate': 3e-4, 
-                    'learning_rate_decay': tune.grid_search([0.001, 1.0]), 
+                    'learning_rate_decay': tune.grid_search([0.999, 1.0]), 
-                    'hidden_layer_size': tune.grid_search([10, 20, 40, 80]),
+                    'hidden_layer_size': tune.grid_search([10, 20, 40]),
-                    'n_hidden_layers': tune.grid_search([2, 3, 4]), 
+                    'n_hidden_layers': tune.grid_search([2, 3]), 
-                    'activation':0,
+                    'activation':tune.grid_search([0, 1]),
                 },
                 'train_epochs': args.epochs,
                 'seed': 0,
@@ -210,3 +210,5 @@ if __name__ == '__main__':
             check_dir = analysis._checkpoints[i]['logdir']
             shutil.copyfile(os.path.join(check_dir,'policy_final.pt'), 
                 os.path.join(savepath, f'policy_seed{s}.pt'))
+            shutil.copyfile(os.path.join(check_dir,'params.json'),
+                os.path.join(savepath, 'config.json')) # copy config automatically

best_configs/gail-trpo_expA.json

@@ -1,31 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "delta": 0.01,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 2,
-        "activation": 0
-    },
-    "value": {
-        "learning_rate": 0.0001,
-        "weight_decay": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 4,
-        "n_hidden_layers_global": 1,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/gail-trpo_expB.json

@@ -1,31 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "delta": 0.01,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 2,
-        "activation": 0
-    },
-    "value": {
-        "learning_rate": 0.0001,
-        "weight_decay": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 4,
-        "n_hidden_layers_global": 1,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne2_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 10,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne2_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 10,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne3_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne3_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/hail-etienne_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne2_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 10,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne2_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 10,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne3_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne3_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne4_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 500,
-        "hidden_layer_size": 40,
-        "n_hidden_layers": 3,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 150,
-    "seed": 0
-}

best_configs/shail-etienne_expA.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

best_configs/shail-etienne_expB.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

eval_experiments.py

@@ -77,7 +77,7 @@ def latex_print(am, light=False):
     print('success rate, distance travelled, RWSE_10, |DeltaV|, AccelJSD')
     if light:
         if 'rwse_10s' in am.keys():
-            print("%2.1f& %2.1f & %1.2f & %2.1f& "
+            print("%2.1f& %2.1f & %2.1f & %1.2f& "
             "%0.3f \\\\" %( 100*am['success rate'][0], am['mean travel distance'][0], am['rwse_10s'][0],
                             am['average absolute average velocity'][0],am['acceleration distribution divergence'][0] ))
             return
@@ -88,7 +88,7 @@ def latex_print(am, light=False):
         return
     
     print("%2.1f \\scriptstyle\\pm %2.1f & %2.1f \\scriptstyle\\pm %2.1f & "
-        "%1.2f \\scriptstyle\\pm %1.2f & %2.1f \\scriptstyle\\pm %1.1f & "
+        "%2.1f \\scriptstyle\\pm %1.1f & %1.2f \\scriptstyle\\pm %1.2f & "
         "%0.3f \\scriptstyle\\pm %0.3f \\\\" %( 100*am['success rate'][0], 100*am['success rate'][1],
                am['mean travel distance'][0] , am['mean travel distance'][1]  ,
                am['rwse_10s'][0] , am['rwse_10s'][1]  ,

gail-experiment.py

@@ -236,3 +236,5 @@ if __name__ == '__main__':
             check_dir = analysis._checkpoints[i]['logdir']
             shutil.copyfile(os.path.join(check_dir,'policy_final.pt'), 
                 os.path.join(savepath, f'policy_seed{s}.pt'))
+            shutil.copyfile(os.path.join(check_dir,'params.json'),
+                os.path.join(savepath, 'config.json')) # copy config automatically

scratch/johannes/intersimple/idm.py

@@ -0,0 +1,44 @@
+# %%
+import torch
+from src.baselines.rule_policies import IDMRulePolicy
+from tqdm import tqdm
+
+from intersim.envs import NRasterizedIncrementingAgent, NRasterizedRandomAgent, NRasterized,IntersimpleLidarFlat
+from intersim.envs.intersimple import speed_reward
+import functools
+
+
+env = IntersimpleLidarFlat(
+    agent = 51,
+    n_rays=5,
+    reward=functools.partial(
+        speed_reward,
+        collision_penalty=1000
+    ),
+    stop_on_collision=True,
+)
+policy = IDMRulePolicy(env)
+
+colliding_agents = []
+
+# for agent in range(151):
+agent = env._agent
+print("Start agent", agent)
+obs = env.reset()
+env.render(mode='post')
+for i in range(300):
+    action, _ = policy.predict(torch.tensor(obs))
+    # action = policy.sample(policy(torch.tensor(obs, dtype=torch.float32)))
+    obs, reward, done, _ = env.step(action)
+    env.render(mode='post')
+    # print('step', i, 'reward', reward)
+    if done:
+        if reward < -500:
+            colliding_agents.append(agent)
+            print("  Collision")
+        break
+env.close(filestr='idm3/agent_{}'.format(agent))
+
+print(len(colliding_agents), "colliding_agents")
+print(colliding_agents)
+# %%

shail-experiment.py

@@ -62,7 +62,7 @@ def training_function(config):
             ), options=option_list[config['policy']['option']],
             safe_actions_collision_method=config['trainenv']['safe_actions_collision_method'], 
             abort_unsafe_collision_method=config['trainenv']['abort_unsafe_collision_method'],
-            ) for _ in range(120)]
+            ) for _ in range(60)]
 
     elif config['experiment'] == 'B': 
         envs = sum([[SafeOptionsEnv(Setobs(
@@ -78,7 +78,7 @@ def training_function(config):
             ), options=option_list[config['policy']['option']],
             safe_actions_collision_method=config['trainenv']['safe_actions_collision_method'], 
             abort_unsafe_collision_method=config['trainenv']['abort_unsafe_collision_method'],
-            ) for _ in range(30)] for track in range(4)],[])
+            ) for _ in range(15)] for track in range(4)],[])
     
     else:
         raise NotImplementedError
@@ -142,7 +142,7 @@ def training_function(config):
         v_opt=v_opt,
         v_iters=config['value']['iterations_per_epoch'],
         epochs=config['train_epochs'],
-        rollout_episodes=120, 
+        rollout_episodes=60, 
         rollout_steps=60,
         gamma=0.99,
         gae_lambda=0.9,
@@ -185,11 +185,11 @@ if __name__ == '__main__':
                     'learning_rate': 3e-4, 
                     'learning_rate_decay': 1.0, 
                     'clip_ratio': 0.2, 
-                    'iterations_per_epoch': tune.grid_search([250, 500, 750]),
+                    'iterations_per_epoch': 100,
-                    'hidden_layer_size': 40, #tune.grid_search([20, 40]),
+                    'hidden_layer_size': tune.grid_search([20, 40]),
-                    'n_hidden_layers': 3, #tune.grid_search([2, 3]), 
+                    'n_hidden_layers': tune.grid_search([2, 3]), 
                     'activation':0,
-                    'option': 0, #tune.grid_search(list(range(len(option_list))))
+                    'option': tune.grid_search(list(range(len(option_list))))
                 },
                 'value': {
                     'learning_rate': 1e-3, 
@@ -199,8 +199,8 @@ if __name__ == '__main__':
                     'learning_rate': 1e-3, 
                     'weight_decay': 1e-4, 
                     'iterations_per_epoch': 100, 
-                    'n_hidden_layers_element': 3, #tune.grid_search([3,4]),
+                    'n_hidden_layers_element': tune.grid_search([3,4]),
-                    'n_hidden_layers_global': 2, #tune.grid_search([1,2]),
+                    'n_hidden_layers_global': tune.grid_search([1,2]),
                     'hidden_layer_size': 10, 
                     'activation': 0,
                 },
@@ -259,3 +259,5 @@ if __name__ == '__main__':
             check_dir = analysis._checkpoints[i]['logdir']
             shutil.copyfile(os.path.join(check_dir,'policy_final.pt'), 
                 os.path.join(savepath, f'policy_seed{s}.pt'))
+            shutil.copyfile(os.path.join(check_dir,'params.json'),
+                os.path.join(savepath, 'config.json')) # copy config automatically

src/baselines/rule_policies.py

@@ -85,11 +85,13 @@ class IDMRulePolicy(BaseAlgorithm):
 
         # Default IDM parameters
         assert target_speed>0, 'negative target speed'
-        self.s_max = target_speed
+        self.v_max = target_speed
         self.a_max = np.array([3.]) # nominal acceleration
         self.tau = 0.5 # desired time headway
         self.b_pref = 2.5 # preferred deceleration
-        self.d_min = 1 #minimum spacing
+        self.d_min = 3 #minimum spacing
+        self.max_pos_error = 2 # m, for matching vehicles to ego path
+        self.max_deg_error = 30 # degree, for matching vehicles to ego path
 
         # for np.remainder nan warnings
         np.seterr(invalid='ignore')
@@ -124,37 +126,63 @@ class IDMRulePolicy(BaseAlgorithm):
             action (np.ndarray): action for controlled agent to take
         """
         agent = self._env._agent
+        state = self._env._env.state.numpy()
         full_state = self._env._env.projected_state.numpy() #(nv, 5)
         ego_state = full_state[agent] # (5,)
-        s = ego_state[2]
+        v_ego = ego_state[2]
-        xy = full_state[:,0:2] # (nv, 2)
         v = full_state[:,2:3] # (nv, 1)
-        psi = full_state[:,3:4] # (nv, 1)
 
-        d, r, i = self.get_ego_dr(agent, xy, v, psi)
+        length = 20
+        step = 0.1
+        x, y = self._env._env._generate_paths(delta=step, n=length/step, is_distance=True)
+        heading = to_circle(np.arctan2(np.diff(y), np.diff(x)))
 
-        # propagate environment forward at constant velocity
+        paths = np.stack([x[:,:-1],y[:,:-1], heading], axis=1) # (nv, 3, (path_length-1))
-        for t in self.t_future:
+        ego_path = paths[agent:agent+1] # (1, 3, path_length-1)
-            if t > 0:
-                xy2 = xy + t * v * np.vstack((np.cos(psi[:,0]), np.sin(psi[:,0]))).T
-                d2, r2, i2 = self.get_ego_dr(agent, xy2, v, psi)
 
-                # choose closer vehicle (now vs imagined)
+        # (x,y,phi) of all vehicles
-                if d2 < d:
+        poses = np.expand_dims(full_state[:, [0,1,3]], 2) # (nv, 3, 1)
-                    d, r, i = d2, r2, i2
 
-        # Update environment interaction graph with i
+        diff = ego_path - poses
-        if i:
+        diff[:, 2, :] = to_circle(diff[:, 2, :])
-            self._env._env._graph._neighbor_dict={agent:[i]}
 
-        if d == np.inf:
+        # Test if position and heading angle are close for some point on the future vehicle track
-            d_des = self.d_min
+        pos_close = np.sum(diff[:, 0:2, :]**2, 1) <= self.max_pos_error**2 # (nv, path_length-1)
-        else:
+        heading_close = np.abs(diff[:, 2, :]) <= self.max_deg_error * np.pi / 180 # (nv, path_length-1)
-            d_des = self.d_min + self.tau * s + s * r / (2* (self.a_max*self.b_pref)**0.5 )
+        # For all vehicles get the path points where they are close to the ego path
+        close = np.logical_and(pos_close, heading_close) # (nv, path_length-1)
+        close[agent, :] = False # exclude ego agent
+
+        leader = agent
+        min_idx = np.Inf
+        # Determine vehicle that is closest to ego in terms of path coordinate
+        for veh_id in range(len(close)):
+            path_idx = np.nonzero(close[veh_id])[0]
+            # veh_id is never close to agent
+            if len(path_idx) == 0:
+                continue
+            # first path index where veh_id is close to agent
+            elif path_idx[0] < min_idx:
+                leader = veh_id
+                min_idx = path_idx[0]
+
+        if leader != agent:
+            # distance along ego path to point with closest distance
+            d = step * min_idx
+
+            # Update environment interaction graph with leader
+            self._env._env._graph._neighbor_dict={agent:[leader]}
+
+            delta_v = v_ego - v[leader, 0]
+            d_des = self.d_min + self.tau * v_ego + v_ego * delta_v / (2* (self.a_max*self.b_pref)**0.5 )
             d_des = max(d_des, self.d_min)
+        else:
+            d = np.Inf
+            d_des = self.d_min
+            self._env._env._graph._neighbor_dict={}
 
         assert (d_des>= self.d_min)
-        action = self.a_max*(1 - (s/self.s_max)**4 - (d_des/d)**2)
+        action = self.a_max*(1 - (v_ego/self.v_max)**4 - (d_des/d)**2)
 
         # normalize action to range if env is a NormalizedActionSpace
         if isinstance(self._env, NormalizedActionSpace):
@@ -163,49 +191,6 @@ class IDMRulePolicy(BaseAlgorithm):
         assert action.shape==(1,)
         return action
 
-    def get_ego_dr(self, agent:int, xy: np.ndarray, 
-        v: np.ndarray, psi: np.ndarray) -> Tuple[float, float, Optional[int]]:
-        """
-        Return distance and relative speed of closest car within half angle from heading
-        
-        Args:
-            agent (int): agent index
-            xy (np.ndarray): (nv, 2) x and y positions
-            v (np.ndarray): (nv, 1) velocity
-            psi (np.ndarray): (nv, 1) heading angle
-
-        Returns:
-            d (float): distance to closest vehicle in cone
-            r (float): relative speed between the two vehicles
-            i (Optional[int]): index of closest vehicle, or None 
-        """
-        nv, nxy = xy.shape
-        nv2, nvel = v.shape
-        nv3, npsi = psi.shape
-        assert nv==nv2==nv3
-        assert nxy==2
-        assert nvel==npsi==1
-
-        dxys = xy - xy[agent] # (nv, 2)
-        ds = np.linalg.norm(dxys,axis=1) # (nv,)
-        df = (dxys*np.hstack((np.cos(psi),np.sin(psi)))).sum(-1) # (nv, )
-        dl = (dxys*np.hstack((-np.sin(psi), np.cos(psi)))).sum(-1) # (nv, )
-        alpha = to_circle(np.arctan2(dl, df))
-
-        val_idx = np.arange(nv)[(np.abs(alpha) < self.half_angle*np.pi/180) & (np.arange(nv) != agent)]
-        
-        if len(val_idx)==0:
-            i = None
-            d = float('inf')
-            r = float('inf')
-        else:
-            idx = np.argmin(ds[val_idx]) # closest car which meets requirements
-            i = int(val_idx[idx])
-            d = ds[i]
-            r = v[i,0]-v[agent,0] 
-            
-        return d, r, i
-    
 def to_circle(x: np.ndarray) -> np.ndarray:
     """
     Casts x (in rad) to [-pi, pi)

test_policies/hail-etienne/expA/config.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

test_policies/hail-etienne/expB/config.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": null,
-        "abort_unsafe_collision_method": null
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

test_policies/shail-etienne/expA/config.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "A",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}

test_policies/shail-etienne/expB/config.json

@@ -1,33 +0,0 @@
-{
-    "experiment": "B",
-    "trainenv": {
-        "stop_on_collision": false,
-        "safe_actions_collision_method": "circle",
-        "abort_unsafe_collision_method": "circle"
-    },
-    "policy": {
-        "learning_rate": 0.0003,
-        "learning_rate_decay": 1.0,
-        "clip_ratio": 0.2,
-        "iterations_per_epoch": 100,
-        "hidden_layer_size": 20,
-        "n_hidden_layers": 4,
-        "activation": 0,
-        "option": 0
-    },
-    "value": {
-        "learning_rate": 0.001,
-        "iterations_per_epoch": 1000
-    },
-    "discriminator": {
-        "learning_rate": 0.001,
-        "weight_decay": 0.0001,
-        "iterations_per_epoch": 100,
-        "n_hidden_layers_element": 3,
-        "n_hidden_layers_global": 2,
-        "hidden_layer_size": 10,
-        "activation": 0
-    },
-    "train_epochs": 100,
-    "seed": 0
-}