def test_transform(self):
transform = Transform()
transform.translate([1.,1.,1.])
self.polygon.transform(transform)
self.assertTrue(np.allclose(np.matrix('1.,3.,3.,1.; 1. 1. 3. 3. ; 1. 1. 1. 1. '), self.polygon.matrix_of_vertices))
def draw_docking_station(ax, goal_object='station'):
"""
:param ax:
:param goal_object
"""
obj_tform = Transform.scale(20) @ Transform.translate(-0.85, 0)
if goal_object == 'station':
obj_points = Point.from_list([(0, 1, 1), (2, 1, 1), (2, 0.5, 1),
(0.5, 0.5, 1), (0.5, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, -1, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
ax.add_patch(
plt.Polygon(obj_points,
facecolor=colors.to_rgba([0, 0.5, 0.5], alpha=0.5),
edgecolor=[0, 0.5, 0.5],
linewidth=1.5))
elif goal_object == 'coloured_station':
face_colours = [[0.839, 0.153, 0.157], [1.000, 0.895, 0.201],
[0.173, 0.627, 0.173]]
obj_points = Point.from_list([(0, 1, 1), (0, 0.5, 1), (2, 0.5, 1),
(2, 1, 1), (0, -1, 1), (0, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, 0.5, 1),
(0, -0.5, 1), (0.5, -0.5, 1),
(0.5, 0.5, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
arm1_points = obj_points[:4]
arm2_points = obj_points[4:8]
back_points = obj_points[8:]
ax.add_patch(
plt.Polygon(back_points,
facecolor=colors.to_rgba(face_colours[1], alpha=0.5),
edgecolor=face_colours[1],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm1_points,
facecolor=colors.to_rgba(face_colours[0], alpha=0.5),
edgecolor=face_colours[0],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm2_points,
facecolor=colors.to_rgba(face_colours[2], alpha=0.5),
edgecolor=face_colours[2],
linewidth=1.5))
else:
raise ValueError("Invalid value for goal_object")
class TestTransform(unittest.TestCase):
def setUp(self):
self.transform = Transform()
self.translation = [1.,1.,1.]
def test_translation(self):
self.transform.translate(self.translation)
reference = np.matrix('1., 0., 1.; 0., 1., 1.; 0., 0., 1.')
tested = self.transform.matrix
self.assertTrue(np.allclose(reference, tested))
def test_rotation(self):
self.transform.rotate(90.)
reference = np.matrix('0., -1., 0.; 1., 0., 0.; 0., 0., 1.')
self.assertTrue(np.allclose(reference, self.transform.matrix))
def init_positions(cls,
marxbot,
d_object,
marxbot_rel_pose,
min_distance=8.5):
"""
:param marxbot
:param d_object
:param marxbot_rel_pose: initial pose of the marxbot, relative to the goal pose, expressed as r, theta and
angle, that is position in polar coordinates and orientation
:param min_distance: the minimum distance between the marXbot and any object, that correspond to the radius
of the marXbot, is 8.5 cm.
"""
# The goal pose of the marXbot, defined with respect to the docking station reference frame, has the same y-axis
# of the docking station and the x-axis translated of the radius of the d_object plus a small arbitrary distance.
# The goal angle of the marXbot is 180 degree (π).
increment = d_object.radius + min_distance
x_goal = d_object.position[0] + increment
y_goal = d_object.position[1] + 0
marxbot.goal_position = (x_goal, y_goal)
marxbot.goal_angle = np.pi
# Transform the initial pose, relative to the goal, from polar to cartesian coordinates
r, theta, angle = marxbot_rel_pose
point_G = Point.from_polar(r, theta)
# Transform the cartesian coordinates from the goal to the world reference frame
trasform_D_G = Transform.translate(increment, 0)
trasform_W_D = Transform.pose_transform(d_object.position,
d_object.angle)
point_W = trasform_W_D @ trasform_D_G @ point_G
marxbot.initial_position = tuple(Point.to_euclidean(point_W))
marxbot.position = marxbot.initial_position
marxbot.initial_angle = angle
marxbot.angle = marxbot.initial_angle
marxbot.goal_reached = False
def f(translation, rotation):
t = Transform()
t.rotate(rotation)
t.translate(translation)
return area_of_intersection(t)
