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