def line_demo(inter_size):
p_start = np.array([-20, -15, 10])
p_end = np.array([20, -15, 10])
r_matrix = interpolate_line(p_start, p_end, inter_size)
print(r_matrix)
ik_matrix = create_joint_matrix(r_matrix)
ik_matrix = np.rint(ik_matrix)
phx.set_wsew(ik_matrix[0])
phx.wait_for_completion()
for positions in range(0, inter_size):
phx.set_wsew(ik_matrix[positions])
def go_to_a():
point_a = [-20, -10, 7]
theta0_4 = 0
go_to_pos(point_a, theta0_4)
phx.wait_for_completion()
def go_to_b():
point_b = [10, 0, 20]
theta0_4 = 0
go_to_pos(point_b, theta0_4)
phx.wait_for_completion()
def go_to_pos(pickup_pos, theta0_4):
joint_angles = kin.ik3(pickup_pos)
theta4 = kin.calculate_theta_4(joint_angles, theta0_4)
phx.set_wse(joint_angles)
phx.set_wrist(theta4)
phx.wait_for_completion()