diff --git a/src/baselines/rule_policies.py b/src/baselines/rule_policies.py index 1d5948b..ecc807c 100644 --- a/src/baselines/rule_policies.py +++ b/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,103 @@ 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 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 +230,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 +246,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 +255,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)