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()
def nao_stand():
naoModule.set_stiffness()
naoModule.posture_stand()
local_action_list = [{"type": "stand", "parameters": [-1]}]
record_action_list(local_action_list)
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()
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)
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)
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)