improved stiffness and walk

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()