Exemplo n.º 1
0
def play_all_actions(app):
    frame_rate = 1 / leapModule.get_bandwidth_status()
    naoModule.set_stiffness()
    for local_action_list in action_list:
        for action in local_action_list:
            parse_action(action, frame_rate)
            app.processEvents()
Exemplo n.º 2
0
def nao_stand():
    naoModule.set_stiffness()
    naoModule.posture_stand()
    local_action_list = [{"type": "stand", "parameters": [-1]}]
    record_action_list(local_action_list)
Exemplo n.º 3
0
def play_single_action(selected_index, app):
    frame_rate = 1 / leapModule.get_bandwidth_status()
    naoModule.set_stiffness()
    for action in action_list[selected_index]:
        parse_action(action, frame_rate)
        app.processEvents()
Exemplo n.º 4
0
def nao_arm(window, app, hand):
    hands = 0

    while hands == 0:
        hands = leapModule.count_hands()

    window.lbl_wait.hide()
    window.lbl_sldr_vertical.setText("Pitch")
    window.lbl_sldr_horiz.setText("Roll")
    window.lbl_title.setText("Movement Status - Shoulder")
    app.processEvents()
    # prepare window for motion

    joint_limits = naoModule.get_arm_joint_limits(hand)
    gesture = leapModule.get_hand_gesture()
    # get joint movement limits and current hand gesture
    close_counter = 0
    local_action_list = []

    naoModule.set_stiffness()
    while close_counter < 300:
        if gesture == 3:
            close_counter += 1
            # close window when hand gesture is fist
        else:
            height = leapModule.get_height()
            scaled_height = scale(height, 100, 500, 30, 90)

            angles = leapModule.get_pitch_yaw_roll()
            angles[2] *= -1  # invert roll
            # scale angles according to height for sensitivity
            angles_scaled_height = \
                [
                    scale(angles[0], -scaled_height, scaled_height, -90, 90),  # pitch
                    scale(angles[1], -scaled_height, scaled_height, -90, 90),  # yaw
                    scale(angles[2], -scaled_height, scaled_height, -90, 90)   # roll
                ]

            if gesture == 0:  # hand extended
                # move shoulder
                # scale angles according to nao arm limits
                scaled_limits = \
                    [
                        scale(angles_scaled_height[0],  # pitch
                              -90, 90,
                              joint_limits["shoulder_pitch_min"], joint_limits["shoulder_pitch_max"]),
                        scale(angles_scaled_height[2],  # roll
                              -90, 90,
                              joint_limits["shoulder_roll_min"], joint_limits["shoulder_roll_max"])
                    ]

                output_pitch = scale(scaled_limits[0],
                                     joint_limits["shoulder_pitch_min"],
                                     joint_limits["shoulder_pitch_max"], 0, 99)
                output_roll = scale(scaled_limits[1],
                                    joint_limits["shoulder_roll_min"],
                                    joint_limits["shoulder_roll_max"], 0, 99)

                window.sldr_vertical.setValue(output_pitch)
                window.sldr_horiz.setValue(output_roll)
                window.lbl_sldr_vertical.setText("Pitch")
                window.lbl_sldr_horiz.setText("Roll")
                window.lbl_title.setText("Movement Status - Shoulder")
                app.processEvents()

                scaled_radians = \
                    [
                        scaled_limits[0] * -0.0174533,
                        scaled_limits[1] * 0.0174533
                    ]

                naoModule.move_shoulder(scaled_radians, hand)
                local_action_list.append({
                    "type": hand + "_shoulder",
                    "parameters": scaled_radians
                })
            elif gesture == 1:  # middle three fingers extended
                # move elbow
                # scale angles according to nao arm limits
                scaled_limits = \
                    [
                        scale(angles_scaled_height[1],  # yaw
                              -90, 90,
                              joint_limits["elbow_yaw_min"], joint_limits["elbow_yaw_max"]),
                        scale(angles_scaled_height[2],  # roll
                              -90, 90,
                              joint_limits["elbow_roll_min"], joint_limits["elbow_roll_max"])
                    ]

                output_yaw = scale(scaled_limits[0],
                                   joint_limits["elbow_yaw_min"],
                                   joint_limits["elbow_yaw_max"], 0, 99)
                output_roll = scale(scaled_limits[1],
                                    joint_limits["elbow_roll_min"],
                                    joint_limits["elbow_roll_max"], 0, 99)

                window.sldr_vertical.setValue(output_roll)
                window.sldr_horiz.setValue(output_yaw)
                window.lbl_sldr_vertical.setText("Roll")
                window.lbl_sldr_horiz.setText("Yaw")
                window.lbl_title.setText("Movement Status - Elbow")

                app.processEvents()

                scaled_radians = \
                    [
                        scaled_limits[0] * -0.0174533,
                        scaled_limits[1] * 0.0174533
                    ]

                naoModule.move_elbow(scaled_radians, hand)
                local_action_list.append({
                    "type": hand + "_elbow",
                    "parameters": scaled_radians
                })
            elif gesture == 2:  # outer two fingers extended
                # move wrist
                # scale angles according to nao arm limits
                scaled_limits = \
                    [
                        scale(angles_scaled_height[2],  # roll
                              -90, 90,
                              joint_limits["wrist_roll_min"], joint_limits["wrist_roll_max"])
                    ]

                output_roll = scale(scaled_limits[0],
                                    joint_limits["wrist_roll_min"],
                                    joint_limits["wrist_roll_max"], 0, 99)

                window.sldr_vertical.setValue(50)
                window.sldr_horiz.setValue(output_roll)
                window.lbl_sldr_vertical.setText("N/A")
                window.lbl_sldr_horiz.setText("Roll")
                window.lbl_title.setText("Movement Status - Wrist")

                app.processEvents()

                scaled_radians = \
                    [
                        scaled_limits[0] * 0.0174533
                    ]

                naoModule.move_wrist(scaled_radians, hand)
                local_action_list.append({
                    "type": hand + "_wrist",
                    "parameters": scaled_radians
                })
            output_height = scale(scaled_height, 30, 90, 0, 99)
            window.sldr_height.setValue(output_height)

            app.processEvents()
        gesture = leapModule.get_hand_gesture()
    if len(local_action_list) > 0:
        record_action_list(local_action_list)
Exemplo n.º 5
0
def nao_walk(window, app):
    hands = 0

    while hands == 0:
        hands = leapModule.count_hands()

    window.lbl_wait.hide()
    window.lbl_sldr_vertical.setText("Yaw")
    window.lbl_sldr_horiz.setText("Pitch")
    app.processEvents()
    # prepare ui for motion

    extended_fingers = leapModule.get_extended_fingers()
    close_counter = 0
    local_action_list = []

    naoModule.set_stiffness()
    while close_counter < 300:
        if extended_fingers == 0:
            close_counter += 1
            # close window on fist gesture
        else:
            height = leapModule.get_height()
            scaled_height = scale(height, 100, 400, 30, 90)

            angles = leapModule.get_pitch_yaw_roll()
            # scale angles according to height for sensitivity
            scaled_angles1 = \
                [
                    scale(angles[0], -scaled_height, scaled_height, -90, 90),
                    scale(angles[1], -scaled_height, scaled_height, -90, 90)
                ]

            # scale angles according to output scale
            scaled_angles2 = \
                [
                    scale(scaled_angles1[0], -90, 90, 0, 99),
                    scale(scaled_angles1[1], -90, 90, 0, 99)
                ]

            # scale angles and height to output limits and output
            output_pitch = scaled_angles2[0]
            output_yaw = scaled_angles2[1]
            window.sldr_vertical.setValue(output_pitch)
            window.sldr_horiz.setValue(output_yaw)
            output_height = scale(scaled_height, 30, 90, 0, 99)
            window.sldr_height.setValue(output_height)
            app.processEvents()

            # check whether pitch is outside of movement threshold (outside of -30 - 30 range)
            scaled_pitch = scale(scaled_angles2[0], 0, 99, -0.1, 0.1)
            if -0.03 < scaled_pitch < 0.03:
                scaled_pitch = 0

            # check whether yaw is outside of movement threshold
            if scaled_angles2[1] < 33:
                rad_yaw = 20
            elif scaled_angles2[1] > 66:
                rad_yaw = -20
            else:
                rad_yaw = 0

            # convert yaw to radians and send to nao
            rad_yaw *= 0.0174533
            naoModule.move_walk_turn(scaled_pitch, rad_yaw)
            # record action
            local_action_list.append({
                "type": "walk",
                "parameters": [scaled_pitch, rad_yaw]
            })

        extended_fingers = leapModule.get_extended_fingers()
    naoModule.move_stop()
    # stop nao moving when out of motion loop
    if len(local_action_list) > 0:
        record_action_list(local_action_list)
Exemplo n.º 6
0
def nao_rotate_head(window, app):
    hands = 0

    while hands == 0:
        hands = leapModule.count_hands()

    window.lbl_wait.hide()
    window.lbl_sldr_vertical.setText("Yaw")
    window.lbl_sldr_horiz.setText("Pitch")
    app.processEvents()
    # setup ui for motion

    extended_fingers = leapModule.get_extended_fingers()
    close_counter = 0
    local_action_list = []

    naoModule.set_stiffness()
    while close_counter < 300:
        if extended_fingers == 0:
            close_counter += 1
            # if fist gesture, close window
        else:
            height = leapModule.get_height()
            scaled_height = scale(height, 100, 400, 30, 90)

            angles = leapModule.get_pitch_yaw_roll()
            # scale angles according to height for sensitivity
            scaled_angles1 = \
                [
                    scale(angles[0], -scaled_height, scaled_height, -90, 90),
                    scale(angles[1], -scaled_height, scaled_height, -90, 90)
                ]

            # calculate pitch limitations based on yaw
            x = scale(scaled_angles1[1], -90, 90, -120, 120)
            pitch_max = \
                (2 * 10 ** -7) * (x ** 4) \
                + (2 * 10 ** -17) * (x ** 3) \
                - 0.0039 * (x ** 2) \
                + (7 * 10 ** -14) * x \
                + 27.918 \
                - 1

            pitch_min = \
                - (3 * 10 ** -7) * (x ** 4) \
                - (9 * 10 ** -18) * (x ** 3) \
                + 0.005 * (x ** 2) \
                + (6 * 10 ** -13) * x \
                + 42.134 \
                * -1

            # scale angles according to nao head limits
            scaled_angles2 = \
                [
                    scale(scaled_angles1[0], -90, 90, pitch_min, pitch_max),
                    scale(scaled_angles1[1], -90, 90, -120, 120)
                ]

            # scale angles and height according to output limits and update ui
            output_pitch = scale(scaled_angles2[0], pitch_min, pitch_max, 0,
                                 99)
            output_yaw = scale(scaled_angles2[1], -120, 120, 0, 99)
            window.sldr_vertical.setValue(output_pitch)
            window.sldr_horiz.setValue(output_yaw)
            output_height = scale(scaled_height, 30, 90, 0, 99)
            window.sldr_height.setValue(output_height)
            app.processEvents()

            # convert angles to radian and send to nao
            rad_scaled_angles = [
                scaled_angles2[0] * -0.0174533, scaled_angles2[1] * -0.0174533
            ]
            naoModule.rotate_head(rad_scaled_angles)

            # record action
            local_action_list.append({
                "type": "head",
                "parameters": rad_scaled_angles
            })
        extended_fingers = leapModule.get_extended_fingers()
    if len(local_action_list) > 0:
        record_action_list(local_action_list)