luccapirovano/naowalk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added wave gesture

Browse Source
This commit is contained in:
Lucca Pirovano
2026-07-15 13:43:07 -04:00
parent 980bca205d
commit 5f12b92f3d
1 changed files with 45 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
naowalk.py
+44 -24
View File
@@ -18,7 +18,6 @@ class NaoTeleop:
self.video_client = None
self.running = True
# Head position tracking
self.head_yaw = 0.0
self.head_pitch = 0.0
@@ -26,14 +25,14 @@ class NaoTeleop:
pygame.init()
self.screen = pygame.display.set_mode((320, 240))
pygame.display.set_caption("NAO Teleop - Head Control IJKL")
pygame.display.set_caption("NAO Teleop - Wave on 1")
self.clock = pygame.time.Clock()
def _init_video_maxfps(self):
try:
self.video = self.session.service("ALVideoDevice")
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25)
print("✅ Camera ready (MAX FPS)")
print("✅ Camera ready")
except:
print("Camera not available")
@@ -51,30 +50,46 @@ class NaoTeleop:
pass
return None
def wave_gesture(self):
print("🤚 Waving...")
try:
# Simple friendly wave with right arm
self.motion.setStiffnesses("RArm", 1.0)
# Wave motion
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"]
angles = [
[-0.3, -0.3, -0.3], # ShoulderPitch
[0.0, 0.3, 0.0], # ShoulderRoll
[0.0, 1.0, 0.0], # ElbowYaw
[0.0, -1.0, 0.0], # ElbowRoll
[0.0, 1.5, 0.0] # WristYaw
]
times = [[1.0, 1.8, 2.6], [1.0, 1.8, 2.6], [1.0, 1.8, 2.6], [1.0, 1.8, 2.6], [1.0, 1.8, 2.6]]
self.motion.angleInterpolation(names, angles, times, True)
# Return to neutral
self.motion.setAngles("RArm", [0.0]*5, 0.4)
except Exception as e:
print(f"Wave error: {e}")
def handle_head_movement(self):
keys = pygame.key.get_pressed()
speed = 0.3 # Head movement speed (radians)
speed = 0.3
changed = False
if keys[pygame.K_i]: # Head Up
self.head_pitch = max(self.head_pitch - speed, -0.5)
changed = True
if keys[pygame.K_k]: # Head Down
self.head_pitch = min(self.head_pitch + speed, 0.5)
changed = True
if keys[pygame.K_j]: # Head Left
self.head_yaw = min(self.head_yaw + speed, 2.0)
changed = True
if keys[pygame.K_l]: # Head Right
self.head_yaw = max(self.head_yaw - speed, -2.0)
changed = True
if keys[pygame.K_i]:
self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True
if keys[pygame.K_k]:
self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True
if keys[pygame.K_j]:
self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True
if keys[pygame.K_l]:
self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True
if changed:
try:
self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)
except:
pass
def run(self):
self.motion.wakeUp()
@@ -82,13 +97,19 @@ class NaoTeleop:
print("🎮 Controls:")
print(" WASD / Arrows = Walk")
print(" I J K L = Head (Up, Left, Down, Right)")
print(" I J K L = Head movement")
print(" 1 = Wave gesture")
print(" ESC = Quit")
while self.running:
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
if event.type == pygame.QUIT:
self.running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
self.running = False
elif event.key == pygame.K_1: # Number key 1
self.wave_gesture()
# Walking
keys = pygame.key.get_pressed()
@@ -103,7 +124,6 @@ class NaoTeleop:
else:
self.motion.stopMove()
# Head control
self.handle_head_movement()
# Camera
@@ -116,7 +136,7 @@ class NaoTeleop:
self.screen.fill((10, 10, 30))
pygame.display.flip()
self.clock.tick(0) # Max speed
self.clock.tick(0)
# Shutdown
print("Shutting down...")