diff --git a/naowalk.py b/naowalk.py index 6c53e9e..7fcc5bb 100644 --- a/naowalk.py +++ b/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]: # 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 + self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3) 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...")