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