Added antenna movement

reachy_f1_commentator/src/motion_controller.py

@@ -216,7 +216,7 @@ class GestureLibrary:
     """Library of predefined gestures for F1 commentary.
     
     Each gesture is defined as a sequence of movements using the 6-DOF
-    neck capabilities (pitch, yaw, roll, x, y, z).
+    neck capabilities (pitch, yaw, roll, x, y, z) plus 2 antenna DOF.
     
     Validates: Requirements 7.3, 7.4, 7.5, 7.6
     """
@@ -225,58 +225,59 @@ class GestureLibrary:
     GESTURES = {
         Gesture.NEUTRAL: GestureSequence(
             movements=[
-                {"pitch": 0, "yaw": 0, "roll": 0, "x": 0, "y": 0, "z": 0, "duration": 1.0}
+                {"pitch": 0, "yaw": 0, "roll": 0, "x": 0, "y": 0, "z": 0, 
+                 "antennas": [0, 0], "duration": 1.0}
             ],
             total_duration=1.0
         ),
         
         Gesture.NOD: GestureSequence(
             movements=[
-                {"pitch": 10, "yaw": 0, "roll": 0, "duration": 0.3},
-                {"pitch": -5, "yaw": 0, "roll": 0, "duration": 0.3},
-                {"pitch": 0, "yaw": 0, "roll": 0, "duration": 0.3}
+                {"pitch": 10, "yaw": 0, "roll": 0, "antennas": [10, 10], "duration": 0.3},
+                {"pitch": -5, "yaw": 0, "roll": 0, "antennas": [-5, -5], "duration": 0.3},
+                {"pitch": 0, "yaw": 0, "roll": 0, "antennas": [0, 0], "duration": 0.3}
             ],
             total_duration=0.9
         ),
         
         Gesture.TURN_LEFT: GestureSequence(
             movements=[
-                {"pitch": 0, "yaw": -30, "roll": 0, "duration": 0.5},
-                {"pitch": 0, "yaw": 0, "roll": 0, "duration": 0.5}
+                {"pitch": 0, "yaw": -30, "roll": 0, "antennas": [-15, 5], "duration": 0.5},
+                {"pitch": 0, "yaw": 0, "roll": 0, "antennas": [0, 0], "duration": 0.5}
             ],
             total_duration=1.0
         ),
         
         Gesture.TURN_RIGHT: GestureSequence(
             movements=[
-                {"pitch": 0, "yaw": 30, "roll": 0, "duration": 0.5},
-                {"pitch": 0, "yaw": 0, "roll": 0, "duration": 0.5}
+                {"pitch": 0, "yaw": 30, "roll": 0, "antennas": [5, -15], "duration": 0.5},
+                {"pitch": 0, "yaw": 0, "roll": 0, "antennas": [0, 0], "duration": 0.5}
             ],
             total_duration=1.0
         ),
         
         Gesture.EXCITED: GestureSequence(
             movements=[
-                # Quick forward lean with look up
-                {"pitch": 15, "yaw": 5, "roll": 0, "z": 10, "duration": 0.3},
-                # Slight turn left
-                {"pitch": 10, "yaw": -10, "roll": 0, "z": 10, "duration": 0.3},
-                # Slight turn right
-                {"pitch": 10, "yaw": 10, "roll": 0, "z": 10, "duration": 0.3},
+                # Quick forward lean with look up and antennas up
+                {"pitch": 15, "yaw": 5, "roll": 0, "z": 10, "antennas": [30, 30], "duration": 0.3},
+                # Slight turn left with asymmetric antennas
+                {"pitch": 10, "yaw": -10, "roll": 0, "z": 10, "antennas": [35, 25], "duration": 0.3},
+                # Slight turn right with asymmetric antennas
+                {"pitch": 10, "yaw": 10, "roll": 0, "z": 10, "antennas": [25, 35], "duration": 0.3},
                 # Return to neutral
-                {"pitch": 0, "yaw": 0, "roll": 0, "z": 0, "duration": 0.4}
+                {"pitch": 0, "yaw": 0, "roll": 0, "z": 0, "antennas": [0, 0], "duration": 0.4}
             ],
             total_duration=1.3
         ),
         
         Gesture.CONCERNED: GestureSequence(
             movements=[
-                # Slow tilt left with slight down look
-                {"pitch": -5, "yaw": 0, "roll": -15, "duration": 0.6},
+                # Slow tilt left with slight down look and drooping antennas
+                {"pitch": -5, "yaw": 0, "roll": -15, "antennas": [-20, -20], "duration": 0.6},
                 # Hold position
-                {"pitch": -5, "yaw": 0, "roll": -15, "duration": 0.4},
+                {"pitch": -5, "yaw": 0, "roll": -15, "antennas": [-20, -20], "duration": 0.4},
                 # Return to neutral slowly
-                {"pitch": 0, "yaw": 0, "roll": 0, "duration": 0.6}
+                {"pitch": 0, "yaw": 0, "roll": 0, "antennas": [0, 0], "duration": 0.6}
             ],
             total_duration=1.6
         ),
@@ -411,13 +412,18 @@ class MotionController:
                     z = movement.get("z", 0)
                     duration = movement.get("duration", 1.0)
                     
+                    # Extract antenna positions (in degrees, will be converted to radians)
+                    antenna_angles = movement.get("antennas", [0, 0])
+                    antennas = np.deg2rad(antenna_angles)
+                    
                     # Apply speed limiting (Requirement 7.8)
                     duration = self._apply_speed_limit(pitch, yaw, roll, duration)
                     
-                    # Execute movement
+                    # Execute movement with antennas
                     success = self.reachy.move_head(
                         x=x, y=y, z=z,
                         roll=roll, pitch=pitch, yaw=yaw,
+                        antennas=antennas,
                         duration=duration,
                         method="minjerk"
                     )