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)