Exemple #1
0
    def __init__(self):

        self.fi_1_limit = [-np.pi / 2, np.pi / 2]
        self.fi_2_limit = [-11 / 36 * np.pi, 25 / 36 * np.pi]
        self.fi_3_limit = [0, 5 / 6 * np.pi]
        self.fi_1 = 0
        self.fi_2 = 0.0
        self.fi_3 = 0.0
        self.l1 = 203 / 3
        self.l2 = 178 / 3
        self.l3 = 178 / 3
        self.step = 0.2

        self.r_z_fi1 = matrix_library.rotation('z', 0)
        self.r_y_fi2 = matrix_library.rotation('y', 0)
        self.r_y_fi3 = matrix_library.rotation('y', 0)

        self.t_z_l1 = matrix_library.transition('z', self.l1)
        self.t_z_l2 = matrix_library.transition('z', self.l2)

        self.A_coord = np.matmul(self.r_z_fi1,
                                 matrix_library.coord_vector(self.l1))
        self.B_coord = np.matmul(
            np.matmul(self.r_z_fi1, self.t_z_l1),
            np.matmul(self.r_y_fi2, matrix_library.coord_vector(self.l2)))
        C_coord_temp = np.matmul(np.matmul(self.r_z_fi1, self.t_z_l1),
                                 np.matmul(self.r_y_fi2, self.t_z_l2))
        self.C_coord = np.matmul(
            C_coord_temp,
            np.matmul(self.r_y_fi3, matrix_library.coord_vector(self.l3)))

        self.trajectory_graph1 = []
        self.trajectory_graph2 = []
        self.points_graph_1 = []
        self.points_graph_2 = []
        self.loop = True
        self.old_fi_1 = self.fi_1
        self.proj_mat = matrix_library.projection()

        self.width = 1000.0
        self.height = 1000.0
        self.distance = 5.0
        self.scale = 200
        self.position = [750, 250]

        # Draw related stuff
        self.my_font1 = pygame.font.SysFont("monospace", 30)
Exemple #2
0
    def update(self):

        old_A = self.A_coord
        old_B = self.B_coord
        old_C = self.C_coord
        self.old_fi_1 = self.fi_1

        self.r_z_fi1 = matrix_library.rotation('z', self.fi_1)
        self.r_y_fi2 = matrix_library.rotation('y', self.fi_2)
        self.r_y_fi3 = matrix_library.rotation('y', self.fi_3)

        self.t_z_l1 = matrix_library.transition('z', self.l1)
        self.t_z_l2 = matrix_library.transition('z', self.l2)

        self.A_coord = np.matmul(self.r_z_fi1,
                                 matrix_library.coord_vector(self.l1))
        self.B_coord = np.matmul(
            np.matmul(self.r_z_fi1, self.t_z_l1),
            np.matmul(self.r_y_fi2, matrix_library.coord_vector(self.l2)))
        C_coord_temp = np.matmul(np.matmul(self.r_z_fi1, self.t_z_l1),
                                 np.matmul(self.r_y_fi2, self.t_z_l2))
        self.C_coord = np.matmul(
            C_coord_temp,
            np.matmul(self.r_y_fi3, matrix_library.coord_vector(self.l3)))

        # ordered iterating
        if self.loop:
            self.fi_1 = self.fi_1 + self.step
            if self.fi_1 > self.fi_1_limit[1]:
                self.fi_1 = self.fi_1_limit[0]
                self.fi_2 = self.fi_2 + self.step
                if self.fi_2 > self.fi_2_limit[1]:
                    self.fi_2 = self.fi_2_limit[0]
                    self.fi_3 = self.fi_3 + self.step
                    if self.fi_3 > self.fi_3_limit[1]:
                        self.loop = False

        return old_A, old_B, old_C
    def update_arm(self, fi1, fi2, fi3):

        # If fi_1 is not passed to function, use old fi_1
        if fi1 is None:
            fi1 = self.old_fi_1

        # If fi_12 is not passed to function, use old fi_2
        if fi2 is None:
            fi2 = self.old_fi_2

        # If fi_3 is not passed to function, use old fi_3
        if fi3 is None:
            fi3 = self.old_fi_3

        # Rotation matrices
        r_z_fi1 = matrix_library.rotation('z', fi1)
        r_y_fi2 = matrix_library.rotation('y', fi2)
        r_y_fi3 = matrix_library.rotation('y', fi3)

        # Translation matrices
        t_z_l1 = matrix_library.transition('z', self.l1)
        t_z_l2 = matrix_library.transition('z', self.l2)

        # Compute new coordinates
        self.A_coord = np.matmul(r_z_fi1, matrix_library.coord_vector(self.l1))
        self.B_coord = np.matmul(
            np.matmul(r_z_fi1, t_z_l1),
            np.matmul(r_y_fi2, matrix_library.coord_vector(self.l2)))
        C_coord_temp = np.matmul(np.matmul(r_z_fi1, t_z_l1),
                                 np.matmul(r_y_fi2, t_z_l2))
        self.C_coord = np.matmul(
            C_coord_temp,
            np.matmul(r_y_fi3, matrix_library.coord_vector(self.l3)))

        # Saving old angles
        self.old_fi_1 = fi1
        self.old_fi_2 = fi2
        self.old_fi_3 = fi3
    def create_edge(self):
        data_C_obalka_1 = []
        data_C_obalka_2 = []
        temp_list = []

        for k in np.linspace(
                self.fi_3_limit[0], self.fi_3_limit[1],
                int((self.fi_3_limit[1] - self.fi_3_limit[0]) / self.step) +
                1):
            for j in np.linspace(
                    self.fi_2_limit[0], self.fi_2_limit[1],
                    int((self.fi_2_limit[1] - self.fi_2_limit[0]) / self.step)
                    + 1):
                for i in np.linspace(
                        self.fi_1_limit[0], self.fi_1_limit[1],
                        int((self.fi_1_limit[1] - self.fi_1_limit[0]) /
                            self.step) + 1):

                    r_z_fi1 = matrix_library.rotation('z', i)
                    r_y_fi2 = matrix_library.rotation('y', j)
                    r_y_fi3 = matrix_library.rotation('y', k)

                    t_z_l1 = matrix_library.transition('z', self.l1)
                    t_z_l2 = matrix_library.transition('z', self.l2)

                    C_coord_temp = np.matmul(np.matmul(r_z_fi1, t_z_l1),
                                             np.matmul(r_y_fi2, t_z_l2))
                    C_coord = np.matmul(
                        C_coord_temp,
                        np.matmul(r_y_fi3,
                                  matrix_library.coord_vector(self.l3)))

                    # append only if point is on the edge

                    # horizontal edge map for fi_2 > 0
                    if (abs(j - (np.pi / 2)) < self.step) or (
                            abs(j - self.fi_2_limit[0]) < self.step):
                        if abs(k - 0) < self.step:
                            data_C_obalka_1.append(C_coord)

                    # horizontal edge map for fi_2 > 0
                    if abs(i - self.fi_1_limit[0]) < self.step or abs(
                            i - self.fi_1_limit[1]) < self.step:
                        data_C_obalka_1.append(C_coord)

                    # vertical edge map for every fi_3 == 0
                    if abs(k - 0) < self.step:
                        if abs(i - 0) < self.step:
                            data_C_obalka_2.append(C_coord)

                    # vertical edge map for every fi_3 when fi_2 == max
                    if abs(j - self.fi_2_limit[1]) < self.step:
                        if abs(i - 0) < self.step:
                            data_C_obalka_2.append(C_coord)

                    # vertical edge map for every fi_3 when fi_2 == min
                    if abs(j - self.fi_2_limit[0]) < self.step:
                        if abs(i - 0) < self.step:
                            temp_list.append(C_coord)

        temp_list = self.reverse(temp_list)
        data_C_obalka_2.extend(temp_list)

        for c in data_C_obalka_1:
            self.xvalues_C_obalka_1.append(
                c[0]), self.yvalues_C_obalka_1.append(
                    c[1]), self.zvalues_C_obalka_1.append(0)

        for c in data_C_obalka_2:
            self.xvalues_C_obalka_2.append(0), self.yvalues_C_obalka_2.append(
                c[1]), self.zvalues_C_obalka_2.append(c[2])
    def plot_arm(self, ax, angle):

        # Plot obalka if radio button is selected for it
        if self.obalka:
            ax.plot(self.xvalues_C_obalka_1,
                    self.yvalues_C_obalka_1,
                    self.zvalues_C_obalka_1,
                    color='blue')
            ax.plot(self.xvalues_C_obalka_1,
                    self.yvalues_C_obalka_1,
                    self.zvalues_C_obalka_1,
                    color='blue')
            ax.plot(self.xvalues_C_obalka_2,
                    self.yvalues_C_obalka_2,
                    self.zvalues_C_obalka_2,
                    color='green')

        # Plot joins
        ax.scatter3D([self.A_coord[0]], [self.A_coord[1]], [self.A_coord[2]],
                     'o',
                     color='red',
                     s=34)
        ax.scatter3D([self.B_coord[0]], [self.B_coord[1]], [self.B_coord[2]],
                     'o',
                     color='red',
                     s=34)
        ax.scatter3D([self.C_coord[0]], [self.C_coord[1]], [self.C_coord[2]],
                     'o',
                     color='red',
                     s=34)

        # Plot arms
        ax.plot([0, self.A_coord[0]], [0, self.A_coord[1]],
                [0, self.A_coord[2]],
                color='black',
                linewidth=4)
        ax.plot([self.A_coord[0], self.B_coord[0]],
                [self.A_coord[1], self.B_coord[1]],
                [self.A_coord[2], self.B_coord[2]],
                color='black',
                linewidth=4)
        ax.plot([self.B_coord[0], self.C_coord[0]],
                [self.B_coord[1], self.C_coord[1]],
                [self.B_coord[2], self.C_coord[2]],
                color='black',
                linewidth=4)

        # Plot coordinate axes
        if self.plotted_axes[0]:
            ax.plot([0, 0], [0, 0], [0, 80], color='green', linewidth=2)
            ax.plot([0, 0], [0, 80], [0, 0], color='yellow', linewidth=2)
            ax.plot([0, 80], [0, 0], [0, 0], color='blue', linewidth=2)

        if self.plotted_axes[1]:
            r = matrix_library.rotation('z', self.old_fi_1)
            a0_y = np.matmul(r, [0, 80, 0, 1])
            a0_x = np.matmul(r, [80, 0, 0, 1])
            a0_z = np.matmul(r, [0, 0, 80, 1])
            ax.plot([0, a0_z[0]], [0, a0_z[1]], [0, a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([0, a0_y[0]], [0, a0_y[1]], [0, a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([0, a0_x[0]], [0, a0_x[1]], [0, a0_x[2]],
                    color='blue',
                    linewidth=2)

        if self.plotted_axes[2]:
            r1 = matrix_library.rotation('z', self.old_fi_1)
            a0_y = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                [0, 80, 0, 1])
            a0_x = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                [80, 0, 0, 1])
            a0_z = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                [0, 0, 80, 1])
            ax.plot([self.A_coord[0], a0_z[0]], [self.A_coord[1], a0_z[1]],
                    [self.A_coord[2], a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([self.A_coord[0], a0_y[0]], [self.A_coord[1], a0_y[1]],
                    [self.A_coord[2], a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([self.A_coord[0], a0_x[0]], [self.A_coord[1], a0_x[1]],
                    [self.A_coord[2], a0_x[2]],
                    color='blue',
                    linewidth=2)

        if self.plotted_axes[3]:
            r1 = matrix_library.rotation('z', self.old_fi_1)
            r2 = matrix_library.rotation('y', self.old_fi_2)
            a0_y = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(r2, [0, 80, 0, 1]))
            a0_x = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(r2, [80, 0, 0, 1]))
            a0_z = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(r2, [0, 0, 80, 1]))
            ax.plot([self.A_coord[0], a0_z[0]], [self.A_coord[1], a0_z[1]],
                    [self.A_coord[2], a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([self.A_coord[0], a0_y[0]], [self.A_coord[1], a0_y[1]],
                    [self.A_coord[2], a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([self.A_coord[0], a0_x[0]], [self.A_coord[1], a0_x[1]],
                    [self.A_coord[2], a0_x[2]],
                    color='blue',
                    linewidth=2)

        if self.plotted_axes[4]:
            r1 = matrix_library.rotation('z', self.old_fi_1)
            r2 = matrix_library.rotation('y', self.old_fi_2)
            a0_y = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    [0, 80, 0, 1]))
            a0_x = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    [80, 0, 0, 1]))
            a0_z = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    [0, 0, 80, 1]))
            ax.plot([self.B_coord[0], a0_z[0]], [self.B_coord[1], a0_z[1]],
                    [self.B_coord[2], a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([self.B_coord[0], a0_y[0]], [self.B_coord[1], a0_y[1]],
                    [self.B_coord[2], a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([self.B_coord[0], a0_x[0]], [self.B_coord[1], a0_x[1]],
                    [self.B_coord[2], a0_x[2]],
                    color='blue',
                    linewidth=2)

        if self.plotted_axes[5]:
            r1 = matrix_library.rotation('z', self.old_fi_1)
            r2 = matrix_library.rotation('y', self.old_fi_2)
            r3 = matrix_library.rotation('y', self.old_fi_3)
            a0_y = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(r3, [0, 80, 0, 1])))
            a0_x = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(r3, [80, 0, 0, 1])))
            a0_z = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(r3, [0, 0, 80, 1])))
            ax.plot([self.B_coord[0], a0_z[0]], [self.B_coord[1], a0_z[1]],
                    [self.B_coord[2], a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([self.B_coord[0], a0_y[0]], [self.B_coord[1], a0_y[1]],
                    [self.B_coord[2], a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([self.B_coord[0], a0_x[0]], [self.B_coord[1], a0_x[1]],
                    [self.B_coord[2], a0_x[2]],
                    color='blue',
                    linewidth=2)

        if self.plotted_axes[6]:
            r1 = matrix_library.rotation('z', self.old_fi_1)
            r2 = matrix_library.rotation('y', self.old_fi_2)
            r3 = matrix_library.rotation('y', self.old_fi_3)
            a0_y = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(
                        np.matmul(r3, matrix_library.transition('z', self.l3)),
                        [0, 80, 0, 1])))
            a0_x = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(
                        np.matmul(r3, matrix_library.transition('z', self.l3)),
                        [80, 0, 0, 1])))
            a0_z = np.matmul(
                np.matmul(r1, matrix_library.transition('z', self.l1)),
                np.matmul(
                    np.matmul(r2, matrix_library.transition('z', self.l2)),
                    np.matmul(
                        np.matmul(r3, matrix_library.transition('z', self.l3)),
                        [0, 0, 80, 1])))
            ax.plot([self.C_coord[0], a0_z[0]], [self.C_coord[1], a0_z[1]],
                    [self.C_coord[2], a0_z[2]],
                    color='green',
                    linewidth=2)
            ax.plot([self.C_coord[0], a0_y[0]], [self.C_coord[1], a0_y[1]],
                    [self.C_coord[2], a0_y[2]],
                    color='orange',
                    linewidth=2)
            ax.plot([self.C_coord[0], a0_x[0]], [self.C_coord[1], a0_x[1]],
                    [self.C_coord[2], a0_x[2]],
                    color='blue',
                    linewidth=2)

        # Plot coordinates
        ax.set_xlabel('x', fontweight='bold')
        ax.set_ylabel('y', fontweight='bold')
        ax.set_zlabel('z', fontweight='bold')
        ax.set_xlim(-400, 400)
        ax.set_ylim(-400, 400)
        ax.set_zlim(-100, 400)
        ax.set_yticks([-300, 0, 300])
        ax.set_xticks([-300, 0, 300])
        ax.set_zticks([0, 300])
        ax.view_init(30, angle)
        plt.draw()