def step_pid(self, desire):
"""Execute single step with lane_pos, velocity controls."""
lane_pos_scale = self._config.track_limits[
'high'] - self._config.track_limits['low']
desire[
0] = self._config.track_limits['low'] + lane_pos_scale * desire[0]
if self._config.normalize_actions:
# [-1, 1] should correspond to [-self._config.target_speed,
# self._config.target_speed]
speed_scale = self._config.target_speed
desire[1] *= speed_scale # Now in m/s
# convert to km/hr
desire[1] *= 3600 / 1000 # Now in km/hr
# Normalize to gym_torcs specs
desire[1] /= self.default_speed
reward = 0.0
for PID_step in range(self._config.pid_settings['pid_latency']):
a_t = self.PID_controller.get_action(desire)
try:
self.ob, r, done, info = TorcsEnv.step(self, PID_step,
self.client, a_t,
self._config.early_stop)
except Exception as e:
logging.debug("Exception {} caught at port {}".format(
str(e), self.torcs_server_port))
self.wait_for_observation()
game_state = {
"torcs_reward": r,
"torcs_done": done,
"distance_traversed": self.client.S.d["distRaced"],
"angle": self.client.S.d["angle"],
"damage": self.client.S.d["damage"],
"trackPos": self.client.S.d["trackPos"],
"racePos": self.client.S.d["racePos"],
"track": self.client.S.d["track"]
}
reward += self.reward_handler.get_reward(self._config, game_state)
if self._config.pid_assist:
self.PID_controller.update(self.ob)
done = self.done_handler.get_done_signal(self._config, game_state)
if done:
self.client.R.d["meta"] = True # Terminate the episode
logging.info('Terminating PID {}'.format(
self.client.serverPID))
break
next_obs = self.observation_handler.get_obs(self.ob, self._config)
info["distRaced"] = self.client.S.d["distRaced"]
info["racePos"] = self.client.S.d["racePos"]
return next_obs, reward, done, info
def step_vanilla(self, action):
"""Execute single step with steer, acceleration, brake controls."""
if self._config.normalize_actions:
# action[1] = (action[1] + 1) / 2.0 # acceleration back to [0, 1]
# action[2] = (action[2] + 1) / 2.0 # brake back to [0, 1]
action[0] = self.clip(action[0], -1, 1)
action[1] = self.clip(action[1], 0, 1)
action[2] = self.clip(action[2], 0, 1)
# print(action)
r = 0.0
try:
self.ob, r, done, info = TorcsEnv.step(self, 0, self.client,
action,
self._config.early_stop)
except Exception as e:
logging.debug("Exception {} caught at port {}".format(
str(e), self.torcs_server_port))
self.wait_for_observation()
if done:
print('torce_done is True')
game_state = {
"torcs_reward": r,
"torcs_done": done,
"distance_traversed": self.client.S.d['distRaced'],
"angle": self.client.S.d["angle"],
"damage": self.client.S.d["damage"],
"trackPos": self.client.S.d["trackPos"],
"racePos": self.client.S.d["racePos"],
"track": self.client.S.d["track"]
}
reward = self.reward_handler.get_reward(self._config, game_state,
action)
done = self.done_handler.get_done_signal(self._config, game_state)
if done:
if self._config.traffic:
self.traffic_manager.kill_all_traffic_agents()
self.client.R.d["meta"] = True # Terminate the episode
logging.info('Terminating PID {}'.format(self.client.serverPID))
# print(self.ob)
next_obs = self.observation_handler.get_obs(self.ob, self._config)
info["distRaced"] = self.client.S.d["distRaced"]
info["racePos"] = self.client.S.d["racePos"]
return next_obs, reward, done, info
def playTraffic(port=3101, target_vel=50.0, angle=0.0, sleep=0):
"""Traffic Play function."""
env = TorcsEnv(vision=False, throttle=True, gear_change=False)
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False)
client.MAX_STEPS = np.inf
client.get_servers_input(step=0)
obs = client.S.d
ob = env.make_observation(obs)
except:
pass
episode_count = cfg['traffic']['max_eps']
max_steps = cfg['traffic']['max_steps_eps']
early_stop = 0
velocity_i = target_vel / 300.0
accel_pid = PID(np.array([10.5, 0.05, 2.8]))
steer_pid = PID(np.array([5.1, 0.001, 0.000001]))
steer = 0.0
accel = 0.0
brake = 0
A = cfg['traffic']['amplitude'] / 300.0
T = cfg['traffic']['time_period']
# print(velocity_i)
for i in range(episode_count):
info = {'termination_cause': 0}
steer = 0.0
accel = 0.0
brake = 0
S = 0.0
for step in range(max_steps):
a_t = np.asarray([steer, accel, brake]) # [steer, accel, brake]
try:
ob, r_t, done, info = env.step(step, client, a_t, early_stop)
if done:
pass
except Exception as e:
print("Exception caught at point A at port " + str(i) + str(e))
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False)
client.MAX_STEPS = np.inf
client.get_servers_input(step=0)
obs = client.S.d
ob = env.make_observation(obs)
except:
pass
continue
if (step <= sleep):
print("WAIT" + str(port))
continue
velocity = velocity_i + A * math.sin(step / T)
opp = ob.opponents
front = np.array([opp[15], opp[16], opp[17], opp[18], opp[19]])
closest_front = np.min(front)
if (step % T == 0):
print("VEL_CHANGE: ", velocity * 300.0)
vel_error = velocity - ob.speedX
angle_error = -(ob.trackPos - angle) / 10 + ob.angle
steer_pid.update_error(angle_error)
accel_pid.update_error(vel_error)
accel = accel_pid.output()
steer = steer_pid.output()
if accel < 0:
brake = 1
else:
brake = 0
if closest_front < ((float(0.5 * ob.speedX * 100) + 10.0) / 200.0):
brake = 1
else:
brake = 0
try:
if 'termination_cause' in info.keys() and info['termination_cause'] == 'hardReset':
print('Hard reset by some agent')
ob, client = env.reset(client=client, relaunch=True)
except Exception as e:
print("Exception caught at point B at port " + str(i) + str(e) )
ob = None
while ob is None:
try:
client = snakeoil3.Client(p=port, vision=False) # Open new UDP in vtorcs
client.MAX_STEPS = np.inf
client.get_servers_input(0) # Get the initial input from torcs
obs = client.S.d # Get the current full-observation from torcs
ob = env.make_observation(obs)
except:
print("Exception caught at at point C at port " + str(i) + str(e))
def step(self, desire):
"""Step method to be called at each time step."""
r_t = 0
for PID_step in range(self.PID_latency):
# Implement the desired trackpos and velocity using PID
if self.pid_assist:
self.accel_PID.update_error((desire[1] - self.prev_vel))
self.steer_PID.update_error(
(-(self.prev_lane - desire[0]) / 10 + self.prev_angle))
if self.accel_PID.output() < 0.0:
brake = 1
else:
brake = 0
a_t = np.asarray(
[self.steer_PID.output(),
self.accel_PID.output(), brake])
else:
a_t = desire
try:
self.ob, r, done, info = TorcsEnv.step(self, PID_step,
self.client, a_t,
self.early_stop)
except Exception as e:
print(("Exception caught at port " + str(e)))
self.ob = None
while self.ob is None:
try:
self.client = snakeoil3.Client(p=self.port,
vision=self.vision)
# Open new UDP in vtorcs
self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
self.client.get_servers_input(0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation from torcs
self.ob = self.make_observation(raw_ob)
except:
pass
continue
self.prev_vel = self.ob.speedX
self.prev_angle = self.ob.angle
self.prev_lane = self.ob.trackPos
if (math.isnan(r)):
r = 0.0
r_t += r # accumulate rewards over all the time steps
self.distance_traversed = self.client.S.d['distRaced']
r_t += (self.distance_traversed - self.prev_dist) /\
self.track_len
self.prev_dist = deepcopy(self.distance_traversed)
if self.distance_traversed >= self.track_len:
done = True
if done:
# self.reset()
break
s_t1 = np.hstack((self.ob.angle, self.ob.track, self.ob.trackPos,
self.ob.speedX, self.ob.speedY, self.ob.speedZ,
self.ob.wheelSpinVel / 100.0, self.ob.rpm))
return s_t1, r_t, done, info
class MadrasTrafficAgent(object):
def __init__(self, port, cfg, name='MadrasTraffic'):
self.cfg = cfg
self.name = name
self.steer = 0.0
self.accel = 0.0
self.brake = 0.0
self.env = TorcsEnv(
vision=(cfg["vision"] if "vision" in cfg else False),
throttle=(cfg["throttle"] if "throttle" in cfg else True),
gear_change=(cfg["gear_change"]
if "gear_change" in cfg else False),
visualise=(self.cfg["visualise"]
if "visualise" in self.cfg else False),
name=self.name)
self.PID_controller = PIDController(cfg["pid_settings"])
self.port = port
self.min_safe_dist = 0.005 * (
cfg["min_safe_dist"] if "min_safe_dist" in cfg else 1) # 1 meter
def wait_for_observation(self):
"""Refresh client and wait for a valid observation to come in."""
self.ob = None
while self.ob is None:
logging.debug("{} Still waiting for observation at {}".format(
self.name, self.port))
try:
self.client = snakeoil3.Client(
p=self.port,
vision=(self.cfg["vision"]
if "vision" in self.cfg else False),
visualise=(self.cfg["visualise"]
if "visualise" in self.cfg else False),
name=self.name)
# Open new UDP in vtorcs
self.client.MAX_STEPS = self.cfg[
"client_max_steps"] if "client_max_steps" in self.cfg else np.inf
self.client.get_servers_input(0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation from torcs
self.ob = self.env.make_observation(raw_ob)
except:
pass
def get_action(self):
raise NotImplementedError(
"Successor classes must implement this method")
def flag_off(self, random_seed=0):
del random_seed
self.wait_for_observation()
logging.debug("[{}]: My server is PID: {}".format(
self.name, self.client.serverPID))
self.is_alive = True
while True:
if self.is_alive:
action = self.get_action()
try:
self.ob, _, done, info = self.env.step(
0, self.client, action)
except Exception as e:
logging.debug(
"Exception {} caught by {} traffic agent at port {}".
format(str(e), self.name, self.port))
self.wait_for_observation()
self.detect_and_prevent_imminent_crash_out_of_track()
self.PID_controller.update(self.ob)
if done:
self.is_alive = False
logging.debug("{} died.".format(self.name))
def get_front_opponents(self):
return np.array([
self.ob.opponents[16],
self.ob.opponents[17],
self.ob.opponents[18],
])
def detect_and_prevent_imminent_crash_out_of_track(self):
while True:
min_dist_from_track = np.min(self.ob.track)
if min_dist_from_track <= self.min_safe_dist:
closest_dist_sensor_id = np.argmin(self.ob.track)
if closest_dist_sensor_id < 9:
action = [1, 0, 1]
elif closest_dist_sensor_id > 9:
action = [-1, 0, 1]
else:
action = [0, 0, 1]
self.ob, _, _, _ = self.env.step(0, self.client, action)
else:
break
def get_collision_cone_radius(self):
speed = self.ob.speedX * self.env.default_speed * (1000.0 / 3600.0
) # speed in m/sec
collision_time_window = self.cfg[
"collision_time_window"] if "collision_time_window" in self.cfg else 1
collision_cone_radius = collision_time_window * speed
return collision_cone_radius / 200.0 # Normalizing
def avoid_impending_collision(self):
# If the opponent in front is too close, brake
opponents_in_front = self.get_front_opponents()
closest_front = np.min(opponents_in_front)
frontal_distance_threshold = self.get_collision_cone_radius()
if closest_front < frontal_distance_threshold:
self.brake = 1
else:
self.brake = 0