huangfu/InteractionImitation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Only take targets driving roughly in the same direction as IDM target

Browse Source
This commit is contained in:
Johannes Fischer
2022-02-28 11:37:08 +01:00
parent 51eb810f5c
commit 6068c87402
1 changed files with 34 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/baselines/rule_policies.py
View File

@@ -82,6 +82,7 @@ class IDMRulePolicy(BaseAlgorithm):
self._env = env self._env = env
self.t_future = t_future self.t_future = t_future
self.half_angle = half_angle self.half_angle = half_angle
self.max_heading_diff = 120
# Default IDM parameters # Default IDM parameters
assert target_speed>0, 'negative target speed' assert target_speed>0, 'negative target speed'
@@ -192,7 +193,11 @@ class IDMRulePolicy(BaseAlgorithm):
dl = (dxys*np.hstack((-np.sin(psi), np.cos(psi)))).sum(-1) # (nv, ) dl = (dxys*np.hstack((-np.sin(psi), np.cos(psi)))).sum(-1) # (nv, )
alpha = to_circle(np.arctan2(dl, df)) 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)] heading_diff = to_circle(psi - psi[agent]).flatten()
val_idx = np.arange(nv)[
(np.abs(alpha) < self.half_angle*np.pi/180) & (np.arange(nv) != agent) & (np.abs(heading_diff) < self.max_heading_diff*np.pi/180)
]
if len(val_idx)==0: if len(val_idx)==0:
i = None i = None