def find_docking_points_to_rotate(box_current_pose, box_goal_pose, box_length, box_width):
"""Find the docking points on the perimeter of the box required
by the robots in order to rotate the box.
Arguments:
box_current_pose (float[]): current box pose
box_goal_pose (float[]): goal box pose
length (float): box length in meters
width (float): box width in meters
Returns:
A list containing the docking points and the normal directions, one for each robot,
and the best angular position of the box required to push it.
"""
box_pose = [box_current_pose[0], box_current_pose[1]]
box_theta = box_current_pose[2]
# Create a box geometry object
box_geometry = BoxGeometry(box_length, box_width, box_pose, box_theta)
p0_pg = np.array(box_goal_pose) - np.array(box_pose)
los_angle = np.arctan2(p0_pg[1], p0_pg[0])
references = [angle_normalization(los_angle + np.pi / 4),\
angle_normalization(los_angle - np.pi / 4),
angle_normalization(los_angle + np.pi /4 + np.pi / 2),
angle_normalization(los_angle - np.pi /4 - np.pi / 2)]
differences = [angle_normalization(reference - box_theta) for reference in references]
edges = [box_geometry.edge(1,2), box_geometry.edge(0, 1), box_geometry.edge(2, 3)]
normals = [edge.normal() for edge in edges]
for angle in [0, np.pi/2, -np.pi/2, np.pi]:
if np.allclose(differences[0], angle):
results = [{'point' : 0, 'normal' : normals[i]} for i in range(3)]
return False, 0, 0, results
abs_differences = np.array([abs(difference) for difference in differences])
argmin_differences = np.argmin(abs_differences)
direction = 0
if differences[argmin_differences] < 0:
# Clockwise rotation
point_position = [1.0 / 2, 1.0 / 4, 1.0 / 4]
points = [edges[i].point(point_position[i]) for i in range(3)]
direction = - 1
else:
# Counterclockwise rotation
point_position = [1.0 / 2, 3.0 / 4, 3.0 / 4]
points = [edges[i].point(point_position[i]) for i in range(3)]
direction = + 1
results = [{'point' : points[i], 'normal' : normals[i]} for i in range(3)]
return True, references[argmin_differences], direction, results
def find_docking_points_to_rotate(box_current_pose, box_goal_pose, box_length,
box_width):
"""Find the docking points on the perimeter of the box required
by the robots in order to rotate the box.
Arguments:
box_current_pose (float[]): current box pose
box_goal_pose (float[]): goal box pose
length (float): box length in meters
width (float): box width in meters
Returns:
A list containing the docking points and the normal directions, one for each robot,
and the best angular position of the box required to push it.
"""
box_pose = [box_current_pose[0], box_current_pose[1]]
box_theta = box_current_pose[2]
# Create a box geometry object
box_geometry = BoxGeometry(box_length, box_width, box_pose, box_theta)
p0_pg = np.array(box_goal_pose) - np.array(box_pose)
los_angle = np.arctan2(p0_pg[1], p0_pg[0])
references = [angle_normalization(los_angle + np.pi / 4),\
angle_normalization(los_angle - np.pi / 4),
angle_normalization(los_angle + np.pi /4 + np.pi / 2),
angle_normalization(los_angle - np.pi /4 - np.pi / 2)]
differences = [
angle_normalization(reference - box_theta) for reference in references
]
edges = [
box_geometry.edge(1, 2),
box_geometry.edge(0, 1),
box_geometry.edge(2, 3)
]
normals = [edge.normal() for edge in edges]
for angle in [0, np.pi / 2, -np.pi / 2, np.pi]:
if np.allclose(differences[0], angle):
results = [{'point': 0, 'normal': normals[i]} for i in range(3)]
return False, 0, 0, results
abs_differences = np.array([abs(difference) for difference in differences])
argmin_differences = np.argmin(abs_differences)
direction = 0
if differences[argmin_differences] < 0:
# Clockwise rotation
point_position = [1.0 / 2, 1.0 / 4, 1.0 / 4]
points = [edges[i].point(point_position[i]) for i in range(3)]
direction = -1
else:
# Counterclockwise rotation
point_position = [1.0 / 2, 3.0 / 4, 3.0 / 4]
points = [edges[i].point(point_position[i]) for i in range(3)]
direction = +1
results = [{'point': points[i], 'normal': normals[i]} for i in range(3)]
return True, references[argmin_differences], direction, results
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
