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 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
