From 9ba030869ff95d09ddfe96120e4831a4257db833 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 14:05:51 -0400 Subject: [PATCH] improved stiffness and walk --- naowalk.py | 77 ++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 51 insertions(+), 26 deletions(-) diff --git a/naowalk.py b/naowalk.py index f8589d7..e449124 100644 --- a/naowalk.py +++ b/naowalk.py @@ -14,18 +14,30 @@ class NaoTeleop: self.session = session self.motion = session.service("ALMotion") self.posture = session.service("ALRobotPosture") + + # New: Hook into the battery service + try: + self.battery = session.service("ALBattery") + except: + self.battery = None + print("⚠️ ALBattery service not found") + self.video = None self.video_client = None self.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 + + # Battery tracking + self.battery_level = 100 + self.last_batt_check = 0 self._init_video_maxfps() pygame.init() self.screen = pygame.display.set_mode((320, 240)) - pygame.display.set_caption("NAO Teleop - Wave on 1") + pygame.display.set_caption("NAO Teleop | Battery: Checking...") self.clock = pygame.time.Clock() def _init_video_maxfps(self): @@ -34,7 +46,7 @@ class NaoTeleop: self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25) print("✅ Camera ready") except: - print("Camera not available") + print("❌ Camera not available") def get_image(self): if not self.video or not self.video_client: @@ -53,40 +65,33 @@ class NaoTeleop: def wave_gesture(self): print("🤚 Waving hello...") try: + # Stiffen arm for accurate wave self.motion.setStiffnesses("RArm", 1.0) + # Use all 6 joints to prevent xAssertArraySize crash names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] angles = [ - [-1.5, -1.5, -1.5, -1.5, -1.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, 1.0, 1.0, 1.0, 1.0] + [-1.5, -1.5, -1.5, -1.5, -1.5], # RShoulderPitch (high up) + [-0.4, -0.7, -0.4, -0.7, -0.4], # RShoulderRoll + [ 0.6, 1.1, 0.6, 1.1, 0.6], # RElbowYaw + [ 0.8, 0.3, 0.8, 0.3, 0.8], # RElbowRoll (proper bending) + [ 0.0, 1.5, 0.0, -1.5, 0.0], # RWristYaw (the wave) + [ 1.0, 1.0, 1.0, 1.0, 1.0] # RHand (keep open) ] - times = [ - [0.5, 1.0, 1.5, 2.0, 2.5], - [0.5, 1.0, 1.5, 2.0, 2.5], - [0.5, 1.0, 1.5, 2.0, 2.5], - [0.5, 1.0, 1.5, 2.0, 2.5], - [0.5, 1.0, 1.5, 2.0, 2.5], - [0.5, 1.0, 1.5, 2.0, 2.5] - ] + times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 6 self.motion.angleInterpolation(names, angles, times, True) - # --- THE FIX --- - # True "StandInit" natural arm angles: Pitch 1.5 (down), slight bends. + # Safely lower arm to true neutral neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] - - # Use angleInterpolationWithSpeed so it finishes the movement BEFORE - # the main loop can interrupt it with a stopMove() command. - # 0.3 means move at 30% of max speed for a smooth reset. self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3) - print("✅ Proper wave completed and reset") + # Relax the arm's stiffness to let the motors cool and walk naturally + self.motion.setStiffnesses("RArm", 0.6) + + print("✅ Proper wave completed and arm relaxed") except Exception as e: print(f"Wave error: {e}") @@ -108,10 +113,27 @@ class NaoTeleop: if changed: self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3) + def check_battery(self): + # Only ping the robot for battery data once every 10 seconds to save FPS + if not self.battery: + return + + now = time.time() + if now - self.last_batt_check > 10: + try: + self.battery_level = self.battery.getBatteryCharge() + pygame.display.set_caption(f"NAO Teleop | Battery: {self.battery_level}%") + except: + pass + self.last_batt_check = now + def run(self): self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) + # Turn natural arm swinging back on so his walk looks normal! + self.motion.setMoveArmsEnabled(True, True) + print("🎮 Controls:") print(" WASD / Arrows = Walk") print(" I J K L = Head") @@ -119,6 +141,9 @@ class NaoTeleop: print(" ESC = Quit") while self.running: + # Zero-impact battery checker + self.check_battery() + for event in pygame.event.get(): if event.type == pygame.QUIT: self.running = False @@ -156,7 +181,7 @@ class NaoTeleop: self.clock.tick(0) # Shutdown - print("Shutting down...") + print("🛑 Shutting down...") self.motion.stopMove() self.motion.rest() if self.video and self.video_client: @@ -175,9 +200,9 @@ if __name__ == "__main__": session = qi.Session() try: session.connect(f"tcp://{args.ip}:{args.port}") - print(f"Connected to {args.ip}") + print(f"✅ Connected to {args.ip}") except Exception as e: - print(f"Connection failed: {e}") + print(f"❌ Connection failed: {e}") sys.exit(1) NaoTeleop(session).run()