changed gesture loader, it has tai chi and gangnam style now and the ability to load choregraphe gestures
This commit is contained in:
Lucca Pirovano
+241
-105
@@ -15,6 +15,10 @@ import os
|
||||
import json
|
||||
import struct
|
||||
import socket
|
||||
import importlib.util
|
||||
import shutil
|
||||
import tempfile
|
||||
import inspect
|
||||
|
||||
# Import the new music module
|
||||
from naomusic import NaoMusicPlayer
|
||||
@@ -456,6 +460,115 @@ class VideoServerLauncher:
|
||||
self.ready = False
|
||||
|
||||
|
||||
# ==========================================
|
||||
# CHOREGRAPHE BEHAVIOR DEPLOY + RUN (for gestures)
|
||||
# ==========================================
|
||||
class ChoregrapheBehaviorRunner:
|
||||
"""Deploys a Choregraphe-exported behavior folder (a manifest.xml
|
||||
plus one or more '<BehaviorName>/behavior.xar' dirs, i.e. exactly
|
||||
what "Export as package" produces before you zip it) to the robot
|
||||
over SFTP, installs it via the PackageManager service, and runs it
|
||||
via ALBehaviorManager - the same three steps Choregraphe's own
|
||||
"Connect to robot" + Play button does, done here in code so a
|
||||
gesture script can just point at a local project folder.
|
||||
|
||||
Caches per local folder for the lifetime of this object: once a
|
||||
package has been deployed+installed this session, repeat runs
|
||||
just call runBehavior() again instead of re-uploading.
|
||||
|
||||
NOTE: PackageManager.install() is documented as "install a package
|
||||
from a path" without saying whether that path can be a raw folder
|
||||
or must be a zipped .pkg - this tries the raw folder first (since
|
||||
that's genuinely what Choregraphe's live "current project" push
|
||||
to a connected robot does) and falls back to zipping it into a
|
||||
.pkg if the robot's PackageManager rejects the folder.
|
||||
"""
|
||||
def __init__(self, session, nao_ip, ssh_user, ssh_password, ssh_port):
|
||||
self.nao_ip = nao_ip
|
||||
self.ssh_user = ssh_user
|
||||
self.ssh_password = ssh_password
|
||||
self.ssh_port = ssh_port
|
||||
try:
|
||||
self.package_mgr = session.service("PackageManager")
|
||||
except Exception:
|
||||
self.package_mgr = session.service("ALPackageManager")
|
||||
self.behavior_mgr = session.service("ALBehaviorManager")
|
||||
self._installed = {} # local_dir -> resolved "<package>/<behavior>" name
|
||||
|
||||
def _sftp_put_dir(self, sftp, local_dir, remote_dir):
|
||||
"""Recursively mirror local_dir onto remote_dir over an
|
||||
already-open SFTP session - paramiko has no built-in recursive
|
||||
put, only single-file put()."""
|
||||
try:
|
||||
sftp.mkdir(remote_dir)
|
||||
except IOError:
|
||||
pass # already exists, fine
|
||||
for entry in sorted(os.listdir(local_dir)):
|
||||
local_path = os.path.join(local_dir, entry)
|
||||
remote_path = remote_dir + "/" + entry
|
||||
if os.path.isdir(local_path):
|
||||
self._sftp_put_dir(sftp, local_path, remote_path)
|
||||
else:
|
||||
sftp.put(local_path, remote_path)
|
||||
|
||||
def ensure_running(self, local_dir, behavior_subdir):
|
||||
"""Deploy+install local_dir (only if not already done this
|
||||
session), then block until '<package>/<behavior_subdir>' has
|
||||
finished running on the robot. local_dir's own folder name
|
||||
becomes the package id, matching how Choregraphe names an
|
||||
exported project after its folder."""
|
||||
if not HAS_SSH:
|
||||
raise RuntimeError("Choregraphe gesture deploy needs paramiko: pip install paramiko")
|
||||
if not os.path.isdir(local_dir):
|
||||
raise RuntimeError(f"No Choregraphe project folder at {local_dir}")
|
||||
|
||||
package_name = os.path.basename(os.path.normpath(local_dir))
|
||||
behavior_name = self._installed.get(local_dir)
|
||||
|
||||
if behavior_name is None:
|
||||
ssh = paramiko.SSHClient()
|
||||
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
||||
ssh.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
|
||||
password=self.ssh_password, timeout=10)
|
||||
try:
|
||||
sftp = ssh.open_sftp()
|
||||
try:
|
||||
remote_home = sftp.normalize(".")
|
||||
remote_dir = f"{remote_home}/naowalk_gestures/{package_name}"
|
||||
print(f"🕺 Copying {package_name} -> {self.nao_ip}:{remote_dir}")
|
||||
self._sftp_put_dir(sftp, local_dir, remote_dir)
|
||||
|
||||
print(f"🕺 Installing package {package_name}...")
|
||||
try:
|
||||
self.package_mgr.install(remote_dir)
|
||||
except Exception as e:
|
||||
print(f"🕺 install() on the raw folder failed ({e}) - "
|
||||
f"retrying as a zipped .pkg...")
|
||||
local_zip = shutil.make_archive(
|
||||
os.path.join(tempfile.gettempdir(), package_name), "zip", local_dir)
|
||||
remote_zip = f"{remote_home}/naowalk_gestures/{package_name}.pkg"
|
||||
sftp.put(local_zip, remote_zip)
|
||||
os.remove(local_zip)
|
||||
self.package_mgr.install(remote_zip)
|
||||
finally:
|
||||
sftp.close()
|
||||
finally:
|
||||
ssh.close()
|
||||
|
||||
behavior_name = f"{package_name}/{behavior_subdir}"
|
||||
if not self.behavior_mgr.isBehaviorInstalled(behavior_name):
|
||||
installed = self.behavior_mgr.getInstalledBehaviors()
|
||||
candidates = [b for b in installed if b.endswith("/" + behavior_subdir)]
|
||||
if not candidates:
|
||||
raise RuntimeError(
|
||||
f"'{behavior_name}' not found after install - installed behaviors: {installed}")
|
||||
behavior_name = candidates[0]
|
||||
self._installed[local_dir] = behavior_name
|
||||
|
||||
print(f"🕺 Running {behavior_name}...")
|
||||
self.behavior_mgr.runBehavior(behavior_name) # blocks until it finishes
|
||||
|
||||
|
||||
# ==========================================
|
||||
# MAIN TELEOP
|
||||
# ==========================================
|
||||
@@ -464,9 +577,13 @@ class NaoTeleop:
|
||||
|
||||
def __init__(self, session, nao_ip, audio_device_index=None, mic_gain=8.0, video_scale=2.0,
|
||||
nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_rate=16000,
|
||||
media_relay=None, deploy_video_server=True, video_server_path=None):
|
||||
media_relay=None, deploy_video_server=True, video_server_path=None,
|
||||
gestures_dir=None):
|
||||
self.session = session
|
||||
self.nao_ip = nao_ip
|
||||
self.nao_ssh_user = nao_ssh_user
|
||||
self.nao_ssh_password = nao_ssh_password
|
||||
self.nao_ssh_port = nao_ssh_port
|
||||
self.motion = session.service("ALMotion")
|
||||
self.posture = session.service("ALRobotPosture")
|
||||
self.tts = session.service("ALTextToSpeech")
|
||||
@@ -507,9 +624,12 @@ class NaoTeleop:
|
||||
self.battery_level = 100
|
||||
self.last_batt_check = 0
|
||||
self.volume = 50
|
||||
self.walk_speed = 0.6
|
||||
self.walk_speed = 1.0 # fixed, not adjustable at runtime - 0.60 was too slow
|
||||
# for the walk engine to stay balanced and NAO kept
|
||||
# tipping over
|
||||
|
||||
self.typing_mode = False
|
||||
self.is_resting = False # tracks manual [DEL] crouch, toggled by idle_crouch()
|
||||
self.chat_message = ""
|
||||
self.music_search_mode = False
|
||||
self.music_search_text = ""
|
||||
@@ -546,6 +666,16 @@ class NaoTeleop:
|
||||
if HAS_AUDIO:
|
||||
self._init_mic_stream()
|
||||
|
||||
# Lets gesture scripts run full Choregraphe-authored behaviors
|
||||
# (.xar, exported as a package folder) instead of hand-coding
|
||||
# angleInterpolation calls - see ChoregrapheBehaviorRunner.
|
||||
self.behavior_runner = ChoregrapheBehaviorRunner(
|
||||
session, nao_ip, nao_ssh_user, nao_ssh_password, nao_ssh_port)
|
||||
|
||||
self.gestures_dir = gestures_dir or os.path.join(
|
||||
os.path.dirname(os.path.abspath(__file__)), "gestures")
|
||||
self._load_gestures()
|
||||
|
||||
pygame.init()
|
||||
self.screen = pygame.display.set_mode((self.display_width, self.display_height))
|
||||
pygame.display.set_caption("NAO Teleop + Music on Robot")
|
||||
@@ -693,95 +823,104 @@ class NaoTeleop:
|
||||
except Exception as e:
|
||||
print(f"❌ Mic init failed: {e}")
|
||||
|
||||
def wave_gesture(self):
|
||||
if self.typing_mode: return
|
||||
print("🤚 Waving hello...")
|
||||
def _load_gestures(self):
|
||||
"""Load number-key gestures from .py files in self.gestures_dir.
|
||||
|
||||
Each file is named '<digit>_<name>.py' (e.g. '4_gangnam_style.py')
|
||||
and must define a module-level run(motion, tts) function - that's
|
||||
the minimum contract, gets the already-connected ALMotion and
|
||||
ALTextToSpeech proxies and does whatever it wants with them.
|
||||
Gestures that need more - e.g. running a Choregraphe behavior via
|
||||
self.behavior_runner, or resetting posture via
|
||||
self.stand_for_walk() - can instead define run(motion, tts,
|
||||
teleop) and get this NaoTeleop instance as a third argument; the
|
||||
arity is detected here at load time so both signatures work. The
|
||||
leading digit is which number key (0-9) triggers it; everything
|
||||
after the underscore becomes its display name in the controls
|
||||
hint, unless the file sets its own module-level NAME string.
|
||||
"""
|
||||
self.gestures = {} # pygame key constant -> (display_name, run_fn, n_params)
|
||||
gestures_dir = self.gestures_dir
|
||||
if not os.path.isdir(gestures_dir):
|
||||
print(f"⚠️ No gestures folder at {gestures_dir}, skipping gesture load")
|
||||
return
|
||||
|
||||
for fname in sorted(os.listdir(gestures_dir)):
|
||||
if not fname.endswith(".py") or fname.startswith("_"):
|
||||
continue
|
||||
stem = fname[:-3]
|
||||
digit_str, sep, rest = stem.partition("_")
|
||||
if not (sep and len(digit_str) == 1 and digit_str.isdigit()):
|
||||
print(f"⚠️ Skipping {fname}: name it '<digit>_name.py' (e.g. '4_gangnam_style.py')")
|
||||
continue
|
||||
|
||||
path = os.path.join(gestures_dir, fname)
|
||||
try:
|
||||
spec = importlib.util.spec_from_file_location(f"nao_gesture_{stem}", path)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
run_fn = getattr(module, "run", None)
|
||||
if not callable(run_fn):
|
||||
print(f"⚠️ Skipping {fname}: no run(motion, tts) function defined")
|
||||
continue
|
||||
try:
|
||||
n_params = len(inspect.signature(run_fn).parameters)
|
||||
except (TypeError, ValueError):
|
||||
n_params = 2
|
||||
except Exception as e:
|
||||
print(f"⚠️ Failed to load gesture {fname}: {e}")
|
||||
continue
|
||||
|
||||
key = getattr(pygame, f"K_{digit_str}")
|
||||
name = getattr(module, "NAME", rest.replace("_", " ").title() or stem)
|
||||
if key in self.gestures:
|
||||
print(f"⚠️ {fname} overrides number key {digit_str} (was {self.gestures[key][0]})")
|
||||
self.gestures[key] = (name, run_fn, n_params)
|
||||
print(f"✅ Loaded gesture: {digit_str}={name}")
|
||||
|
||||
def run_gesture(self, key):
|
||||
if self.typing_mode or self.is_resting: return
|
||||
entry = self.gestures.get(key)
|
||||
if not entry: return
|
||||
name, run_fn, n_params = entry
|
||||
print(f"🕺 {name}...")
|
||||
try:
|
||||
self.motion.setStiffnesses("RArm", 1.0)
|
||||
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
|
||||
angles = [[-1.5]*5, [-0.4,-0.7,-0.4,-0.7,-0.4], [0.6,1.1,0.6,1.1,0.6],
|
||||
[0.8,0.3,0.8,0.3,0.8], [0.0,1.5,0.0,-1.5,0.0], [1.0]*5]
|
||||
times = [[0.5,1.0,1.5,2.0,2.5]] * 6
|
||||
self.motion.angleInterpolation(names, angles, times, True)
|
||||
neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
|
||||
self.motion.angleInterpolationWithSpeed(names, neutral, 0.3)
|
||||
# Relaxed stiffness after completion
|
||||
self.motion.setStiffnesses("RArm", 0.3)
|
||||
if n_params >= 3:
|
||||
run_fn(self.motion, self.tts, self)
|
||||
else:
|
||||
run_fn(self.motion, self.tts)
|
||||
except Exception as e:
|
||||
print(f"Wave error: {e}")
|
||||
print(f"{name} gesture error: {e}")
|
||||
|
||||
def cena_gesture(self):
|
||||
if self.typing_mode: return
|
||||
print("👋 YOU CAN'T SEE ME!")
|
||||
try:
|
||||
self.tts.say("You can't see me")
|
||||
|
||||
self.motion.setStiffnesses("RArm", 1.0)
|
||||
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
|
||||
|
||||
# Setup for tight, face-level sweep
|
||||
tight_setup = [-0.4, -0.2, 0.0, 1.2, 1.5, 1.0]
|
||||
self.motion.angleInterpolation(names, tight_setup, [0.5]*6, True)
|
||||
|
||||
# The horizontal "backhand" sweep
|
||||
self.motion.angleInterpolation(["RElbowYaw"], [0.8], 0.4, True)
|
||||
self.motion.angleInterpolation(["RElbowYaw"], [-0.8], 0.6, True)
|
||||
self.motion.angleInterpolation(["RElbowYaw"], [0.0], 0.4, True)
|
||||
|
||||
# Return to neutral
|
||||
neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
|
||||
self.motion.angleInterpolationWithSpeed(names, neutral, 0.3)
|
||||
# Relaxed stiffness after completion
|
||||
self.motion.setStiffnesses("RArm", 0.3)
|
||||
except Exception as e:
|
||||
print(f"Cena gesture error: {e}")
|
||||
def stand_for_walk(self):
|
||||
"""Re-stiffen and drop straight into walk-ready posture -
|
||||
mirrors the startup sequence in run() (stiffen -> walk arms ->
|
||||
relax arm stiffness -> moveInit) rather than
|
||||
goToPosture("Stand"), which goes through its own separate
|
||||
stand-up animation instead of just being walk-ready. Shared by
|
||||
the [DEL] wake-up toggle and by any gesture that needs to
|
||||
reset his stance afterward (e.g. after a Choregraphe behavior
|
||||
that moved the legs/hips)."""
|
||||
self._safe("Restiffen", lambda: self.motion.setStiffnesses("Body", 1.0))
|
||||
self._safe("Walk arms", lambda: self.motion.setMoveArmsEnabled(True, True))
|
||||
self._safe("Relax arms", lambda: self.motion.setStiffnesses(["RArm", "LArm"], 0.3))
|
||||
self._safe("moveInit", self.motion.moveInit)
|
||||
self.is_resting = False
|
||||
|
||||
def six_seven_gesture(self):
|
||||
if self.typing_mode: return
|
||||
print("🖐️ SIX... SEVEN...")
|
||||
try:
|
||||
threading.Thread(target=self.tts.say, args=("six seven",), daemon=True).start()
|
||||
|
||||
self.motion.setStiffnesses(["RArm", "LArm"], 1.0)
|
||||
|
||||
# Lock both arms up in front of the body, hands held out like a scale.
|
||||
# ElbowYaw is rotated ~90° off zero here - that's what points the elbow's
|
||||
# bend axis so flexing tips the forearm forward/down instead of curling
|
||||
# it in across the chest. These joints don't move again until the return.
|
||||
hold_names = [
|
||||
"RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RWristYaw", "RHand",
|
||||
"LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LWristYaw", "LHand"
|
||||
]
|
||||
hold_angles = [
|
||||
0.2, -0.2, 1.4, 1.57, 0.6,
|
||||
0.2, 0.2, -1.4, -1.57, 0.6
|
||||
]
|
||||
self.motion.angleInterpolationWithSpeed(hold_names, hold_angles, 0.25)
|
||||
|
||||
# Only the elbows seesaw now - a small tip back and forth, one arm
|
||||
# rising while the other falls, like weighing something in each hand
|
||||
# ("six... seven..."). Small amplitude so it stays a wobble, not a fold.
|
||||
elbow_names = ["RElbowRoll", "LElbowRoll"]
|
||||
r_low, r_high = 0.6, 1.0
|
||||
l_low, l_high = -0.6, -1.0
|
||||
|
||||
elbow_angles = [
|
||||
[r_low, r_high, r_low, r_high, r_low, r_high],
|
||||
[l_high, l_low, l_high, l_low, l_high, l_low],
|
||||
]
|
||||
times = [[0.35, 0.7, 1.05, 1.4, 1.75, 2.1]] * 2
|
||||
self.motion.angleInterpolation(elbow_names, elbow_angles, times, True)
|
||||
|
||||
# Return everything to neutral
|
||||
neutral_names = hold_names + elbow_names
|
||||
neutral = [1.5, -0.15, 1.2, 0.0, 0.0,
|
||||
1.5, 0.15, -1.2, 0.0, 0.0,
|
||||
0.5, -0.5]
|
||||
self.motion.angleInterpolationWithSpeed(neutral_names, neutral, 0.3)
|
||||
# Relaxed stiffness after completion
|
||||
self.motion.setStiffnesses(["RArm", "LArm"], 0.3)
|
||||
except Exception as e:
|
||||
print(f"Six seven gesture error: {e}")
|
||||
def idle_crouch(self):
|
||||
"""The relaxed sit/crouch posture NAO settles into on shutdown -
|
||||
toggled on demand with the Delete key. rest() cuts stiffness to
|
||||
the whole body (that's the point of "rest"), so simply calling
|
||||
it again does nothing on the second press - we have to
|
||||
explicitly re-stiffen ourselves via stand_for_walk()."""
|
||||
if not self.is_resting:
|
||||
print("😴 Resting (idle crouch)...")
|
||||
self._safe("stopMove", self.motion.stopMove)
|
||||
self._safe("rest", self.motion.rest)
|
||||
self.is_resting = True
|
||||
else:
|
||||
print("🧍 Waking up...")
|
||||
self.stand_for_walk()
|
||||
|
||||
def handle_head_movement(self):
|
||||
if self.typing_mode or self.music_search_mode: return
|
||||
@@ -872,7 +1011,11 @@ class NaoTeleop:
|
||||
self._movement_thread = threading.Thread(target=self._movement_loop, daemon=True)
|
||||
self._movement_thread.start()
|
||||
|
||||
print("\n🎮 Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | 2=Cena | 3=6-7 | M=Music search | X=Stop | -/==Volume | T=TTS | V=Push-to-talk (hold) | ESC=Quit")
|
||||
gesture_hint = " | ".join(
|
||||
f"{pygame.key.name(key)}={name}" for key, (name, *_rest) in sorted(self.gestures.items())
|
||||
)
|
||||
print(f"\n🎮 Controls: WASD/Arrows=Walk | IJKL=Head | SPACE=Reset head | {gesture_hint} | "
|
||||
f"DEL=Toggle crouch/stand | M=Music search | X=Stop | -/==Volume | T=TTS | V=Push-to-talk (hold) | ESC=Quit")
|
||||
|
||||
try:
|
||||
while self.running:
|
||||
@@ -948,12 +1091,10 @@ class NaoTeleop:
|
||||
else:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
self.running = False
|
||||
elif event.key == pygame.K_1:
|
||||
self.wave_gesture()
|
||||
elif event.key == pygame.K_2:
|
||||
self.cena_gesture()
|
||||
elif event.key == pygame.K_3:
|
||||
self.six_seven_gesture()
|
||||
elif event.key == pygame.K_DELETE:
|
||||
self.idle_crouch()
|
||||
elif event.key in self.gestures:
|
||||
self.run_gesture(event.key)
|
||||
elif event.key == pygame.K_SPACE:
|
||||
self.reset_head()
|
||||
elif event.key == pygame.K_m:
|
||||
@@ -984,16 +1125,6 @@ class NaoTeleop:
|
||||
if getattr(self, "sound_receiver", None):
|
||||
self.sound_receiver.muted = False
|
||||
|
||||
speed_changed = False
|
||||
if keys[pygame.K_LEFTBRACKET]:
|
||||
self.walk_speed = max(0.1, self.walk_speed - 0.01)
|
||||
speed_changed = True
|
||||
if keys[pygame.K_RIGHTBRACKET]:
|
||||
self.walk_speed = min(1.0, self.walk_speed + 0.01)
|
||||
speed_changed = True
|
||||
if speed_changed:
|
||||
self.update_title()
|
||||
|
||||
x = y = theta = 0.0
|
||||
if keys[pygame.K_w] or keys[pygame.K_UP]: x = self.walk_speed
|
||||
if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -self.walk_speed
|
||||
@@ -1161,6 +1292,10 @@ if __name__ == "__main__":
|
||||
parser.add_argument("--video-server-path", type=str, default=None,
|
||||
help="Local path to nao_video_server.py to deploy (default: the "
|
||||
"copy sitting next to naowalk.py)")
|
||||
parser.add_argument("--gestures-dir", type=str, default=None,
|
||||
help="Folder of number-key gestures to load, each file named "
|
||||
"'<digit>_name.py' with a run(motion, tts) function "
|
||||
"(default: the 'gestures' folder next to naowalk.py)")
|
||||
args = parser.parse_args()
|
||||
|
||||
session = qi.Session()
|
||||
@@ -1181,4 +1316,5 @@ if __name__ == "__main__":
|
||||
nao_ssh_port=args.nao_ssh_port,
|
||||
relay_rate=args.relay_rate,
|
||||
deploy_video_server=not args.no_deploy_video_server,
|
||||
video_server_path=args.video_server_path).run()
|
||||
video_server_path=args.video_server_path,
|
||||
gestures_dir=args.gestures_dir).run()
|
||||
|
||||
Reference in New Issue
Block a user