Merge pull request #34 from pollen-robotics/25-external-threads-should-raise-flags-and-never-mute-states-themselves-this-would-remove-the-need-for-most-locks

pyproject.toml

@@ -17,6 +17,7 @@ dependencies = [
     "mediapipe>=0.10.14",
     "num2words",
     "onnxruntime",
+    "opencv-python>=4.12.0.88",
     "openai>=2.1",
     "PyGObject>=3.42.2,<=3.46.0",
     "python-dotenv",

src/reachy_mini_conversation_demo/audio/head_wobbler.py

@@ -5,7 +5,7 @@ import queue
 import base64
 import logging
 import threading
-from typing import Optional
+from typing import Tuple, Optional
 
 import numpy as np
 
@@ -20,22 +20,29 @@ logger = logging.getLogger(__name__)
 class HeadWobbler:
     """Converts audio deltas (base64) into head movement offsets."""
 
-    def __init__(self, set_offsets):
+    def __init__(self, set_speech_offsets):
         """Initialize the head wobbler."""
-        self._apply_offsets = set_offsets
+        self._apply_offsets = set_speech_offsets
         self._base_ts: Optional[float] = None
         self._hops_done: int = 0
 
-        self.audio_queue: queue.Queue = queue.Queue()
+        self.audio_queue: queue.Queue[Tuple[int, int, np.ndarray]] = queue.Queue()
         self.sway = SwayRollRT()
 
+        # Synchronization primitives
+        self._state_lock = threading.Lock()
+        self._sway_lock = threading.Lock()
+        self._generation = 0
+
         self._stop_event = threading.Event()
         self._thread: Optional[threading.Thread] = None
 
     def feed(self, delta_b64: str) -> None:
         """Thread-safe: push audio into the consumer queue."""
         buf = np.frombuffer(base64.b64decode(delta_b64), dtype=np.int16).reshape(1, -1)
-        self.audio_queue.put((SAMPLE_RATE, buf))
+        with self._state_lock:
+            generation = self._generation
+        self.audio_queue.put((generation, SAMPLE_RATE, buf))
 
     def start(self) -> None:
         """Start the head wobbler loop in a thread."""
@@ -57,54 +64,84 @@ class HeadWobbler:
 
         logger.debug("Head wobbler thread started")
         while not self._stop_event.is_set():
+            queue_ref = self.audio_queue
             try:
-                sr, chunk = self.audio_queue.get_nowait()  # (1,N) int16
+                chunk_generation, sr, chunk = queue_ref.get_nowait()  # (gen, sr, data)
             except queue.Empty:
                 # avoid while to never exit
                 time.sleep(MOVEMENT_LATENCY_S)
                 continue
 
-            pcm = np.asarray(chunk).squeeze(0)
-            results = self.sway.feed(pcm, sr)
-            self.audio_queue.task_done()
+            try:
+                with self._state_lock:
+                    current_generation = self._generation
+                if chunk_generation != current_generation:
+                    continue
 
-            if self._base_ts is None:
-                self._base_ts = time.time()
+                pcm = np.asarray(chunk).squeeze(0)
+                with self._sway_lock:
+                    results = self.sway.feed(pcm, sr)
 
-            i = 0
-            while i < len(results):
                 if self._base_ts is None:
-                    self._base_ts = time.time()
-                    continue
-
-                target = self._base_ts + MOVEMENT_LATENCY_S + self._hops_done * hop_dt
-                now = time.time()
-
-                if now - target >= hop_dt:
-                    lag_hops = int((now - target) / hop_dt)
-                    drop = min(lag_hops, len(results) - i - 1)
-                    if drop > 0:
-                        self._hops_done += drop
-                        i += drop
-                        continue
-
-                if target > now:
-                    time.sleep(target - now)
-
-                r = results[i]
-                offsets = (
-                    r["x_mm"] / 1000.0,
-                    r["y_mm"] / 1000.0,
-                    r["z_mm"] / 1000.0,
-                    r["roll_rad"],
-                    r["pitch_rad"],
-                    r["yaw_rad"],
-                )
-
-                self._apply_offsets(offsets)
-
-                self._hops_done += 1
-                i += 1
+                    with self._state_lock:
+                        if self._base_ts is None:
+                            self._base_ts = time.time()
+
+                i = 0
+                while i < len(results):
+                    with self._state_lock:
+                        if self._generation != current_generation:
+                            break
+                        base_ts = self._base_ts
+                        hops_done = self._hops_done
+
+                    if base_ts is None:
+                        base_ts = time.time()
+                        with self._state_lock:
+                            if self._base_ts is None:
+                                self._base_ts = base_ts
+                                hops_done = self._hops_done
+
+                    target = base_ts + MOVEMENT_LATENCY_S + hops_done * hop_dt
+                    now = time.time()
+
+                    if now - target >= hop_dt:
+                        lag_hops = int((now - target) / hop_dt)
+                        drop = min(lag_hops, len(results) - i - 1)
+                        if drop > 0:
+                            with self._state_lock:
+                                self._hops_done += drop
+                                hops_done = self._hops_done
+                            i += drop
+                            continue
+
+                    if target > now:
+                        time.sleep(target - now)
+                        with self._state_lock:
+                            if self._generation != current_generation:
+                                break
+
+                    r = results[i]
+                    offsets = (
+                        r["x_mm"] / 1000.0,
+                        r["y_mm"] / 1000.0,
+                        r["z_mm"] / 1000.0,
+                        r["roll_rad"],
+                        r["pitch_rad"],
+                        r["yaw_rad"],
+                    )
+
+                    with self._state_lock:
+                        if self._generation != current_generation:
+                            break
+
+                    self._apply_offsets(offsets)
+
+                    with self._state_lock:
+                        self._hops_done += 1
+                    i += 1
+            finally:
+                queue_ref.task_done()
         logger.debug("Head wobbler thread exited")
 
     '''
@@ -119,8 +156,24 @@ class HeadWobbler:
 
     def reset(self) -> None:
         """Reset the internal state."""
-        # self.drain_audio_queue()
-        self.audio_queue = queue.Queue()
-        self._base_ts = None
-        self._hops_done = 0
-        self.sway.reset()
+        with self._state_lock:
+            self._generation += 1
+            self._base_ts = None
+            self._hops_done = 0
+
+        # Drain any queued audio chunks from previous generations
+        drained_any = False
+        while True:
+            try:
+                _, _, _ = self.audio_queue.get_nowait()
+            except queue.Empty:
+                break
+            else:
+                drained_any = True
+                self.audio_queue.task_done()
+
+        with self._sway_lock:
+            self.sway.reset()
+
+        if drained_any:
+            logger.debug("Head wobbler queue drained during reset")

src/reachy_mini_conversation_demo/main.py

@@ -31,17 +31,16 @@ def main():
     logger = setup_logger(args.debug)
     logger.info("Starting Reachy Mini Conversation Demo")
 
-    robot = ReachyMini()
+    robot = ReachyMini(use_sim=False)
 
-    camera_worker, head_tracker, vision_manager = handle_vision_stuff(args, robot)
+    camera_worker, _, vision_manager = handle_vision_stuff(args, robot)
 
     movement_manager = MovementManager(
         current_robot=robot,
-        head_tracker=head_tracker,
         camera_worker=camera_worker,
     )
 
-    head_wobbler = HeadWobbler(set_offsets=movement_manager.set_offsets)
+    head_wobbler = HeadWobbler(set_speech_offsets=movement_manager.set_speech_offsets)
 
     deps = ToolDependencies(
         reachy_mini=robot,

src/reachy_mini_conversation_demo/moves.py

@@ -1,16 +1,42 @@
 """Movement system with sequential primary moves and additive secondary moves.
 
-This module implements the movement architecture from main_works.py:
-- Primary moves (sequential): emotions, dances, goto, breathing
-- Secondary moves (additive): speech offsets + face tracking
-- Single set_target() control point with pose fusion
+Design overview
+- Primary moves (emotions, dances, goto, breathing) are mutually exclusive and run
+  sequentially.
+- Secondary moves (speech sway, face tracking) are additive offsets applied on top
+  of the current primary pose.
+- There is a single control point to the robot: `ReachyMini.set_target`.
+- The control loop runs near 100 Hz and is phase-aligned via a monotonic clock.
+- Idle behaviour starts an infinite `BreathingMove` after a short inactivity delay
+  unless listening is active.
+
+Threading model
+- A dedicated worker thread owns all real-time state and issues `set_target`
+  commands.
+- Other threads communicate via a command queue (enqueue moves, mark activity,
+  toggle listening).
+- Secondary offset producers set pending values guarded by locks; the worker
+  snaps them atomically.
+
+Units and frames
+- Secondary offsets are interpreted as metres for x/y/z and radians for
+  roll/pitch/yaw in the world frame (unless noted by `compose_world_offset`).
+- Antennas and `body_yaw` are in radians.
+- Head pose composition uses `compose_world_offset(primary_head, secondary_head)`;
+  the secondary offset must therefore be expressed in the world frame.
+
+Safety
+- Listening freezes antennas, then blends them back on unfreeze.
+- Interpolations and blends are used to avoid jumps at all times.
+- `set_target` errors are rate-limited in logs.
 """
 
 from __future__ import annotations
 import time
 import logging
 import threading
-from typing import Tuple, Optional
+from queue import Empty, Queue
+from typing import Any, Tuple, Optional
 from collections import deque
 from dataclasses import dataclass
 
@@ -27,6 +53,9 @@ from reachy_mini.utils.interpolation import (
 
 logger = logging.getLogger(__name__)
 
+# Configuration constants
+CONTROL_LOOP_FREQUENCY_HZ = 100.0  # Hz - Target frequency for the movement control loop
+
 # Type definitions
 FullBodyPose = Tuple[np.ndarray, Tuple[float, float], float]  # (head_pose_4x4, antennas, body_yaw)
 
@@ -113,8 +142,9 @@ def combine_full_body(primary_pose: FullBodyPose, secondary_pose: FullBodyPose)
     primary_head, primary_antennas, primary_body_yaw = primary_pose
     secondary_head, secondary_antennas, secondary_body_yaw = secondary_pose
 
-    # Combine head poses using compose_world_offset
-    # primary_head is T_abs, secondary_head is T_off_world
+    # Combine head poses using compose_world_offset; the secondary pose must be an
+    # offset expressed in the world frame (T_off_world) applied to the absolute
+    # primary transform (T_abs).
     combined_head = compose_world_offset(primary_head, secondary_head, reorthonormalize=True)
 
     # Sum antennas and body_yaw
@@ -171,32 +201,65 @@ class MovementState:
 
     def update_activity(self) -> None:
         """Update the last activity time."""
-        self.last_activity_time = time.time()
+        self.last_activity_time = time.monotonic()
 
 
-class MovementManager:
-    """Enhanced movement manager that reproduces main_works.py behavior.
+@dataclass
+class LoopFrequencyStats:
+    """Track rolling loop frequency statistics."""
+
+    mean: float = 0.0
+    m2: float = 0.0
+    min_freq: float = float("inf")
+    count: int = 0
+    last_freq: float = 0.0
+    potential_freq: float = 0.0
 
-    - Sequential primary moves via queue
-    - Additive secondary moves (speech + face tracking)
-    - Single set_target control loop with pose fusion
-    - Automatic breathing after inactivity
+    def reset(self) -> None:
+        """Reset accumulators while keeping the last potential frequency."""
+        self.mean = 0.0
+        self.m2 = 0.0
+        self.min_freq = float("inf")
+        self.count = 0
+
+
+class MovementManager:
+    """Coordinate sequential moves, additive offsets, and robot output at 100 Hz.
+
+    Responsibilities:
+    - Own a real-time loop that samples the current primary move (if any), fuses
+      secondary offsets, and calls `set_target` exactly once per tick.
+    - Start an idle `BreathingMove` after `idle_inactivity_delay` when not
+      listening and no moves are queued.
+    - Expose thread-safe APIs so other threads can enqueue moves, mark activity,
+      or feed secondary offsets without touching internal state.
+
+    Timing:
+    - All elapsed-time calculations rely on `time.monotonic()` through `self._now`
+      to avoid wall-clock jumps.
+    - The loop attempts 100 Hz
+
+    Concurrency:
+    - External threads communicate via `_command_queue` messages.
+    - Secondary offsets are staged via dirty flags guarded by locks and consumed
+      atomically inside the worker loop.
     """
 
     def __init__(
         self,
         current_robot: ReachyMini,
-        head_tracker=None,
         camera_worker=None,
     ):
         """Initialize movement manager."""
         self.current_robot = current_robot
-        self.head_tracker = head_tracker
         self.camera_worker = camera_worker
 
+        # Single timing source for durations
+        self._now = time.monotonic
+
         # Movement state
         self.state = MovementState()
-        self.state.last_activity_time = time.time()
+        self.state.last_activity_time = self._now()
         neutral_pose = create_head_pose(0, 0, 0, 0, 0, 0, degrees=True)
         self.state.last_primary_pose = (neutral_pose, (0.0, 0.0), 0.0)
 
@@ -204,186 +267,328 @@ class MovementManager:
         self.move_queue = deque()
 
         # Configuration
-        self.idle_inactivity_delay = 5.0  # seconds
-        self.target_frequency = 50.0  # Hz
+        self.idle_inactivity_delay = 0.3  # seconds
+        self.target_frequency = CONTROL_LOOP_FREQUENCY_HZ
         self.target_period = 1.0 / self.target_frequency
 
         self._stop_event = threading.Event()
         self._thread: Optional[threading.Thread] = None
-        self._state_lock = threading.RLock()
         self._is_listening = False
         self._last_commanded_pose: FullBodyPose = clone_full_body_pose(self.state.last_primary_pose)
         self._listening_antennas: Tuple[float, float] = self._last_commanded_pose[1]
         self._antenna_unfreeze_blend = 1.0
         self._antenna_blend_duration = 0.4  # seconds to blend back after listening
-        self._last_listening_blend_time = time.monotonic()
+        self._last_listening_blend_time = self._now()
+        self._breathing_active = False  # true when breathing move is running or queued
+        self._listening_debounce_s = 0.15
+        self._last_listening_toggle_time = self._now()
+        self._last_set_target_err = 0.0
+        self._set_target_err_interval = 1.0  # seconds between error logs
+        self._set_target_err_suppressed = 0
+
+        # Cross-thread signalling
+        self._command_queue: Queue[tuple[str, Any]] = Queue()
+        self._speech_offsets_lock = threading.Lock()
+        self._pending_speech_offsets: Tuple[float, float, float, float, float, float] = (
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        )
+        self._speech_offsets_dirty = False
+
+        self._face_offsets_lock = threading.Lock()
+        self._pending_face_offsets: Tuple[float, float, float, float, float, float] = (
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        )
+        self._face_offsets_dirty = False
+
+        self._shared_state_lock = threading.Lock()
+        self._shared_last_activity_time = self.state.last_activity_time
+        self._shared_is_listening = self._is_listening
+        self._status_lock = threading.Lock()
+        self._freq_stats = LoopFrequencyStats()
+        self._freq_snapshot = LoopFrequencyStats()
 
     def queue_move(self, move: Move) -> None:
-        """Add a move to the primary move queue."""
-        with self._state_lock:
-            self.move_queue.append(move)
-            self.state.update_activity()
-            logger.info(f"Queued move with duration {move.duration}s, queue size: {len(self.move_queue)}")
+        """Queue a primary move to run after the currently executing one.
+
+        Thread-safe: the move is enqueued via the worker command queue so the
+        control loop remains the sole mutator of movement state.
+        """
+        self._command_queue.put(("queue_move", move))
 
     def clear_queue(self) -> None:
-        """Clear all queued moves and stop current move."""
-        with self._state_lock:
-            self.move_queue.clear()
-            self.state.current_move = None
-            self.state.move_start_time = None
-            self.state.is_playing_move = False
-            logger.info("Cleared move queue and stopped current move")
+        """Stop the active move and discard any queued primary moves.
 
-    def set_speech_offsets(self, offsets: Tuple[float, float, float, float, float, float]) -> None:
-        """Set speech head offsets (secondary move)."""
-        with self._state_lock:
-            self.state.speech_offsets = offsets
+        Thread-safe: executed by the worker thread via the command queue.
+        """
+        self._command_queue.put(("clear_queue", None))
 
-    def set_offsets(self, offsets: Tuple[float, float, float, float, float, float]) -> None:
-        """Compatibility alias for set_speech_offsets."""
-        self.set_speech_offsets(offsets)
+    def set_speech_offsets(self, offsets: Tuple[float, float, float, float, float, float]) -> None:
+        """Update speech-induced secondary offsets (x, y, z, roll, pitch, yaw).
 
-    def set_face_tracking_offsets(self, offsets: Tuple[float, float, float, float, float, float]) -> None:
-        """Set face tracking offsets (secondary move)."""
-        with self._state_lock:
-            self.state.face_tracking_offsets = offsets
+        Offsets are interpreted as metres for translation and radians for
+        rotation in the world frame. Thread-safe via a pending snapshot.
+        """
+        with self._speech_offsets_lock:
+            self._pending_speech_offsets = offsets
+            self._speech_offsets_dirty = True
 
     def set_moving_state(self, duration: float) -> None:
-        """Set legacy moving state for goto moves."""
-        with self._state_lock:
-            self.state.moving_start = time.time()
-            self.state.moving_for = duration
-            self.state.update_activity()
+        """Mark the robot as actively moving for the provided duration.
 
-    def is_idle(self):
-        """Check if the robot is idle based on inactivity delay."""
-        with self._state_lock:
-            if self._is_listening:
-                return False
-            current_time = time.time()
-            time_since_activity = current_time - self.state.last_activity_time
-            return time_since_activity >= self.idle_inactivity_delay
+        Legacy hook used by goto helpers to keep inactivity and breathing logic
+        aware of manual motions. Thread-safe via the command queue.
+        """
+        self._command_queue.put(("set_moving_state", duration))
+
+    def is_idle(self) -> bool:
+        """Return True when the robot has been inactive longer than the idle delay."""
+        with self._shared_state_lock:
+            last_activity = self._shared_last_activity_time
+            listening = self._shared_is_listening
+
+        if listening:
+            return False
+
+        return self._now() - last_activity >= self.idle_inactivity_delay
 
     def mark_user_activity(self) -> None:
-        """Record recent user activity to delay idle behaviours."""
-        with self._state_lock:
-            self.state.update_activity()
+        """Record external activity and postpone idle behaviours (thread-safe)."""
+        self._command_queue.put(("mark_activity", None))
 
     def set_listening(self, listening: bool) -> None:
-        """Toggle listening mode, freezing antennas when active."""
-        with self._state_lock:
-            if self._is_listening == listening:
+        """Enable or disable listening mode without touching shared state directly.
+
+        While listening:
+        - Antenna positions are frozen at the last commanded values.
+        - Blending is reset so that upon unfreezing the antennas return smoothly.
+        - Idle breathing is suppressed.
+
+        Thread-safe: the change is posted to the worker command queue.
+        """
+        with self._shared_state_lock:
+            if self._shared_is_listening == listening:
+                return
+        self._command_queue.put(("set_listening", listening))
+
+    def _poll_signals(self, current_time: float) -> None:
+        """Apply queued commands and pending offset updates."""
+        self._apply_pending_offsets()
+
+        while True:
+            try:
+                command, payload = self._command_queue.get_nowait()
+            except Empty:
+                break
+            self._handle_command(command, payload, current_time)
+
+    def _apply_pending_offsets(self) -> None:
+        """Apply the most recent speech/face offset updates."""
+        speech_offsets: Optional[Tuple[float, float, float, float, float, float]] = None
+        with self._speech_offsets_lock:
+            if self._speech_offsets_dirty:
+                speech_offsets = self._pending_speech_offsets
+                self._speech_offsets_dirty = False
+
+        if speech_offsets is not None:
+            self.state.speech_offsets = speech_offsets
+            self.state.update_activity()
+
+        face_offsets: Optional[Tuple[float, float, float, float, float, float]] = None
+        with self._face_offsets_lock:
+            if self._face_offsets_dirty:
+                face_offsets = self._pending_face_offsets
+                self._face_offsets_dirty = False
+
+        if face_offsets is not None:
+            self.state.face_tracking_offsets = face_offsets
+            self.state.update_activity()
+
+    def _handle_command(self, command: str, payload: Any, current_time: float) -> None:
+        """Handle a single cross-thread command."""
+        if command == "queue_move":
+            if isinstance(payload, Move):
+                self.move_queue.append(payload)
+                self.state.update_activity()
+                duration = getattr(payload, "duration", None)
+                if duration is not None:
+                    try:
+                        duration_str = f"{float(duration):.2f}"
+                    except (TypeError, ValueError):
+                        duration_str = str(duration)
+                else:
+                    duration_str = "?"
+                logger.info(
+                    "Queued move with duration %ss, queue size: %s",
+                    duration_str,
+                    len(self.move_queue),
+                )
+            else:
+                logger.warning("Ignored queue_move command with invalid payload: %s", payload)
+        elif command == "clear_queue":
+            self.move_queue.clear()
+            self.state.current_move = None
+            self.state.move_start_time = None
+            self.state.is_playing_move = False
+            self._breathing_active = False
+            logger.info("Cleared move queue and stopped current move")
+        elif command == "set_moving_state":
+            try:
+                duration = float(payload)
+            except (TypeError, ValueError):
+                logger.warning("Invalid moving state duration: %s", payload)
+                return
+            self.state.moving_start = current_time
+            self.state.moving_for = max(0.0, duration)
+            self.state.update_activity()
+        elif command == "mark_activity":
+            self.state.update_activity()
+        elif command == "set_listening":
+            desired_state = bool(payload)
+            now = self._now()
+            if now - self._last_listening_toggle_time < self._listening_debounce_s:
                 return
-            self._is_listening = listening
-            self._last_listening_blend_time = time.monotonic()
-            if listening:
-                # Capture the last antenna command so we keep that pose during listening
+            self._last_listening_toggle_time = now
+
+            if self._is_listening == desired_state:
+                return
+
+            self._is_listening = desired_state
+            self._last_listening_blend_time = now
+            if desired_state:
+                # Freeze: snapshot current commanded antennas and reset blend
                 self._listening_antennas = (
                     float(self._last_commanded_pose[1][0]),
                     float(self._last_commanded_pose[1][1]),
                 )
                 self._antenna_unfreeze_blend = 0.0
+            else:
+                # Unfreeze: restart blending from frozen pose
+                self._antenna_unfreeze_blend = 0.0
             self.state.update_activity()
+        else:
+            logger.warning("Unknown command received by MovementManager: %s", command)
+
+    def _publish_shared_state(self) -> None:
+        """Expose idle-related state for external threads."""
+        with self._shared_state_lock:
+            self._shared_last_activity_time = self.state.last_activity_time
+            self._shared_is_listening = self._is_listening
 
     def _manage_move_queue(self, current_time: float) -> None:
         """Manage the primary move queue (sequential execution)."""
-        with self._state_lock:
-            if self.state.current_move is None or (
-                self.state.move_start_time is not None
-                and current_time - self.state.move_start_time >= self.state.current_move.duration
-            ):
-                self.state.current_move = None
-                self.state.move_start_time = None
-
-                if self.move_queue:
-                    self.state.current_move = self.move_queue.popleft()
-                    self.state.move_start_time = current_time
-                    logger.debug(f"Starting new move, duration: {self.state.current_move.duration}s")
+        if self.state.current_move is None or (
+            self.state.move_start_time is not None
+            and current_time - self.state.move_start_time >= self.state.current_move.duration
+        ):
+            self.state.current_move = None
+            self.state.move_start_time = None
+
+            if self.move_queue:
+                self.state.current_move = self.move_queue.popleft()
+                self.state.move_start_time = current_time
+                # Any real move cancels breathing mode flag
+                self._breathing_active = isinstance(self.state.current_move, BreathingMove)
+                logger.info(f"Starting new move, duration: {self.state.current_move.duration}s")
 
     def _manage_breathing(self, current_time: float) -> None:
         """Manage automatic breathing when idle."""
-        # Start breathing after inactivity delay if no moves in queue
-        with self._state_lock:
-            if self.state.current_move is None and not self.move_queue:
-                time_since_activity = current_time - self.state.last_activity_time
+        if (
+            self.state.current_move is None
+            and not self.move_queue
+            and not self._is_listening
+            and not self._breathing_active
+        ):
+            idle_for = current_time - self.state.last_activity_time
+            if idle_for >= self.idle_inactivity_delay:
+                try:
+                    # These 2 functions return the latest available sensor data from the robot, but don't perform I/O synchronously.
+                    # Therefore, we accept calling them inside the control loop.
+                    _, current_antennas = self.current_robot.get_current_joint_positions()
+                    current_head_pose = self.current_robot.get_current_head_pose()
+
+                    self._breathing_active = True
+                    self.state.update_activity()
+
+                    breathing_move = BreathingMove(
+                        interpolation_start_pose=current_head_pose,
+                        interpolation_start_antennas=current_antennas,
+                        interpolation_duration=1.0,
+                    )
+                    self.move_queue.append(breathing_move)
+                    logger.info("Started breathing after %.1fs of inactivity", idle_for)
+                except Exception as e:
+                    self._breathing_active = False
+                    logger.error("Failed to start breathing: %s", e)
+
+        if isinstance(self.state.current_move, BreathingMove) and self.move_queue:
+            self.state.current_move = None
+            self.state.move_start_time = None
+            self._breathing_active = False
+            logger.info("Stopping breathing due to new move activity")
 
-                if self.is_idle():
-                    try:
-                        _, current_antennas = self.current_robot.get_current_joint_positions()
-                        current_head_pose = self.current_robot.get_current_head_pose()
-
-                        breathing_move = BreathingMove(
-                            interpolation_start_pose=current_head_pose,
-                            interpolation_start_antennas=current_antennas,
-                            interpolation_duration=1.0,
-                        )
-                        self.move_queue.append(breathing_move)
-                        self.state.update_activity()
-                        logger.debug(f"Started breathing after {time_since_activity:.1f}s of inactivity")
-                    except Exception as e:
-                        logger.error(f"Failed to start breathing: {e}")
-
-            if (
-                self.state.current_move is not None
-                and isinstance(self.state.current_move, BreathingMove)
-                and self.move_queue
-            ):
-                self.state.current_move = None
-                self.state.move_start_time = None
-                logger.info("Stopping breathing due to new move activity")
+        if self.state.current_move is not None and not isinstance(self.state.current_move, BreathingMove):
+            self._breathing_active = False
 
     def _get_primary_pose(self, current_time: float) -> FullBodyPose:
         """Get the primary full body pose from current move or neutral."""
-        with self._state_lock:
-            # When a primary move is playing, sample it and cache the resulting pose
-            if self.state.current_move is not None and self.state.move_start_time is not None:
-                move_time = current_time - self.state.move_start_time
-                head, antennas, body_yaw = self.state.current_move.evaluate(move_time)
-
-                if head is None:
-                    head = create_head_pose(0, 0, 0, 0, 0, 0, degrees=True)
-                if antennas is None:
-                    antennas = np.array([0.0, 0.0])
-                if body_yaw is None:
-                    body_yaw = 0.0
-
-                antennas_tuple = (float(antennas[0]), float(antennas[1]))
-                head_copy = head.copy()
-                primary_full_body_pose = (
-                    head_copy,
-                    antennas_tuple,
-                    float(body_yaw),
-                )
+        # When a primary move is playing, sample it and cache the resulting pose
+        if self.state.current_move is not None and self.state.move_start_time is not None:
+            move_time = current_time - self.state.move_start_time
+            head, antennas, body_yaw = self.state.current_move.evaluate(move_time)
+
+            if head is None:
+                head = create_head_pose(0, 0, 0, 0, 0, 0, degrees=True)
+            if antennas is None:
+                antennas = np.array([0.0, 0.0])
+            if body_yaw is None:
+                body_yaw = 0.0
+
+            antennas_tuple = (float(antennas[0]), float(antennas[1]))
+            head_copy = head.copy()
+            primary_full_body_pose = (
+                head_copy,
+                antennas_tuple,
+                float(body_yaw),
+            )
 
-                self.state.is_playing_move = True
-                self.state.is_moving = True
-                self.state.last_primary_pose = clone_full_body_pose(primary_full_body_pose)
-            else:
-                # Otherwise reuse the last primary pose so we avoid jumps between moves
-                self.state.is_playing_move = False
-                self.state.is_moving = time.time() - self.state.moving_start < self.state.moving_for
+            self.state.is_playing_move = True
+            self.state.is_moving = True
+            self.state.last_primary_pose = clone_full_body_pose(primary_full_body_pose)
+        else:
+            # Otherwise reuse the last primary pose so we avoid jumps between moves
+            self.state.is_playing_move = False
+            self.state.is_moving = current_time - self.state.moving_start < self.state.moving_for
 
-                if self.state.last_primary_pose is not None:
-                    primary_full_body_pose = clone_full_body_pose(self.state.last_primary_pose)
-                else:
-                    neutral_head_pose = create_head_pose(0, 0, 0, 0, 0, 0, degrees=True)
-                    primary_full_body_pose = (neutral_head_pose, (0.0, 0.0), 0.0)
-                    self.state.last_primary_pose = clone_full_body_pose(primary_full_body_pose)
+            if self.state.last_primary_pose is not None:
+                primary_full_body_pose = clone_full_body_pose(self.state.last_primary_pose)
+            else:
+                neutral_head_pose = create_head_pose(0, 0, 0, 0, 0, 0, degrees=True)
+                primary_full_body_pose = (neutral_head_pose, (0.0, 0.0), 0.0)
+                self.state.last_primary_pose = clone_full_body_pose(primary_full_body_pose)
 
         return primary_full_body_pose
 
     def _get_secondary_pose(self) -> FullBodyPose:
         """Get the secondary full body pose from speech and face tracking offsets."""
         # Combine speech sway offsets + face tracking offsets for secondary pose
-        with self._state_lock:
-            secondary_offsets = [
-                self.state.speech_offsets[0] + self.state.face_tracking_offsets[0],
-                self.state.speech_offsets[1] + self.state.face_tracking_offsets[1],
-                self.state.speech_offsets[2] + self.state.face_tracking_offsets[2],
-                self.state.speech_offsets[3] + self.state.face_tracking_offsets[3],
-                self.state.speech_offsets[4] + self.state.face_tracking_offsets[4],
-                self.state.speech_offsets[5] + self.state.face_tracking_offsets[5],
-            ]
+        secondary_offsets = [
+            self.state.speech_offsets[0] + self.state.face_tracking_offsets[0],
+            self.state.speech_offsets[1] + self.state.face_tracking_offsets[1],
+            self.state.speech_offsets[2] + self.state.face_tracking_offsets[2],
+            self.state.speech_offsets[3] + self.state.face_tracking_offsets[3],
+            self.state.speech_offsets[4] + self.state.face_tracking_offsets[4],
+            self.state.speech_offsets[5] + self.state.face_tracking_offsets[5],
+        ]
 
         secondary_head_pose = create_head_pose(
             x=secondary_offsets[0],
@@ -395,127 +600,236 @@ class MovementManager:
             degrees=False,
             mm=False,
         )
-        return (secondary_head_pose, (0, 0), 0)
+        return (secondary_head_pose, (0.0, 0.0), 0.0)
+
+    def _compose_full_body_pose(self, current_time: float) -> FullBodyPose:
+        """Compose primary and secondary poses into a single command pose."""
+        primary = self._get_primary_pose(current_time)
+        secondary = self._get_secondary_pose()
+        return combine_full_body(primary, secondary)
+
+    def _update_primary_motion(self, current_time: float) -> None:
+        """Advance queue state and idle behaviours for this tick."""
+        self._manage_move_queue(current_time)
+        self._manage_breathing(current_time)
+
+    def _calculate_blended_antennas(self, target_antennas: Tuple[float, float]) -> Tuple[float, float]:
+        """Blend target antennas with listening freeze state and update blending."""
+        now = self._now()
+        listening = self._is_listening
+        listening_antennas = self._listening_antennas
+        blend = self._antenna_unfreeze_blend
+        blend_duration = self._antenna_blend_duration
+        last_update = self._last_listening_blend_time
+        self._last_listening_blend_time = now
+
+        if listening:
+            antennas_cmd = listening_antennas
+            new_blend = 0.0
+        else:
+            dt = max(0.0, now - last_update)
+            if blend_duration <= 0:
+                new_blend = 1.0
+            else:
+                new_blend = min(1.0, blend + dt / blend_duration)
+            antennas_cmd = (
+                listening_antennas[0] * (1.0 - new_blend) + target_antennas[0] * new_blend,
+                listening_antennas[1] * (1.0 - new_blend) + target_antennas[1] * new_blend,
+            )
+
+        if listening:
+            self._antenna_unfreeze_blend = 0.0
+        else:
+            self._antenna_unfreeze_blend = new_blend
+            if new_blend >= 1.0:
+                self._listening_antennas = (
+                    float(target_antennas[0]),
+                    float(target_antennas[1]),
+                )
+
+        return antennas_cmd
+
+    def _issue_control_command(self, head: np.ndarray, antennas: Tuple[float, float], body_yaw: float) -> None:
+        """Send the fused pose to the robot with throttled error logging."""
+        try:
+            self.current_robot.set_target(head=head, antennas=antennas, body_yaw=body_yaw)
+        except Exception as e:
+            now = self._now()
+            if now - self._last_set_target_err >= self._set_target_err_interval:
+                msg = f"Failed to set robot target: {e}"
+                if self._set_target_err_suppressed:
+                    msg += f" (suppressed {self._set_target_err_suppressed} repeats)"
+                    self._set_target_err_suppressed = 0
+                logger.error(msg)
+                self._last_set_target_err = now
+            else:
+                self._set_target_err_suppressed += 1
+        else:
+            with self._status_lock:
+                self._last_commanded_pose = clone_full_body_pose((head, antennas, body_yaw))
+
+    def _update_frequency_stats(
+        self, loop_start: float, prev_loop_start: float, stats: LoopFrequencyStats
+    ) -> LoopFrequencyStats:
+        """Update frequency statistics based on the current loop start time."""
+        period = loop_start - prev_loop_start
+        if period > 0:
+            stats.last_freq = 1.0 / period
+            stats.count += 1
+            delta = stats.last_freq - stats.mean
+            stats.mean += delta / stats.count
+            stats.m2 += delta * (stats.last_freq - stats.mean)
+            stats.min_freq = min(stats.min_freq, stats.last_freq)
+        return stats
+
+    def _schedule_next_tick(self, loop_start: float, stats: LoopFrequencyStats) -> tuple[float, LoopFrequencyStats]:
+        """Compute sleep time to maintain target frequency and update potential freq."""
+        computation_time = self._now() - loop_start
+        stats.potential_freq = 1.0 / computation_time if computation_time > 0 else float("inf")
+        sleep_time = max(0.0, self.target_period - computation_time)
+        return sleep_time, stats
+
+    def _record_frequency_snapshot(self, stats: LoopFrequencyStats) -> None:
+        """Store a thread-safe snapshot of current frequency statistics."""
+        with self._status_lock:
+            self._freq_snapshot = LoopFrequencyStats(
+                mean=stats.mean,
+                m2=stats.m2,
+                min_freq=stats.min_freq,
+                count=stats.count,
+                last_freq=stats.last_freq,
+                potential_freq=stats.potential_freq,
+            )
+
+    def _maybe_log_frequency(self, loop_count: int, print_interval_loops: int, stats: LoopFrequencyStats) -> None:
+        """Emit frequency telemetry when enough loops have elapsed."""
+        if loop_count % print_interval_loops != 0 or stats.count == 0:
+            return
+
+        variance = stats.m2 / stats.count if stats.count > 0 else 0.0
+        lowest = stats.min_freq if stats.min_freq != float("inf") else 0.0
+        logger.debug(
+            "Loop freq - avg: %.2fHz, variance: %.4f, min: %.2fHz, last: %.2fHz, potential: %.2fHz, target: %.1fHz",
+            stats.mean,
+            variance,
+            lowest,
+            stats.last_freq,
+            stats.potential_freq,
+            self.target_frequency,
+        )
+        stats.reset()
 
     def _update_face_tracking(self, current_time: float) -> None:
         """Get face tracking offsets from camera worker thread."""
         if self.camera_worker is not None:
             # Get face tracking offsets from camera worker thread
             offsets = self.camera_worker.get_face_tracking_offsets()
-            with self._state_lock:
-                self.state.face_tracking_offsets = offsets
+            self.state.face_tracking_offsets = offsets
         else:
             # No camera worker, use neutral offsets
-            with self._state_lock:
-                self.state.face_tracking_offsets = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+            self.state.face_tracking_offsets = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
 
     def start(self) -> None:
-        """Start the move worker loop in a thread."""
+        """Start the worker thread that drives the 100 Hz control loop."""
+        if self._thread is not None and self._thread.is_alive():
+            logger.warning("Move worker already running; start() ignored")
+            return
         self._stop_event.clear()
         self._thread = threading.Thread(target=self.working_loop, daemon=True)
         self._thread.start()
         logger.debug("Move worker started")
 
     def stop(self) -> None:
-        """Stop the move worker loop."""
+        """Request the worker thread to stop and wait for it to exit."""
         self._stop_event.set()
         if self._thread is not None:
             self._thread.join()
+            self._thread = None
+        logger.debug("Move worker stopped")
         logger.debug("Move worker stopped")
 
+    def get_status(self) -> dict[str, Any]:
+        """Return a lightweight status snapshot for observability."""
+        with self._status_lock:
+            pose_snapshot = clone_full_body_pose(self._last_commanded_pose)
+            freq_snapshot = LoopFrequencyStats(
+                mean=self._freq_snapshot.mean,
+                m2=self._freq_snapshot.m2,
+                min_freq=self._freq_snapshot.min_freq,
+                count=self._freq_snapshot.count,
+                last_freq=self._freq_snapshot.last_freq,
+                potential_freq=self._freq_snapshot.potential_freq,
+            )
+
+        head_matrix = pose_snapshot[0].tolist() if pose_snapshot else None
+        antennas = pose_snapshot[1] if pose_snapshot else None
+        body_yaw = pose_snapshot[2] if pose_snapshot else None
+
+        return {
+            "queue_size": len(self.move_queue),
+            "is_listening": self._is_listening,
+            "breathing_active": self._breathing_active,
+            "last_commanded_pose": {
+                "head": head_matrix,
+                "antennas": antennas,
+                "body_yaw": body_yaw,
+            },
+            "loop_frequency": {
+                "last": freq_snapshot.last_freq,
+                "mean": freq_snapshot.mean,
+                "min": freq_snapshot.min_freq,
+                "potential": freq_snapshot.potential_freq,
+                "samples": freq_snapshot.count,
+            },
+        }
+
     def working_loop(self) -> None:
         """Control loop main movements - reproduces main_works.py control architecture.
 
         Single set_target() call with pose fusion.
         """
-        logger.debug("Starting enhanced movement control loop (50Hz)")
+        logger.debug("Starting enhanced movement control loop (100Hz)")
 
         loop_count = 0
-        last_print_time = time.time()
+        prev_loop_start = self._now()
+        print_interval_loops = max(1, int(self.target_frequency * 2))
+        freq_stats = self._freq_stats
 
         while not self._stop_event.is_set():
-            loop_start_time = time.time()
+            loop_start = self._now()
             loop_count += 1
-            current_time = time.time()
 
-            # 1. Manage move queue (sequential primary moves)
-            self._manage_move_queue(current_time)
+            if loop_count > 1:
+                freq_stats = self._update_frequency_stats(loop_start, prev_loop_start, freq_stats)
+            prev_loop_start = loop_start
 
-            # 2. Manage breathing (automatic idle behavior)
-            self._manage_breathing(current_time)
+            # 1) Poll external commands and apply pending offsets (atomic snapshot)
+            self._poll_signals(loop_start)
 
-            # 3. Update face tracking offsets
-            self._update_face_tracking(current_time)
+            # 2) Manage the primary move queue (start new move, end finished move, breathing)
+            self._update_primary_motion(loop_start)
 
-            # 4. Get primary pose from current move or neutral
-            primary_full_body_pose = self._get_primary_pose(current_time)
+            # 3) Update vision-based secondary offsets
+            self._update_face_tracking(loop_start)
 
-            # 5. Get secondary pose from speech and face tracking offsets
-            secondary_full_body_pose = self._get_secondary_pose()
+            # 4) Build primary and secondary full-body poses, then fuse them
+            head, antennas, body_yaw = self._compose_full_body_pose(loop_start)
 
-            # 6. Combine primary and secondary poses
-            global_full_body_pose = combine_full_body(primary_full_body_pose, secondary_full_body_pose)
+            # 5) Apply listening antenna freeze or blend-back
+            antennas_cmd = self._calculate_blended_antennas(antennas)
 
-            # 7. Extract pose components
-            head, antennas, body_yaw = global_full_body_pose
-            now_monotonic = time.monotonic()
-            with self._state_lock:
-                listening = self._is_listening
-                listening_antennas = self._listening_antennas
-                blend = self._antenna_unfreeze_blend
-                blend_duration = self._antenna_blend_duration
-                last_update = self._last_listening_blend_time
-                self._last_listening_blend_time = now_monotonic
+            # 6) Single set_target call - the only control point
+            self._issue_control_command(head, antennas_cmd, body_yaw)
 
-            # Blend antenna outputs back to the live motion when leaving listening mode
-            if listening:
-                antennas_cmd = listening_antennas
-                new_blend = 0.0
-            else:
-                dt = max(0.0, now_monotonic - last_update)
-                if blend_duration <= 0:
-                    new_blend = 1.0
-                else:
-                    new_blend = min(1.0, blend + dt / blend_duration)
-                antennas_cmd = (
-                    listening_antennas[0] * (1.0 - new_blend) + antennas[0] * new_blend,
-                    listening_antennas[1] * (1.0 - new_blend) + antennas[1] * new_blend,
-                )
+            # 7) Adaptive sleep to align to next tick, then publish shared state
+            sleep_time, freq_stats = self._schedule_next_tick(loop_start, freq_stats)
+            self._publish_shared_state()
+            self._record_frequency_snapshot(freq_stats)
 
-            with self._state_lock:
-                if listening:
-                    self._antenna_unfreeze_blend = 0.0
-                else:
-                    self._antenna_unfreeze_blend = new_blend
-                    if new_blend >= 1.0:
-                        self._listening_antennas = (
-                            float(antennas[0]),
-                            float(antennas[1]),
-                        )
-
-            # 8. Single set_target call - the one and only place we control the robot
-            try:
-                self.current_robot.set_target(head=head, antennas=antennas_cmd, body_yaw=body_yaw)
-            except Exception as e:
-                logger.error(f"Failed to set robot target: {e}")
-            else:
-                with self._state_lock:
-                    self._last_commanded_pose = clone_full_body_pose((head, antennas_cmd, body_yaw))
-
-            # 9. Calculate computation time and adjust sleep for 50Hz
-            computation_time = time.time() - loop_start_time
-            sleep_time = max(0, self.target_period - computation_time)
-
-            # 10. Print frequency info every 100 loops (~2 seconds)
-            if loop_count % 100 == 0:
-                elapsed = current_time - last_print_time
-                actual_freq = 100.0 / elapsed if elapsed > 0 else 0
-                potential_freq = 1.0 / computation_time if computation_time > 0 else float("inf")
-                logger.debug(
-                    f"Loop freq - Actual: {actual_freq:.1f}Hz, Potential: {potential_freq:.1f}Hz, Target: {self.target_frequency:.1f}Hz"
-                )
-                last_print_time = current_time
+            # 8) Periodic telemetry on loop frequency
+            self._maybe_log_frequency(loop_count, print_interval_loops, freq_stats)
 
-            time.sleep(sleep_time)
+            if sleep_time > 0:
+                time.sleep(sleep_time)
 
         logger.debug("Movement control loop stopped")

tests/audio/test_head_wobbler.py

@@ -0,0 +1,109 @@
+"""Regression tests for the audio-driven head wobble behaviour."""
+
+import math
+import time
+import base64
+import threading
+from typing import List, Tuple, Callable
+
+import numpy as np
+
+from reachy_mini_conversation_demo.audio.head_wobbler import HeadWobbler
+
+
+def _make_audio_chunk(duration_s: float = 0.3, frequency_hz: float = 220.0) -> str:
+    """Generate a base64-encoded mono PCM16 sine wave."""
+    sample_rate = 24000
+    sample_count = int(sample_rate * duration_s)
+    t = np.linspace(0, duration_s, sample_count, endpoint=False)
+    wave = 0.6 * np.sin(2 * math.pi * frequency_hz * t)
+    pcm = np.clip(wave * np.iinfo(np.int16).max, -32768, 32767).astype(np.int16)
+    return base64.b64encode(pcm.tobytes()).decode("ascii")
+
+
+def _wait_for(predicate: Callable[[], bool], timeout: float = 0.6) -> bool:
+    """Poll `predicate` until true or timeout."""
+    end_time = time.time() + timeout
+    while time.time() < end_time:
+        if predicate():
+            return True
+        time.sleep(0.01)
+    return False
+
+
+def _start_wobbler() -> Tuple[HeadWobbler, List[Tuple[float, Tuple[float, float, float, float, float, float]]]]:
+    captured: List[Tuple[float, Tuple[float, float, float, float, float, float]]] = []
+
+    def capture(offsets: Tuple[float, float, float, float, float, float]) -> None:
+        captured.append((time.time(), offsets))
+
+    wobbler = HeadWobbler(set_speech_offsets=capture)
+    wobbler.start()
+    return wobbler, captured
+
+
+def test_reset_drops_pending_offsets() -> None:
+    """Reset should stop wobble output derived from pre-reset audio."""
+    wobbler, captured = _start_wobbler()
+    try:
+        wobbler.feed(_make_audio_chunk(duration_s=0.35))
+        assert _wait_for(lambda: len(captured) > 0), "wobbler did not emit initial offsets"
+
+        pre_reset_count = len(captured)
+        wobbler.reset()
+        time.sleep(0.3)
+        assert len(captured) == pre_reset_count, "offsets continued after reset without new audio"
+    finally:
+        wobbler.stop()
+
+
+def test_reset_allows_future_offsets() -> None:
+    """After reset, fresh audio must still produce wobble offsets."""
+    wobbler, captured = _start_wobbler()
+    try:
+        wobbler.feed(_make_audio_chunk(duration_s=0.35))
+        assert _wait_for(lambda: len(captured) > 0), "wobbler did not emit initial offsets"
+
+        wobbler.reset()
+        pre_second_count = len(captured)
+
+        wobbler.feed(_make_audio_chunk(duration_s=0.35, frequency_hz=440.0))
+        assert _wait_for(lambda: len(captured) > pre_second_count), "no offsets after reset"
+        assert wobbler._thread is not None and wobbler._thread.is_alive()
+    finally:
+        wobbler.stop()
+
+
+def test_reset_during_inflight_chunk_keeps_worker(monkeypatch) -> None:
+    """Simulate reset during chunk processing to ensure the worker survives."""
+    wobbler, captured = _start_wobbler()
+    ready = threading.Event()
+    release = threading.Event()
+
+    original_feed = wobbler.sway.feed
+
+    def blocking_feed(pcm, sr):  # type: ignore[no-untyped-def]
+        ready.set()
+        release.wait(timeout=2.0)
+        return original_feed(pcm, sr)
+
+    monkeypatch.setattr(wobbler.sway, "feed", blocking_feed)
+
+    try:
+        wobbler.feed(_make_audio_chunk(duration_s=0.35))
+        assert ready.wait(timeout=1.0), "worker thread did not dequeue audio"
+
+        wobbler.reset()
+        release.set()
+
+        # Allow the worker to finish processing the first chunk (which should be discarded)
+        time.sleep(0.1)
+
+        assert wobbler._thread is not None and wobbler._thread.is_alive(), "worker thread died after reset"
+
+        pre_second = len(captured)
+        wobbler.feed(_make_audio_chunk(duration_s=0.35, frequency_hz=440.0))
+        assert _wait_for(lambda: len(captured) > pre_second), "no offsets emitted after in-flight reset"
+        assert wobbler._thread.is_alive()
+    finally:
+        wobbler.stop()

tests/conftest.py

@@ -0,0 +1,10 @@
+"""Pytest configuration for path setup."""
+
+import sys
+from pathlib import Path
+
+
+PROJECT_ROOT = Path(__file__).resolve().parents[1]
+SRC_PATH = PROJECT_ROOT / "src"
+if str(SRC_PATH) not in sys.path:
+    sys.path.insert(0, str(SRC_PATH))