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