Пример #1
0
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)
Пример #2
0
    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]