def consensus(state, neighbours_state, reference, max_control):
"""Return consensus controller output for a SISO system.
Arguments:
state (float): robot angular state
neighbors_state (float[]): neighbors angular state
reference (float): desired angular state
max_control (float): maximum allowed output
"""
neighbours_state.append(reference)
output = sum([correct_delta(neighbour_state - state) for neighbour_state in neighbours_state])
saturated_output = saturation(output, max_control)
return saturated_output
def proportional_control(k_p, r, y, u_max, avoid_overturning):
"""Implement proportional control law.
Arguments:
k_p (float): Proportional gain
r (float): reference signal
y (float): system output signal
u_max (float): maximum control effort
avoid_overturning (bool): if True avoids rotation greater than pi
"""
error = r - y
if avoid_overturning:
if abs(error) > np.pi:
error += -2 * np.pi * np.sign(error)
u = k_p * error
saturated_u = saturation(u ,u_max)
return saturated_u
def control_law(self, robot_state, robot_velocity, obstacle_avoidance = False, robots_state = [], robots_velocity = [], controller_index = 0):
"""Return controller output
Arguments:
robot_state (float[]): robot state (x, y, theta)
robot_velocity (float): forward robot velocity
obstacle_avoidance (boolean): enable obstacle avoidance mode
robots_state (float[[]]): other robots state necessary in obstacle avoidance mode
robots_velocity (float[]): other forward robots velocity necessary in obstacle avoidance mode
"""
xr = robot_state[0]
yr = robot_state[1]
theta = robot_state[2]
vr = robot_velocity
k = 1
rho_rg = np.sqrt((xr - self.xg) ** 2 + (yr - self.yg) ** 2)
phi_rg = np.arctan2((self.yg - yr), (self.xg - xr))
phi_rg_dot = -(vr * np.sin(theta - phi_rg)) / rho_rg
# Obstacle avoidance mode
if obstacle_avoidance:
rho_ros = [np.sqrt((robot_state[0] - xr) ** 2 + (robot_state[1] - yr) ** 2) for robot_state in robots_state]
# Nearest obstacle
index_min = np.argmin(rho_ros)
rho_ro = rho_ros[index_min]
x_ob = robots_state[index_min][0]
y_ob = robots_state[index_min][1]
theta_ob = robots_state[index_min][2]
v_ob = robots_velocity[index_min]
phi_ro = np.arctan2((yr - y_ob), (xr - x_ob))
phi_ro_dot = (-(v_ob * np.sin(theta_ob - phi_ro)) +\
(vr * np.sin(theta - phi_ro))) / rho_ro
if rho_ro < 2 * self.robot_radius + 0.2:
delta = utils.angle_normalization(theta - phi_ro)+ phi_ro_dot
forward_velocity = 0
if abs(delta) < np.pi / 18:
forward_velocity = 0.1
angular_velocity = -k * delta + phi_ro_dot
saturated_forward_velocity = utils.saturation(forward_velocity, self.max_forward_velocity)
saturated_angular_velocity = utils.saturation(angular_velocity, self.max_angular_velocity)
return False, saturated_forward_velocity, saturated_angular_velocity
if rho_rg > 0.01:
forward_velocity = rho_rg
if obstacle_avoidance:
forward_velocity = 0.2
if controller_index == 2:
forward_velocity = 0.16
if rho_rg < 0.2:
forward_velocity = rho_rg
angular_velocity = -k * utils.angle_normalization(theta - phi_rg) + phi_rg_dot
saturated_forward_velocity = utils.saturation(forward_velocity, self.max_forward_velocity)
saturated_angular_velocity = utils.saturation(angular_velocity, self.max_angular_velocity)
return False, saturated_forward_velocity, saturated_angular_velocity
return True, 0, 0
def control_law(self,
robot_state,
robot_velocity,
obstacle_avoidance=False,
robots_state=[],
robots_velocity=[],
controller_index=0):
"""Return controller output
Arguments:
robot_state (float[]): robot state (x, y, theta)
robot_velocity (float): forward robot velocity
obstacle_avoidance (boolean): enable obstacle avoidance mode
robots_state (float[[]]): other robots state necessary in obstacle avoidance mode
robots_velocity (float[]): other forward robots velocity necessary in obstacle avoidance mode
"""
xr = robot_state[0]
yr = robot_state[1]
theta = robot_state[2]
vr = robot_velocity
k = 1
rho_rg = np.sqrt((xr - self.xg)**2 + (yr - self.yg)**2)
phi_rg = np.arctan2((self.yg - yr), (self.xg - xr))
phi_rg_dot = -(vr * np.sin(theta - phi_rg)) / rho_rg
# Obstacle avoidance mode
if obstacle_avoidance:
rho_ros = [
np.sqrt((robot_state[0] - xr)**2 + (robot_state[1] - yr)**2)
for robot_state in robots_state
]
# Nearest obstacle
index_min = np.argmin(rho_ros)
rho_ro = rho_ros[index_min]
x_ob = robots_state[index_min][0]
y_ob = robots_state[index_min][1]
theta_ob = robots_state[index_min][2]
v_ob = robots_velocity[index_min]
phi_ro = np.arctan2((yr - y_ob), (xr - x_ob))
phi_ro_dot = (-(v_ob * np.sin(theta_ob - phi_ro)) +\
(vr * np.sin(theta - phi_ro))) / rho_ro
if rho_ro < 2 * self.robot_radius + 0.2:
delta = utils.angle_normalization(theta - phi_ro) + phi_ro_dot
forward_velocity = 0
if abs(delta) < np.pi / 18:
forward_velocity = 0.1
angular_velocity = -k * delta + phi_ro_dot
saturated_forward_velocity = utils.saturation(
forward_velocity, self.max_forward_velocity)
saturated_angular_velocity = utils.saturation(
angular_velocity, self.max_angular_velocity)
return False, saturated_forward_velocity, saturated_angular_velocity
if rho_rg > 0.01:
forward_velocity = rho_rg
if obstacle_avoidance:
forward_velocity = 0.2
if controller_index == 2:
forward_velocity = 0.16
if rho_rg < 0.2:
forward_velocity = rho_rg
angular_velocity = -k * utils.angle_normalization(
theta - phi_rg) + phi_rg_dot
saturated_forward_velocity = utils.saturation(
forward_velocity, self.max_forward_velocity)
saturated_angular_velocity = utils.saturation(
angular_velocity, self.max_angular_velocity)
return False, saturated_forward_velocity, saturated_angular_velocity
return True, 0, 0
