def __init__(self, vision=False, throttle=True,
gear_change=False, port=3001, pid_assist=True,
CLIENT_MAX_STEPS=np.inf):
"""Init Method."""
self.pid_assist = pid_assist
if self.pid_assist:
self.action_dim = 2 # LanePos, Velocity
else:
self.action_dim = 3 # Accel, Steer, Brake
TorcsEnv.__init__(self, vision=False, throttle=True, gear_change=False)
self.state_dim = 29 # No. of sensors input
self.env_name = 'Madras_Env'
self.port = port
self.CLIENT_MAX_STEPS = CLIENT_MAX_STEPS
self.client_type = 0 # Snakeoil client type
self.initial_reset = True
self.early_stop = True
if self.pid_assist:
self.PID_latency = 10
else:
self.PID_latency = 1
self.accel_PID = PID(np.array([10.5, 0.05, 2.8])) # accel PID
self.steer_PID = PID(np.array([5.1, 0.001, 0.000001])) # steer PID
self.prev_lane = 0
self.prev_angle = 0
self.prev_vel = 0
self.prev_dist = 0
self.ob = None
self.track_len = 7014.6
def __init__(self, attrs, Ts):
self.kp_phi = 1
self.kd_phi = 0
self.ki_phi = 0
self.delta_a_limit = np.inf
self.delta_e_limit = np.inf
self.kd_phi_tau = 0
self.roll_hold_controller = PID(self.kp_phi, self.ki_phi, self.kd_phi, self.delta_a_limit, Ts, self.kd_phi_tau)
self.kp_chi = 0
self.ki_chi = 0
self.heading_hold_controller = PID(self.kp_chi, self.ki_chi, 0, np.inf, Ts * 1.0, 0)
self.kp_theta = 0
self.ki_theta = 0
self.pitch_hold_controller = PID(self.kp_theta, self.ki_theta, 0, self.delta_e_limit, Ts, 0)
self.kp_h = 1
self.ki_h = 0
self.altitude_hold_controller = PID(self.kp_h, self.ki_h, 0, np.inf, Ts * 1.0, 0)
self.kp_v1 = 1
self.ki_v1 = 0
self.airspeed_hold_with_pitch_controller = PID(self.kp_v1, self.ki_v1, 0, np.inf, Ts, 0)
self.kp_v = 1
self.ki_v = 0
self.airspeed_hold_with_throttle_controller = PID(self.kp_v, self.ki_v, 0, np.inf, Ts, 0)
self.config = attrs
def __init__(self, name, goal, N, net_input, **kwargs):
super().__init__(**kwargs)
self.name = name
self.goal = goal
self.N = N
self.p_distributed_controller = PID(5,
0,
0,
max_out=16.6,
min_out=-16.6)
self.net_input = net_input
def __init__(self, name, index, p, i, d, **kwargs) -> None:
super().__init__(**kwargs)
self.name = name
self.index = index
self.initial_position = None
self.goal_position = None
self.goal_angle = 0
self.dictionary = None
self.colour = None
self.p_distributed_controller = PID(p,
i,
d,
max_out=16.6,
min_out=-16.6)
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 = 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
print(velocity)
for i in range(episode_count):
info = {'termination_cause': 0}
steer = 0.0
accel = 0.0
brake = 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:
break
except Exception as e:
print("Exception caught 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
opp = ob.opponents
front = np.array([opp[15], opp[16], opp[17], opp[18], opp[19]])
closest_front = np.min(front)
print(ob.speedX * 300)
vel_error = velocity - ob.speedX
angle_error = -(ob.trackPos + angle
) / 10 + ob.angle + random.choice([1, -1]) * 0.05
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 < (
(madras.floatX(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))
