From 8acfbc79933655b3fb3d2c32a187f679bd6d4bc4 Mon Sep 17 00:00:00 2001 From: Arec Date: Mon, 19 Jul 2021 07:10:28 -0700 Subject: [PATCH] updating expert data to keep track of deviations and check for collisions --- src/expert_data.py | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/expert_data.py b/src/expert_data.py index 4d394bd..1bc37f9 100644 --- a/src/expert_data.py +++ b/src/expert_data.py @@ -6,7 +6,7 @@ import numpy as np import intersim from intersim.utils import get_map_path, get_svt, SVT_to_sim_stateactions - +from intersim import collisions import os opj = os.path.join @@ -36,15 +36,29 @@ def generate_expert_data(path: str='expert_data', loc: int = 0, track:int = 0, * env.reset() done = False - obs, actions_taken = [], [] + obs, actions_taken, max_devs = [], [], [] i = 0 - print(actions.shape) - while not done: + while not done and i < len(actions): + # check state deviation + env_state = env.projected_state + nni = ~torch.isnan(env_state[:,0]) + norms = torch.norm(env_state[nni,:2]-states[i,nni,:2], dim=1) + max_devs.append(norms.max()) + # print("Step: %04i, Maximum Deviation: %f m" %(i, max_devs[-1])) + + # propagate environment ob, r, done, info = env.step(actions[i]) obs.append(ob) actions_taken.append(info['action_taken']) i += 1 + print("Maximum environment deviation from track: %f m" %(max(max_devs))) + + # check for collisions + x = torch.stack([ob['state'] for ob in obs]) + cols = collisions.check_collisions_trajectory(x, svt.lengths, svt.widths) + assert ~torch.any(cols), 'Error: Collisions found at indices {}'.format(cols.nonzero(as_tuple=True)) + # shift actions actions_taken.pop(0) obs.pop(-1)