def controller_operation(joystick, group, hebi_feedback, command, L1, L2, T):
K = np.matrix([[0.05, 0], [0, 0.05]])
axis = get_axis(joystick)
axis_f = input_filter(axis, 10, T)
axis_f = np.squeeze(np.asarray(axis_f))
theta, omega, torque, hebi_limit_stop_flag = get_hebi_feedback(
group, hebi_feedback)
theta1 = theta[0]
theta2 = theta[1]
Jinv = np.matrix([[
-cos(theta1 + theta2) / (L1 * cos(theta2)),
-sin(theta1 + theta2) / (L1 * cos(theta2))
],
[(L2 * cos(theta1 + theta2) + L1 * sin(theta1)) /
(L1 * L2 * cos(theta2)),
(L2 * sin(theta1 + theta2) - L1 * cos(theta1)) /
(L1 * L2 * cos(theta2))]])
omega_d = Jinv @ K @ axis_f
omega_d = np.squeeze(np.asarray(omega_d))
command.velocity = omega_d
group.send_command(command)
L2 = 0.22
#======#
#=== variables for 3 dof ===#
# l1 = 0.268
# l2 = 0.472
#======#
i = 0
t0 = time()
K = np.matrix([[0.125, 0], [0, 0.125]])
while True:
theta, omega, torque, hebi_limit_stop_flag = get_hebi_feedback(
group, hebi_feedback)
t = time() - t0
pos = trajectory(t, traj)
animate_ball(animation_window, animation_canvas, pos, ball_traj)
pos_draw = encoder_draw(animation_window,
animation_canvas,
arduino,
ball_input,
offset,
draw=True)
axis = get_axis(joystick)
axis[1] = -axis[1]
theta_e = get_encoder_feedback(arduino, num_encoders=2)
theta = theta + np.array([-1.58170749, np.pi / 2 - 1.48693642
]) # offsetting transform
theta1 = theta[0]