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