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, 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 __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)
class Autopilot:
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)
def compute_delta_a(self, phi_c, phi, *args):
return self.roll_hold_controller.compute_control_input(phi_c, phi, *args)
def compute_roll(self, chi_c, chi):
return self.heading_hold_controller.compute_control_input(chi_c, chi)
def compute_delta_e(self, theta_c, theta, *args):
return self.pitch_hold_controller.compute_control_input(theta_c, theta, *args)
def compute_pitch(self, h_c, h, *args):
return self.altitude_hold_controller.compute_control_input(h_c, h, *args)
def compute_pitch_for_airspeed(self, Va_c, Va, *args):
return self.airspeed_hold_with_pitch_controller.compute_control_input(Va_c, Va, *args)
def compute_throttle_for_airspeed(self, Va_c, Va, *args):
return self.airspeed_hold_with_throttle_controller.compute_control_input(Va_c, Va, *args)
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))
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
class Autopilot(object):
__metaclass__ = ABCMeta
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 compute_delta_a(self, phi_c, phi, *args):
return self.roll_hold_controller.compute_control_input(phi_c, phi, *args)
def compute_roll(self, chi_c, chi):
return self.heading_hold_controller.compute_control_input(chi_c, chi)
def compute_delta_e(self, theta_c, theta, *args):
return self.pitch_hold_controller.compute_control_input(theta_c, theta, *args)
def compute_pitch(self, h_c, h, *args):
return self.altitude_hold_controller.compute_control_input(h_c, h, *args)
def compute_pitch_for_airspeed(self, Va_c, Va, *args):
return self.airspeed_hold_with_pitch_controller.compute_control_input(Va_c, Va, *args)
def compute_throttle_for_airspeed(self, Va_c, Va, *args):
return self.airspeed_hold_with_throttle_controller.compute_control_input(Va_c, Va, *args)
@abstractmethod
def set_pitch(self, pitch_c): pass
@abstractmethod
def set_roll(self, roll_c): pass
@abstractmethod
def get_roll_for_heading(self, chi_c): pass
@abstractmethod
def get_throttle_for_airspeed(self, Va_c): pass
@abstractmethod
def get_pitch_for_airspeed(self, Va_c): pass
@abstractmethod
def get_pitch_for_altitude(self, h_c): pass
@abstractmethod
def set_throttle(self, throttle_c): pass
class ManualController:
"""
The robots are moved following a “distributed controller”, that is a simple proportional controller PID,
with P fixed to -0.01, whose goal is to align the robots by minimizing the difference between the values
recorded by the front and rear sensors.
:param name: name of the controller used (in this case manual)
:param goal: task to perform (in this case distribute)
:param N: number of agents in the simulation
:param net_input: input of the network (between: prox_values, prox_comm and all_sensors)
:param kwargs: other arguments
:var p_distributed_controller: a simple proportional controller that returns the speed to apply
"""
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 neighbors_distance(self, state):
"""
Check if there is a robot ahead using the infrared sensor 2 (front-front)
[2 and 9 in case of all sensors].
Check if there is a robot ahead using the infrared sensor 5 (back-left) and 6 (back-right)
[5, 6 and 12, 13 in case of all sensors].
The distance from the neighbors is computed using a mapping function obtained experimentally that converts the
intensities values contained in the sensor readings into distances.
.. note:: The response values are actually intensities: the front correspond to the
frontal center sensor and the back to the mean of the response values of the rear sensors.
:param state: object containing the agent information
:return back, front: back distance and front distance of the thymio from the others
"""
sensing = utils.get_input_sensing(self.net_input,
state,
normalise=False)
if sensing == [0] * len(sensing):
return 0, 0
file = os.path.join('source/controllers', 'sensing_to_distances.pkl')
distances, front_prox_values, back_prox_values, front_prox_comm, back_prox_comm = np.load(
file, allow_pickle=True)
f_pv_indices = np.nonzero(front_prox_values)[0]
f_pv_indices = f_pv_indices[np.argsort(
front_prox_values[f_pv_indices])]
b_pv_indices = np.nonzero(back_prox_values)[0]
b_pv_indices = b_pv_indices[np.argsort(back_prox_values[b_pv_indices])]
f_pc_indices = np.nonzero(front_prox_comm)[0]
f_pc_indices = f_pc_indices[np.argsort(front_prox_comm[f_pc_indices])]
b_pc_indices = np.nonzero(back_prox_comm)[0]
b_pc_indices = b_pc_indices[np.argsort(back_prox_comm[b_pc_indices])]
front_distance_pv = np.interp(sensing[2],
front_prox_values[f_pv_indices],
distances[f_pv_indices])
back_distance_pv = np.interp(
np.mean(np.array([sensing[5], sensing[6]])),
back_prox_values[b_pv_indices], distances[b_pv_indices])
if sensing[7:] == [0] * 7 or self.net_input != 'all_sensors':
return back_distance_pv, front_distance_pv
elif sensing[0:7] == [0] * 7:
front_distance_pc = np.interp(sensing[9],
front_prox_comm[f_pc_indices],
distances[f_pc_indices])
back_distance_pc = np.interp(
np.mean(np.array([sensing[12], sensing[13]])),
back_prox_comm[b_pc_indices], distances[b_pc_indices])
return back_distance_pc, front_distance_pc
elif self.net_input == 'all_sensors':
front_distance_pc = np.interp(sensing[9],
front_prox_comm[f_pc_indices],
distances[f_pc_indices])
back_distance_pc = np.interp(
np.mean(np.array([sensing[12], sensing[13]])),
back_prox_comm[b_pc_indices], distances[b_pc_indices])
if sensing[2] == 0.0:
front = front_distance_pc
else:
front = front_distance_pv
if np.mean([sensing[5], sensing[6]]) == 0.0:
back = back_distance_pc
elif np.mean([sensing[12], sensing[13]]) == 0.0:
back = back_distance_pv
else:
back = np.mean([back_distance_pv, back_distance_pc])
return back, front
else:
raise ValueError('Impossible values for sensing.')
def compute_difference(self, state):
"""
.. note:: Apply a small correction to the distance measured by the rear sensors since the front sensor used
is at a different x coordinate from the point to which the rear sensor of the robot that follows
points. This is because of the curved shape of the face of the Thymio.
The final difference is computed as follow:
.. math:: out = front - (back - correction)
:param state: object containing the agent information
:return out: the difference between the front and the rear distances
"""
back, front = self.neighbors_distance(state)
if back == 0 and front == 0:
return 0
# compute the correction
delta_x = 7.41150769
x = 7.95
# Maximum possible response values
# delta_x_m = 4505 * delta_x / 14
# x_m = 4505 * x / 14
# correction = x_m - delta_x_m
correction = x - delta_x
out = front - (back - correction)
return out
def perform_control(self, state, dt):
"""
Keeping still the robots at the ends of the line, moves the others using the distributed controller,
setting the ``target {left, right} wheel speed`` each at the same value in order to moves the robot
straight ahead.
This distributed controller is a simple proportional controller ``PID(5, 0, 0, max_out=16.6, min_out=-16.6)``
that takes in input the difference between the front and back distances measured by the agent.
:param state: object containing the agent information
:param dt: control step duration
:return speed, communication: the velocity and the message to communicate
"""
# Don't move the first and last robots in the line
# if state.index == 0 or state.index == self.N - 1:
if np.isclose(round(state.goal_position[0], 2),
round(state.initial_position[0], 2),
rtol=1e-2):
return 0, 0
else:
speed = self.p_distributed_controller.step(
self.compute_difference(state), dt)
return speed, 0
class MadrasEnv(TorcsEnv):
"""Definition of the Gym Madras Env."""
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 reset(self, prev_step_info=None):
"""Reset Method. To be called at the end of each episode"""
if self.initial_reset:
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(step=0)
# Get the initial input from torcs
raw_ob = self.client.S.d
# Get the current full-observation
self.ob = self.make_observation(raw_ob)
except:
pass
self.initial_reset = False
else:
try:
if 'termination_cause' in list(prev_step_info.keys()) and\
prev_step_info['termination_cause'] == 'hardReset':
self.ob, self.client =\
TorcsEnv.reset(self, client=self.client, relaunch=True)
else:
self.ob, self.client =\
TorcsEnv.reset(self, client=self.client, relaunch=True)
except Exception as 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(step=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
self.distance_traversed = 0
s_t = 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_t
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:
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 Thymio(pyenki.Thymio2):
"""
Superclass: `pyenki.Thymio2` -> the world update step will automatically call the Thymio `controlStep`.
:param name: name of the agent
:param index: index of the agents in the row
:param p: proportional term of the PID controller
:param i: integral term of the PID controller
:param d: derivative term of the PID controller
:param kwargs: other arguments
:var initial_position: the initial position of the agent is set to None
:var goal_position: the goal position of the agent is set to None
:var goal_angle: the goal angle of the agent is set to None
:var dictionary: the dictionary containing all the agent attributes is set to None
:var colour: the colour of the agent is set to None
:var p_distributed_controller: a simple proportional controller that returns the speed to apply
"""
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 get_input_sensing(self):
"""
:return sensing: the sensing perceived by the robot based on the net input
"""
if len(self.prox_comm_events) == 0:
prox_comm = {'sender': {'intensities': [0, 0, 0, 0, 0, 0, 0]}}
else:
prox_comm = utils.get_prox_comm(self)
prox_values = getattr(self, 'prox_values').copy()
sensing = utils.get_all_sensors(prox_values, prox_comm)
return sensing
def controlStep(self, dt: float) -> None:
"""
Perform one control step:
Compute the error as the difference between the goal and the actual position
and use it to compute the velocity of the robot through a proportional controller.
:param dt: control step duration
"""
error = self.goal_position[0] - self.position[0]
speed = self.p_distributed_controller.step(error, dt)
self.motor_right_target = speed
self.motor_left_target = speed
