def find_docking_points_to_push(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 move the box to the goal position. 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. """ singularity = False 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) # Test if (box_goal_pose - box_pose) is parallel to an edge of the box # difference = np.array(box_goal_pose) - np.array(box_pose) # angle_between = angle_normalization(np.arctan2(difference[1], difference[0]) - box_theta) # tolerance = 0.1 # for angle in [0, + np.pi / 2, - np.pi / 2, np.pi]: # if abs(angle_between - angle) < tolerance: # singularity = True # break # Calculate the distances between the goal and the vertices distances = [ np.linalg.norm(np.array(box_goal_pose) - np.array(v)) for v in box_geometry.vertices() ] # Take into account the indexes of the vertices dist_indexed = [(i, distances[i]) for i in range(4)] # Sort the distances dist_ordered = sorted(dist_indexed, key=lambda item: item[1], reverse=True) # Indexes of interest argmax0 = dist_ordered[0][0] argmax1 = dist_ordered[1][0] argmax2 = dist_ordered[2][0] pair_01 = [argmax0, argmax1] pair_01.sort() pair_02 = [argmax0, argmax2] pair_02.sort() # In case of pair_0i[0] == 0 and pair__0i[1] == 3 they should be reversed if pair_01[0] == 0 and pair_01[1] == 3: pair_01.reverse() if pair_02[0] == 0 and pair_02[1] == 3: pair_02.reverse() points = [] edges = [] # Find docking points and normals if singularity == True: edges.append(box_geometry.edge(*pair_01)) edges.append( box_geometry.edge((pair_01[0] - 1) % 4, (pair_01[1] - 1) % 4)) edges.append( box_geometry.edge((pair_01[0] + 1) % 4, (pair_01[1] + 1) % 4)) points = [1.0 / 2] * 3 else: shorter_edge = box_geometry.edge(*pair_01) longer_edge = box_geometry.edge(*pair_02) if shorter_edge >= longer_edge: shorter_edge, longer_edge = longer_edge, shorter_edge edges.append(shorter_edge) edges.append(longer_edge) edges.append(longer_edge) points = [1.0 / 2, 1.0 / 4, 3.0 / 4] return [{ 'point': edges[i].point(points[i]), 'normal': edges[i].normal() } for i in range(3)]
def find_docking_points_to_push(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 move the box to the goal position. 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. """ singularity = False 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) # Test if (box_goal_pose - box_pose) is parallel to an edge of the box # difference = np.array(box_goal_pose) - np.array(box_pose) # angle_between = angle_normalization(np.arctan2(difference[1], difference[0]) - box_theta) # tolerance = 0.1 # for angle in [0, + np.pi / 2, - np.pi / 2, np.pi]: # if abs(angle_between - angle) < tolerance: # singularity = True # break # Calculate the distances between the goal and the vertices distances = [np.linalg.norm(np.array(box_goal_pose) - np.array(v)) for v in box_geometry.vertices()] # Take into account the indexes of the vertices dist_indexed = [(i, distances[i]) for i in range(4)] # Sort the distances dist_ordered = sorted(dist_indexed, key = lambda item : item[1], reverse = True) # Indexes of interest argmax0 = dist_ordered[0][0] argmax1 = dist_ordered[1][0] argmax2 = dist_ordered[2][0] pair_01 = [argmax0, argmax1] pair_01.sort() pair_02 = [argmax0, argmax2] pair_02.sort() # In case of pair_0i[0] == 0 and pair__0i[1] == 3 they should be reversed if pair_01[0] == 0 and pair_01[1] == 3: pair_01.reverse() if pair_02[0] == 0 and pair_02[1] == 3: pair_02.reverse() points = [] edges = [] # Find docking points and normals if singularity == True: edges.append(box_geometry.edge(*pair_01)) edges.append(box_geometry.edge((pair_01[0] - 1) % 4, (pair_01[1] - 1) % 4)) edges.append(box_geometry.edge((pair_01[0] + 1) % 4, (pair_01[1] + 1) % 4)) points = [1.0 / 2] * 3 else: shorter_edge = box_geometry.edge(*pair_01) longer_edge = box_geometry.edge(*pair_02) if shorter_edge >= longer_edge: shorter_edge, longer_edge = longer_edge, shorter_edge edges.append(shorter_edge) edges.append(longer_edge) edges.append(longer_edge) points = [1.0 / 2, 1.0 / 4, 3.0 / 4] return [{'point' : edges[i].point(points[i]) , 'normal' : edges[i].normal() } for i in range(3)]