reachy-mini 1.2.5rc1__py3-none-any.whl → 1.2.11__py3-none-any.whl

reachy_mini/daemon/app/routers/state.py

@@ -13,7 +13,7 @@ from fastapi import APIRouter, Depends, WebSocket, WebSocketDisconnect
 
 from ....daemon.backend.abstract import Backend
 from ..dependencies import get_backend, ws_get_backend
-from ..models import AnyPose, FullState, as_any_pose
+from ..models import AnyPose, DoAInfo, FullState, as_any_pose
 
 router = APIRouter(prefix="/state")
 
@@ -54,6 +54,23 @@ async def get_antenna_joint_positions(
     return (pos[0], pos[1])
 
 
+@router.get("/doa")
+async def get_doa(
+    backend: Backend = Depends(get_backend),
+) -> DoAInfo | None:
+    """Get the Direction of Arrival from the microphone array.
+
+    Returns the angle in radians (0=left, π/2=front, π=right) and speech detection status.
+    Returns None if the audio device is not available.
+    """
+    if not backend.audio:
+        return None
+    result = backend.audio.get_DoA()
+    if result is None:
+        return None
+    return DoAInfo(angle=result[0], speech_detected=result[1])
+
+
 @router.get("/full")
 async def get_full_state(
     with_control_mode: bool = True,
@@ -66,6 +83,7 @@ async def get_full_state(
     with_antenna_positions: bool = True,
     with_target_antenna_positions: bool = False,
     with_passive_joints: bool = False,
+    with_doa: bool = False,
     use_pose_matrix: bool = False,
     backend: Backend = Depends(get_backend),
 ) -> FullState:
@@ -100,6 +118,10 @@ async def get_full_state(
             result["passive_joints"] = list(joints.values())
         else:
             result["passive_joints"] = None
+    if with_doa and backend.audio:
+        doa_result = backend.audio.get_DoA()
+        if doa_result:
+            result["doa"] = DoAInfo(angle=doa_result[0], speech_detected=doa_result[1])
 
     result["timestamp"] = datetime.now(timezone.utc)
     return FullState.model_validate(result)
@@ -118,6 +140,7 @@ async def ws_full_state(
     with_antenna_positions: bool = True,
     with_target_antenna_positions: bool = False,
     with_passive_joints: bool = False,
+    with_doa: bool = False,
     use_pose_matrix: bool = False,
     backend: Backend = Depends(ws_get_backend),
 ) -> None:
@@ -137,6 +160,7 @@ async def ws_full_state(
                 with_antenna_positions=with_antenna_positions,
                 with_target_antenna_positions=with_target_antenna_positions,
                 with_passive_joints=with_passive_joints,
+                with_doa=with_doa,
                 use_pose_matrix=use_pose_matrix,
                 backend=backend,
             )

reachy_mini/daemon/app/routers/wifi_config.py

@@ -149,6 +149,81 @@ def scan_wifi() -> list[str]:
     return ssids
 
 
+@router.post("/forget")
+def forget_wifi_network(ssid: str) -> None:
+    """Forget a saved WiFi network. Falls back to Hotspot if forgetting the active network."""
+    if ssid == "Hotspot":
+        raise HTTPException(status_code=400, detail="Cannot forget Hotspot connection.")
+
+    if not check_if_connection_exists(ssid):
+        raise HTTPException(
+            status_code=404, detail=f"Network '{ssid}' not found in saved networks."
+        )
+
+    if busy_lock.locked():
+        raise HTTPException(status_code=409, detail="Another operation is in progress.")
+
+    def forget() -> None:
+        global error
+        with busy_lock:
+            try:
+                error = None
+                was_active = check_if_connection_active(ssid)
+                logger.info(f"Forgetting WiFi network '{ssid}'...")
+                remove_connection(ssid)
+
+                if was_active:
+                    logger.info("Was connected, falling back to hotspot...")
+                    setup_wifi_connection(
+                        name="Hotspot",
+                        ssid=HOTSPOT_SSID,
+                        password=HOTSPOT_PASSWORD,
+                        is_hotspot=True,
+                    )
+            except Exception as e:
+                error = e
+                logger.error(f"Failed to forget network '{ssid}': {e}")
+
+    Thread(target=forget).start()
+
+
+@router.post("/forget_all")
+def forget_all_wifi_networks() -> None:
+    """Forget all saved WiFi networks (except Hotspot). Falls back to Hotspot."""
+    if busy_lock.locked():
+        raise HTTPException(status_code=409, detail="Another operation is in progress.")
+
+    def forget_all() -> None:
+        global error
+        with busy_lock:
+            try:
+                error = None
+                connections = get_wifi_connections()
+                forgotten = []
+
+                for conn in connections:
+                    if conn.name != "Hotspot":
+                        remove_connection(conn.name)
+                        forgotten.append(conn.name)
+
+                logger.info(f"Forgotten {len(forgotten)} networks: {forgotten}")
+
+                # Always ensure we have connectivity after forgetting all
+                if get_current_wifi_mode() == WifiMode.DISCONNECTED:
+                    logger.info("No connection left, setting up hotspot...")
+                    setup_wifi_connection(
+                        name="Hotspot",
+                        ssid=HOTSPOT_SSID,
+                        password=HOTSPOT_PASSWORD,
+                        is_hotspot=True,
+                    )
+            except Exception as e:
+                error = e
+                logger.error(f"Failed to forget networks: {e}")
+
+    Thread(target=forget_all).start()
+
+
 # NMCLI WRAPPERS
 def scan_available_wifi() -> list[nmcli.data.device.DeviceWifi]:
     """Scan for available WiFi networks."""

reachy_mini/daemon/app/services/bluetooth/bluetooth_service.py

@@ -278,7 +278,7 @@ class CommandCharacteristic(Characteristic):
 
 
 class ResponseCharacteristic(Characteristic):
-    """Response Characteristic.""" ""
+    """Response Characteristic."""
 
     def __init__(self, bus, index, service):
         """Initialize the Response Characteristic."""

reachy_mini/daemon/app/services/bluetooth/commands/WIFI_RESET.sh

@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+
+nmcli --escape yes -t -f NAME,TYPE connection show | grep ':802-11-wireless$' | while IFS= read -r line; do
+  conn="${line%:802-11-wireless}"
+  conn="${conn//\\:/\:}"
+  [ "$conn" != "Hotspot" ] && nmcli connection delete "$conn" 2>/dev/null
+done
+systemctl restart reachy-mini-daemon.service

reachy_mini/daemon/app/services/wireless/launcher.sh

@@ -1,5 +1,11 @@
 #!/bin/bash
-/venvs/src/reachy_mini/src/reachy_mini/daemon/app/services/wireless/generate_asoundrc.sh
+# Get the directory where this script is located
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+"$SCRIPT_DIR/generate_asoundrc.sh"
 source /venvs/mini_daemon/bin/activate
 export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:/opt/gst-plugins-rs/lib/aarch64-linux-gnu/
-python -m reachy_mini.daemon.app.main --wireless-version --no-autostart
+export PATH=$PATH:/opt/uv
+
+# Run Python in unbuffered mode (-u) to ensure logs are immediately forwarded to systemd
+python -u -m reachy_mini.daemon.app.main --wireless-version --no-autostart

reachy_mini/daemon/app/services/wireless/reachy-mini-daemon.service

@@ -0,0 +1,13 @@
+[Unit]
+Description=Reachy Mini AP Launcher Service
+After=network.target
+
+[Service]
+Type=simple
+ExecStart=/venvs/mini_daemon/lib/python3.12/site-packages/reachy_mini/daemon/app/services/wireless/launcher.sh
+Restart=on-failure
+User=pollen
+WorkingDirectory=/venvs/mini_daemon/lib/python3.12/site-packages/reachy_mini/daemon/app/services/wireless
+
+[Install]
+WantedBy=multi-user.target

reachy_mini/daemon/backend/abstract.py

@@ -25,6 +25,7 @@ from numpy.typing import NDArray
 from scipy.spatial.transform import Rotation as R
 
 if typing.TYPE_CHECKING:
+    from reachy_mini.daemon.backend.mockup_sim.backend import MockupSimBackendStatus
     from reachy_mini.daemon.backend.mujoco.backend import MujocoBackendStatus
     from reachy_mini.daemon.backend.robot.backend import RobotBackendStatus
     from reachy_mini.kinematics import AnyKinematics
@@ -131,6 +132,7 @@ class Backend:
         self.joint_positions_publisher: zenoh.Publisher | None = None
         self.pose_publisher: zenoh.Publisher | None = None
         self.recording_publisher: zenoh.Publisher | None = None
+        self.imu_publisher: zenoh.Publisher | None = None
         self.error: str | None = None  # To store any error that occurs during execution
         self.is_recording = False  # Flag to indicate if recording is active
         self.recorded_data: list[dict[str, Any]] = []  # List to store recorded data
@@ -182,7 +184,9 @@ class Backend:
 
         # Guard to ensure only one play_move/goto is executed at a time (goto itself uses play_move, so we need an RLock)
         self._play_move_lock = threading.RLock()
-        self._active_move_depth = 0  # Tracks nested acquisitions within the owning thread
+        self._active_move_depth = (
+            0  # Tracks nested acquisitions within the owning thread
+        )
 
     # Life cycle methods
     def wrapped_run(self) -> None:
@@ -202,13 +206,18 @@ class Backend:
         raise NotImplementedError("The method run should be overridden by subclasses.")
 
     def close(self) -> None:
-        """Close the backend.
+        """Close the backend and release resources.
 
-        This method is a placeholder and should be overridden by subclasses.
+        Subclasses should override this method to add their own cleanup logic,
+        and call super().close() at the end to ensure audio resources are released.
+
+        Note: This base implementation handles common cleanup (audio).
+        Subclasses must still implement their own cleanup for backend-specific resources.
         """
-        raise NotImplementedError(
-            "The method close should be overridden by subclasses."
-        )
+        self.logger.debug("Backend.close() - cleaning up audio resources")
+        if self.audio is not None:
+            self.audio.close()
+            self.audio = None
 
     @property
     def is_move_running(self) -> bool:
@@ -228,7 +237,9 @@ class Backend:
             self._active_move_depth -= 1
         self._play_move_lock.release()
 
-    def get_status(self) -> "RobotBackendStatus | MujocoBackendStatus":
+    def get_status(
+        self,
+    ) -> "RobotBackendStatus | MujocoBackendStatus | MockupSimBackendStatus":
         """Return backend statistics.
 
         This method is a placeholder and should be overridden by subclasses.
@@ -256,6 +267,15 @@ class Backend:
         """
         self.pose_publisher = publisher
 
+    def set_imu_publisher(self, publisher: zenoh.Publisher) -> None:
+        """Set the publisher for IMU data.
+
+        Args:
+            publisher: A publisher object that will be used to publish IMU data.
+
+        """
+        self.imu_publisher = publisher
+
     def update_target_head_joints_from_ik(
         self,
         pose: Annotated[NDArray[np.float64], (4, 4)] | None = None,
@@ -388,8 +408,8 @@ class Backend:
 
         try:
             if initial_goto_duration > 0.0:
-                start_head_pose, start_antennas_positions, start_body_yaw = move.evaluate(
-                    0.0
+                start_head_pose, start_antennas_positions, start_body_yaw = (
+                    move.evaluate(0.0)
                 )
                 await self.goto_target(
                     head=start_head_pose,

reachy_mini/daemon/backend/mockup_sim/__init__.py

@@ -0,0 +1,12 @@
+"""Mockup Simulation Backend for Reachy Mini Daemon.
+
+A lightweight simulation backend that doesn't require MuJoCo.
+Uses only kinematics (no physics simulation).
+"""
+
+from reachy_mini.daemon.backend.mockup_sim.backend import (
+    MockupSimBackend,
+    MockupSimBackendStatus,
+)
+
+__all__ = ["MockupSimBackend", "MockupSimBackendStatus"]

reachy_mini/daemon/backend/mockup_sim/backend.py

@@ -0,0 +1,176 @@
+"""Mockup Simulation Backend for Reachy Mini.
+
+A lightweight simulation backend that doesn't require MuJoCo.
+Target positions become current positions immediately (no physics).
+The kinematics engine is still used for FK/IK computations.
+
+Apps open the webcam/microphone directly (like with a real robot).
+"""
+
+import json
+import time
+from dataclasses import dataclass
+from typing import Annotated
+
+import numpy as np
+import numpy.typing as npt
+
+from ..abstract import Backend, MotorControlMode
+
+
+class MockupSimBackend(Backend):
+    """Lightweight simulated Reachy Mini without MuJoCo.
+
+    This backend provides a simple simulation where target positions
+    are applied immediately without physics simulation.
+
+    Apps access webcam/microphone directly (not via UDP streaming).
+    """
+
+    def __init__(
+        self,
+        check_collision: bool = False,
+        kinematics_engine: str = "AnalyticalKinematics",
+        use_audio: bool = True,
+    ) -> None:
+        """Initialize the MockupSimBackend.
+
+        Args:
+            check_collision: If True, enable collision checking. Default is False.
+            kinematics_engine: Kinematics engine to use. Defaults to "AnalyticalKinematics".
+            use_audio: If True, use audio. Default is True.
+
+        """
+        super().__init__(
+            check_collision=check_collision,
+            kinematics_engine=kinematics_engine,
+            use_audio=use_audio,
+        )
+
+        from reachy_mini.reachy_mini import (
+            SLEEP_ANTENNAS_JOINT_POSITIONS,
+            SLEEP_HEAD_JOINT_POSITIONS,
+        )
+
+        # Initialize with sleep positions
+        self._head_joint_positions: npt.NDArray[np.float64] = np.array(
+            SLEEP_HEAD_JOINT_POSITIONS, dtype=np.float64
+        )
+        self._antenna_joint_positions: npt.NDArray[np.float64] = np.array(
+            SLEEP_ANTENNAS_JOINT_POSITIONS, dtype=np.float64
+        )
+
+        self._motor_control_mode = MotorControlMode.Enabled
+
+        # Control loop frequency
+        self.control_frequency = 50.0  # Hz
+
+    def run(self) -> None:
+        """Run the simulation loop.
+
+        In mockup-sim mode, target positions are applied immediately.
+        """
+        control_period = 1.0 / self.control_frequency
+
+        # Initialize kinematics with current positions
+        self.update_head_kinematics_model(
+            self._head_joint_positions,
+            self._antenna_joint_positions,
+        )
+
+        while not self.should_stop.is_set():
+            start_t = time.time()
+
+            # Apply target positions immediately (no physics)
+            if self.target_head_joint_positions is not None:
+                self._head_joint_positions = self.target_head_joint_positions.copy()
+            if self.target_antenna_joint_positions is not None:
+                self._antenna_joint_positions = (
+                    self.target_antenna_joint_positions.copy()
+                )
+
+            # Update current states
+            self.current_head_joint_positions = self._head_joint_positions.copy()
+            self.current_antenna_joint_positions = self._antenna_joint_positions.copy()
+
+            # Update kinematics model (computes FK)
+            self.update_head_kinematics_model(
+                self.current_head_joint_positions,
+                self.current_antenna_joint_positions,
+            )
+
+            # Update target head joint positions from IK if necessary
+            if self.ik_required:
+                try:
+                    self.update_target_head_joints_from_ik(
+                        self.target_head_pose, self.target_body_yaw
+                    )
+                except ValueError:
+                    pass  # IK failed, keep current positions
+
+            # Publish joint positions via Zenoh
+            if (
+                self.joint_positions_publisher is not None
+                and self.pose_publisher is not None
+                and not self.is_shutting_down
+            ):
+                self.joint_positions_publisher.put(
+                    json.dumps(
+                        {
+                            "head_joint_positions": self.current_head_joint_positions.tolist(),
+                            "antennas_joint_positions": self.current_antenna_joint_positions.tolist(),
+                        }
+                    ).encode("utf-8")
+                )
+                self.pose_publisher.put(
+                    json.dumps(
+                        {
+                            "head_pose": self.get_present_head_pose().tolist(),
+                        }
+                    ).encode("utf-8")
+                )
+
+            self.ready.set()
+
+            # Sleep to maintain control frequency
+            elapsed = time.time() - start_t
+            time.sleep(max(0, control_period - elapsed))
+
+    def get_status(self) -> "MockupSimBackendStatus":
+        """Get the status of the backend."""
+        return MockupSimBackendStatus(motor_control_mode=self._motor_control_mode)
+
+    def get_present_head_joint_positions(
+        self,
+    ) -> Annotated[npt.NDArray[np.float64], (7,)]:
+        """Get the current joint positions of the head."""
+        return self._head_joint_positions.copy()
+
+    def get_present_antenna_joint_positions(
+        self,
+    ) -> Annotated[npt.NDArray[np.float64], (2,)]:
+        """Get the current joint positions of the antennas."""
+        return self._antenna_joint_positions.copy()
+
+    def get_motor_control_mode(self) -> MotorControlMode:
+        """Get the motor control mode."""
+        return self._motor_control_mode
+
+    def set_motor_control_mode(self, mode: MotorControlMode) -> None:
+        """Set the motor control mode."""
+        self._motor_control_mode = mode
+
+    def set_motor_torque_ids(self, ids: list[str], on: bool) -> None:
+        """Set the motor torque state for specific motor names.
+
+        No-op in mockup-sim mode.
+        """
+        pass
+
+
+@dataclass
+class MockupSimBackendStatus:
+    """Status of the MockupSim backend."""
+
+    motor_control_mode: MotorControlMode
+    error: str | None = None

reachy_mini/daemon/backend/mujoco/backend.py

@@ -339,11 +339,6 @@ class MujocoBackend(Backend):
         mj_current_head_pose[2, 3] -= 0.177
         return mj_current_head_pose
 
-    def close(self) -> None:
-        """Close the Mujoco backend."""
-        # TODO Do something in mujoco here ?
-        pass
-
     def get_status(self) -> "MujocoBackendStatus":
         """Get the status of the Mujoco backend.
 

reachy_mini/daemon/backend/robot/backend.py

@@ -17,6 +17,7 @@ from typing import Annotated, Any
 import log_throttling
 import numpy as np
 import numpy.typing as npt
+import zenoh
 from reachy_mini_motor_controller import ReachyMiniPyControlLoop
 
 from reachy_mini.utils.hardware_config.parser import parse_yaml_config
@@ -70,7 +71,7 @@ class RobotBackend(Backend):
                 seconds=1.0 / self.control_loop_frequency
             ),
             allowed_retries=5,
-            stats_pub_period=None,
+            stats_pub_period=timedelta(seconds=1.0),
         )
 
         self.name2id = self.c.get_motor_name_id()
@@ -108,7 +109,29 @@ class RobotBackend(Backend):
         self.target_antenna_joint_current = None  # Placeholder for antenna joint torque
         self.target_head_joint_current = None  # Placeholder for head joint torque
 
-        self.hardware_error_check_frequency = hardware_error_check_frequency  # seconds
+        if hardware_error_check_frequency <= 0:
+            raise ValueError(
+                "hardware_error_check_frequency must be positive and non-zero (Hz)."
+            )
+
+        self.hardware_error_check_period = (
+            1.0 / hardware_error_check_frequency
+        )  # seconds
+
+        # Initialize IMU for wireless version
+        if wireless_version:
+            try:
+                from bmi088 import BMI088
+
+                self.bmi088 = BMI088(i2c_bus=4)
+                self.logger.info("BMI088 IMU initialized successfully")
+            except Exception as e:
+                self.logger.warning(f"Failed to initialize IMU: {e}")
+                self.bmi088 = None
+        else:
+            self.bmi088 = None
+
+        self.imu_publisher: zenoh.Publisher | None = None
 
     def run(self) -> None:
         """Run the control loop for the robot backend.
@@ -234,6 +257,12 @@ class RobotBackend(Backend):
                         )
                     )
 
+                    # Publish IMU data if available
+                    if self.imu_publisher is not None and self.bmi088 is not None:
+                        imu_data = self.get_imu_data()
+                        if imu_data is not None:
+                            self.imu_publisher.put(json.dumps(imu_data))
+
                 self.last_alive = time.time()
 
                 self.ready.set()  # Mark the backend as ready
@@ -264,6 +293,9 @@ class RobotBackend(Backend):
                     self._status.control_loop_stats["nb_error"] = self._stats[
                         "nb_error"
                     ]
+                    self._status.control_loop_stats["motor_controller"] = str(
+                        self.c.get_stats()
+                    )
 
                 self._stats["timestamps"].clear()
                 self._stats["nb_error"] = 0
@@ -271,7 +303,7 @@ class RobotBackend(Backend):
 
             if (
                 time.time() - self.last_hardware_error_check_time
-                > self.hardware_error_check_frequency
+                > self.hardware_error_check_period
             ):
                 hardware_errors = self.read_hardware_errors()
                 if hardware_errors:
@@ -282,10 +314,11 @@ class RobotBackend(Backend):
                 self.last_hardware_error_check_time = time.time()
 
     def close(self) -> None:
-        """Close the motor controller connection."""
+        """Close the motor controller connection and release resources."""
         if self.c is not None:
             self.c.close()
         self.c = None
+        super().close()
 
     def get_status(self) -> "RobotBackendStatus":
         """Get the current status of the robot backend."""
@@ -378,7 +411,11 @@ class RobotBackend(Backend):
 
         """
         assert self.c is not None, "Motor controller not initialized or already closed."
-        assert mode in [0, 3, 5], (
+        assert mode in [
+            0,
+            3,
+            5,
+        ], (
             "Invalid operation mode. Must be one of [0 (torque), 3 (position), 5 (current-limiting position)]."
         )
 
@@ -437,6 +474,42 @@ class RobotBackend(Backend):
         """
         return np.array(self.get_all_joint_positions()[1])
 
+    def get_imu_data(self) -> dict[str, list[float] | float] | None:
+        """Get current IMU data (accelerometer, gyroscope, quaternion, temperature).
+
+        Returns:
+            dict with 'accelerometer', 'gyroscope', 'quaternion', and 'temperature' keys,
+            or None if IMU is not available.
+
+        """
+        if self.bmi088 is None:
+            return None
+
+        try:
+            # Read accelerometer (returns tuple of x, y, z in m/s^2)
+            accel_x, accel_y, accel_z = self.bmi088.read_accelerometer(m_per_s2=True)
+
+            # Read gyroscope (returns tuple of x, y, z in rad/s)
+            gyro_x, gyro_y, gyro_z = self.bmi088.read_gyroscope(deg_per_s=False)
+
+            # Get quaternion orientation (dt = control loop period)
+            dt = 1.0 / self.control_loop_frequency  # 0.02 seconds at 50Hz
+            quat = self.bmi088.get_quat(dt)
+
+            # Read temperature in Celsius
+            temperature = self.bmi088.read_temperature()
+
+            # Convert all numpy types to native Python floats for JSON serialization
+            return {
+                "accelerometer": [float(accel_x), float(accel_y), float(accel_z)],
+                "gyroscope": [float(gyro_x), float(gyro_y), float(gyro_z)],
+                "quaternion": [float(q) for q in quat],
+                "temperature": float(temperature),
+            }
+        except Exception as e:
+            self.logger.error(f"Error reading IMU data: {e}")
+            return None
+
     def compensate_head_gravity(self) -> None:
         """Calculate the currents necessary to compensate for gravity."""
         assert self.kinematics_engine == "Placo", (