added wave gesture

naowalk.py

@@ -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]:   
-        if keys[pygame.K_i]:   # Head Up
+            self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True
-            self.head_pitch = max(self.head_pitch - speed, -0.5)
+        if keys[pygame.K_k]:   
-            changed = True
+            self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True
-        if keys[pygame.K_k]:   # Head Down
+        if keys[pygame.K_j]:   
-            self.head_pitch = min(self.head_pitch + speed, 0.5)
+            self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True
-            changed = True
+        if keys[pygame.K_l]:   
-        if keys[pygame.K_j]:   # Head Left
+            self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True
-            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 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...")