Scratch for horner scheme

scratch/johannes/horner_scheme.py

@@ -0,0 +1,90 @@
+# %%
+
+import numpy as np
+import torch
+from timeit import default_timer as timer
+
+# %%
+
+def powerseries(x, deg):
+    return torch.stack([x**i for i in range(deg+1)],dim=-1)
+
+def improved_powerseries(x, deg):
+    r = torch.ones(*x.shape, deg+1, dtype=torch.float64)
+    for i in range(1,deg+1):
+        r[:, :, i] = r[:, :, i-1] * x
+    return r
+
+def horner_scheme(x, poly):
+    deg = poly.shape[-1]
+    nsteps = x.shape[-1]
+    r = poly[:, -1:].repeat(1, nsteps)
+    for i in range(2, deg+1):
+        r *= x
+        r += poly[:, -i:1-i]
+    return r
+
+# %%
+
+nv = 151
+delta = 10
+n = 20
+
+state_s = torch.rand((nv, 1))
+nan_idx = np.random.choice([True, False], 151)
+state_s[nan_idx] = np.nan
+
+# %%
+
+n_coef = 21
+xpoly = torch.rand((nv, n_coef),dtype=torch.float64)
+ypoly = torch.rand((nv, n_coef),dtype=torch.float64)
+
+ds = delta * torch.arange(1,n+1).repeat(nv,1)
+
+s = ds + state_s
+s = s.type(torch.float64)
+
+smax = s[:, 0]
+smax = smax.unsqueeze(-1)
+
+
+start = timer()
+for _ in range(100):
+    deg = xpoly.shape[-1] - 1
+    expand_sims = powerseries(s, deg) # (nv, n, deg+1)
+    # print(expand_sims.shape)
+    y = (ypoly.unsqueeze(1) * expand_sims).sum(dim=-1)
+    x = (xpoly.unsqueeze(1) * expand_sims).sum(dim=-1)
+end = timer()
+print("Powerseries: {}".format((end-start)*1))
+
+start = timer()
+for _ in range(100):
+    deg = xpoly.shape[-1] - 1
+    expand_sims = improved_powerseries(s, deg) # (nv, n, deg+1)
+    # print(expand_sims.shape)
+    yp = (ypoly.unsqueeze(1) * expand_sims).sum(dim=-1)
+    xp = (xpoly.unsqueeze(1) * expand_sims).sum(dim=-1)
+end = timer()
+print("Improved Powerseries: {}".format((end-start)*1))
+
+start = timer()
+for _ in range(100):
+    x_horner = horner_scheme(s, xpoly)
+    y_horner = horner_scheme(s, ypoly)
+end = timer()
+print("Horner: {}".format((end-start)*1))
+
+start = timer()
+for _ in range(100):
+    x_max = horner_scheme(smax, xpoly)
+    y_max = horner_scheme(smax, ypoly)
+end = timer()
+# print("Horner smax: {}".format((end-start)*1))
+
+assert np.all(np.isclose(xp,x)[~nan_idx])
+assert np.all(np.isclose(yp,y)[~nan_idx])
+assert np.all(np.isclose(x_horner,x)[~nan_idx])
+assert np.all(np.isclose(y_horner,y)[~nan_idx])
+# %%

scratch/johannes/intersimple/idm.py

@@ -0,0 +1,42 @@
+# %%
+import torch
+from src.baselines.rule_policies import IDMRulePolicy
+from tqdm import tqdm
+
+from intersim.envs import NRasterizedIncrementingAgent, NRasterizedRandomAgent, NRasterized
+from intersim.envs.intersimple import speed_reward
+import functools
+
+
+env = NRasterizedIncrementingAgent(
+    # agent = 4,
+    reward=functools.partial(
+        speed_reward,
+        collision_penalty=1000
+    ),
+    stop_on_collision=True,
+)
+policy = IDMRulePolicy(env)
+
+colliding_agents = []
+
+for agent in range(151):
+    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:
+                collising_agents.append(agent)
+                print("  Collision")
+            break
+    env.close(filestr='idm/agent_{}'.format(agent))
+
+print(len(colliding_agents), "colliding_agents")
+print(colliding_agents)
+# %%

src/baselines/rule_policies.py

@@ -14,9 +14,9 @@ class PControllerPolicy(BaseAlgorithm):
         self._env = env
         self.target_v = 8.94 # m/s
         self.attn_weight = 20
-    
+
     # BaseAlgorithm abstract methods
-    def _setup_model(self): 
+    def _setup_model(self):
         return None
     def learn(self, *args, **kwargs):
         return self
@@ -26,7 +26,7 @@ class PControllerPolicy(BaseAlgorithm):
         Generate action, state from observation
 
         (But actually generate next action from underlying environment state)
-        
+
         Args:
             observation (np.ndarray): instantaneous observation from environment
 
@@ -36,16 +36,16 @@ class PControllerPolicy(BaseAlgorithm):
         """
         agent = self._env._agent
         ego_state = self._env._env.projected_state[agent].numpy() # (5,) tensor
-        
-        
+
+
         # relative_state = np.delete(self._env._env.relative_state[agent].numpy(), agent, axis=0) #(nv-1, 6) tensor
-        
+
         # calculate front and left distances from ego
 
         # calculate relative speed in direction of position difference vector
-        
+
         # calculate angle alpha and distance d of vehicle i from ego heading
-        
+
         # attn[i] ~= exp( -(alpha[i])^2 - .01 * d[i] - .1 * vrel[i]
 
 
@@ -60,14 +60,14 @@ class IDMRulePolicy(BaseAlgorithm):
 
     The front car is chosen as the closer of:
         - closest car within a 45 degree half angle cone of the ego's heading
-        - ''' after propagating the environment forward by `t_future' seconds with 
+        - ''' after propagating the environment forward by `t_future' seconds with
             current headings and velocities
-    
+
     """
 
-    def __init__(self, env: Intersimple, 
-        target_speed:float= 8.94, 
-        t_future:List[float]=[0., 1., 2., 3.], 
+    def __init__(self, env: Intersimple,
+        target_speed:float= 8.94,
+        t_future:List[float]=[0., 1., 2., 3.],
         half_angle:float=60.):
         """
         Initialize policy with pointer to environment it will run on and target speed
@@ -85,7 +85,7 @@ 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
@@ -95,18 +95,18 @@ class IDMRulePolicy(BaseAlgorithm):
         np.seterr(invalid='ignore')
 
     # BaseAlgorithm abstract methods
-    def _setup_model(self): 
+    def _setup_model(self):
         return None
     def learn(self, *args, **kwargs):
         return self
-    
-    def predict(self, observation:np.ndarray, 
+
+    def predict(self, observation:np.ndarray,
         *args, **kwargs) -> Tuple[np.ndarray, None]:
         """
         Predict action, state from observation
 
         (But actually generate next action from underlying environment state)
-        
+
         Args:
             observation (np.ndarray): instantaneous observation from environment
 
@@ -124,50 +124,104 @@ 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]
-        xy = full_state[:,0:2] # (nv, 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)
+        # psi = full_state[:,3:4] # (nv, 1)
 
-        d, r, i = self.get_ego_dr(agent, xy, v, psi)
+        length = 20
+        step = 0.5
+        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)))
+        velocities = state[:,1]
 
-        # propagate environment forward at constant velocity
-        for t in self.t_future:
-            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)
-                if d2 < d:
-                    d, r, i = d2, r2, i2
-        
-        # Update environment interaction graph with i
-        if i:
-            self._env._env._graph._neighbor_dict={agent:[i]}
+        # something like this could be done to also take future proximity of vehicles to ego path into account
+        # time_horizon = np.array(range(3))
+        # predictions = state[:,0:1] + np.outer(state[:,1], time_horizon)
 
-        if d == np.inf:
-            d_des = self.d_min
-        else:
-            d_des = self.d_min + self.tau * s + s * r / (2* (self.a_max*self.b_pref)**0.5 )
+        paths = np.stack([x[:,:-1],y[:,:-1], heading], axis=1) # (nv x 3 x (path_length-1))
+        ego_path = paths[agent:agent+1] # (1 x 3 x path_length-1)
+
+        # (x,y,phi) of all vehicles
+        poses = np.expand_dims(full_state[:, [0,1,3]], 2) # (nv x 3 x 1)
+
+        diff = ego_path - poses
+        diff[:, 2, :] = to_circle(diff[:, 2, :])
+
+        # Test if position and heading angle are close for some point on the future vehicle track
+        max_pos_error = 1
+        pos_close = np.sum(diff[:, 0:2, :]**2, 1) <= max_pos_error**2 # (nv x path_length-1)
+        max_deg_error = 20
+        heading_close = np.abs(diff[:, 2, :]) <= 20 * np.pi / 180 # (nv x path_length-1)
+        # For all vehicles get the path points where they are close to the ego path
+        close = np.logical_and(pos_close, heading_close) # (nv x 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]
+
+        # alternative vectorized code
+        # def findfirst(a):
+        #     idx = np.argwhere(a)
+        #     if len(idx) == 0:
+        #         return np.NaN
+        #     else:
+        #         return float(idx[0]) # float conversion, to get a numpy array of dtype=float64
+        # d = np.apply_along_axis(findfirst, 1, close) # (nv)
+        # if np.all(np.isnan(d)):
+        #     leader = agent
+        # else:
+        #     leader = np.nanargmin(d)
+        #     path_idx = d[leader]
+        #     min_idx = np.sqrt(np.sum(diff[leader, 0:2, path_idx]**2))
+
+
+        if leader != agent:
+            # distance along ego path to point with closest distance
+            d = step * min_idx
+            # add distance from ego path point with closest distance to actual vehicle position
+            d += np.sqrt(np.sum(diff[leader, 0:2, min_idx]**2))
+
+            # 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):
             action = self._env._normalize(action)
 
         assert action.shape==(1,)
         return action
-    
-    def get_ego_dr(self, agent:int, xy: np.ndarray, 
+
+
+    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
@@ -177,7 +231,7 @@ class IDMRulePolicy(BaseAlgorithm):
         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 
+            i (Optional[int]): index of closest vehicle, or None
         """
         nv, nxy = xy.shape
         nv2, nvel = v.shape
@@ -193,7 +247,7 @@ class IDMRulePolicy(BaseAlgorithm):
         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')
@@ -202,10 +256,10 @@ class IDMRulePolicy(BaseAlgorithm):
             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] 
-            
+            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)