diff --git a/naowalk.py b/naowalk.py index 0e4be49..f8589d7 100644 --- a/naowalk.py +++ b/naowalk.py @@ -55,27 +55,38 @@ class NaoTeleop: try: self.motion.setStiffnesses("RArm", 1.0) - # Friendly high wave - names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"] + names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] - # Better motion: arm raised higher + side-to-side wave angles = [ - [-1.0, -0.8, -1.0, -0.8, -1.0], # RShoulderPitch (raise arm high) - [ 0.2, -0.5, 0.2, -0.5, 0.2], # RShoulderRoll - [ 0.8, 1.2, 0.8, 1.2, 0.8], # RElbowYaw - [-1.0, -0.3, -1.0, -0.3, -1.0], # RElbowRoll - [ 0.0, 1.5, 0.0, -1.5, 0.0] # RWristYaw (hand wave) + [-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] ] - times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 5 + 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] + ] self.motion.angleInterpolation(names, angles, times, True) - # Strong return to neutral position - self.motion.setAngles("RArm", [0.0] * 5, 0.6) - time.sleep(0.8) # Give time to settle + # --- THE FIX --- + # True "StandInit" natural arm angles: Pitch 1.5 (down), slight bends. + neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] - print("✅ Better wave completed") + # 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") except Exception as e: print(f"Wave error: {e}")