reachy-mini 1.2.5rc1__py3-none-any.whl

reachy_mini/utils/rerun.py

@@ -0,0 +1,320 @@
+"""Rerun logging for Reachy Mini.
+
+This module provides functionality to log the state of the Reachy Mini robot to Rerun,
+ a tool for visualizing and debugging robotic systems.
+
+It includes methods to log joint positions, camera images, and other relevant data.
+"""
+
+import json
+import logging
+import os
+import tempfile
+import time
+from threading import Event, Thread
+from typing import Dict, List, Optional
+
+import numpy as np
+import requests
+import rerun as rr
+from scipy.spatial.transform import Rotation as R
+from urdf_parser_py import urdf
+
+from reachy_mini.kinematics.placo_kinematics import PlacoKinematics
+from reachy_mini.media.media_manager import MediaBackend
+from reachy_mini.reachy_mini import ReachyMini
+
+
+class Rerun:
+    """Rerun logging for Reachy Mini."""
+
+    def __init__(
+        self,
+        reachymini: ReachyMini,
+        app_id: str = "reachy_mini_rerun",
+        spawn: bool = True,
+    ):
+        """Initialize the Rerun logging for Reachy Mini.
+
+        Args:
+            reachymini (ReachyMini): The Reachy Mini instance to log.
+            app_id (str): The application ID for Rerun. Defaults to reachy_mini_daemon.
+            spawn (bool): If True, spawn the Rerun server. Defaults to True.
+
+        """
+        rr.init(app_id, spawn=spawn)
+        self.app_id = app_id
+        self._reachymini = reachymini
+        self.logger = logging.getLogger(__name__)
+        self.logger.setLevel(reachymini.logger.getEffectiveLevel())
+
+        self._robot_ip = "localhost"
+        if self._reachymini.client.get_status()["wireless_version"]:
+            self._robot_ip = self._reachymini.client.get_status()["wlan_ip"]
+
+        self.recording = rr.get_global_data_recording()
+
+        script_dir = os.path.dirname(os.path.abspath(__file__))
+
+        urdf_path = os.path.join(
+            script_dir, "../descriptions/reachy_mini/urdf/robot_no_collision.urdf"
+        )
+        asset_path = os.path.join(script_dir, "../descriptions/reachy_mini/urdf")
+
+        fixed_urdf = self.set_absolute_path_to_urdf(urdf_path, asset_path)
+        self.logger.debug(
+            f"Using URDF file: {fixed_urdf} with absolute paths for Rerun."
+        )
+
+        self.head_kinematics = PlacoKinematics(fixed_urdf)
+
+        self.urdf_model = urdf.URDF.from_xml_file(fixed_urdf)
+        self._joints_by_name: Dict[str, urdf.Joint] = {
+            joint.name: joint for joint in self.urdf_model.joints
+        }
+        self._entity_paths = UrdfEntityPaths(self.urdf_model, "ReachyMini")
+
+        rr.set_time("reachymini", timestamp=time.time(), recording=self.recording)
+
+        # Use the native URDF loader in Rerun to visualize Reachy Mini's model
+        # Logging as non-static to allow updating joint positions
+        rr.log_file_from_path(
+            fixed_urdf,
+            static=False,
+            entity_path_prefix=self._entity_paths.prefix,
+            recording=self.recording,
+        )
+
+        self.running = Event()
+        self.thread_log_camera: Optional[Thread] = None
+        if (
+            reachymini.media.backend == MediaBackend.GSTREAMER
+            or reachymini.media.backend == MediaBackend.DEFAULT
+        ):
+            self.thread_log_camera = Thread(target=self.log_camera, daemon=True)
+        self.thread_log_movements = Thread(target=self.log_movements, daemon=True)
+
+    def set_absolute_path_to_urdf(self, urdf_path: str, abs_path: str) -> str:
+        """Set the absolute paths in the URDF file. Rerun cannot read the "package://" paths."""
+        with open(urdf_path, "r") as f:
+            urdf_content = f.read()
+        urdf_content_mod = urdf_content.replace("package://", f"file://{abs_path}/")
+
+        with tempfile.NamedTemporaryFile("w", delete=False, suffix=".urdf") as tmp_file:
+            tmp_file.write(urdf_content_mod)
+            return tmp_file.name
+
+    def start(self) -> None:
+        """Start the Rerun logging thread."""
+        if self.thread_log_camera is not None:
+            self.thread_log_camera.start()
+        self.thread_log_movements.start()
+
+    def stop(self) -> None:
+        """Stop the Rerun logging thread."""
+        self.running.set()
+
+    def _get_joint(self, joint_name: str) -> urdf.Joint:
+        try:
+            return self._joints_by_name[joint_name]
+        except KeyError as exc:
+            raise RuntimeError(f"Invalid joint name: {joint_name}") from exc
+
+    def _joint_entity_path(self, joint: urdf.Joint) -> str:
+        return self._entity_paths.joint_paths[joint.name]
+
+    def log_camera(self) -> None:
+        """Log the camera image to Rerun."""
+        if self._reachymini.media.camera is None:
+            self.logger.warning("Camera is not initialized.")
+            return
+
+        self.logger.info("Starting camera logging to Rerun.")
+
+        while not self.running.is_set():
+            rr.set_time("reachymini", timestamp=time.time(), recording=self.recording)
+            frame = self._reachymini.media.get_frame()
+            if frame is not None:
+                if isinstance(frame, bytes):
+                    self.logger.warning(
+                        "Received frame is jpeg. Please use default backend."
+                    )
+                    return
+
+            else:
+                return
+
+            K = np.array(
+                [
+                    [550.3564, 0.0, 638.0112],
+                    [0.0, 549.1653, 364.589],
+                    [0.0, 0.0, 1.0],
+                ]
+            )
+
+            cam_joint = self._get_joint("camera_optical_frame")
+            cam_path = self._joint_entity_path(cam_joint)
+            rr.log(
+                f"{cam_path}/image",
+                rr.Pinhole(
+                    image_from_camera=rr.datatypes.Mat3x3(K),
+                    width=frame.shape[1],
+                    height=frame.shape[0],
+                    image_plane_distance=0.8,
+                    camera_xyz=rr.ViewCoordinates.RDF,
+                ),
+                rr.Image(frame, color_model="bgr").compress(),
+                recording=self.recording,
+            )
+
+            time.sleep(0.03)  # ~30fps
+
+    def _log_joint_angle(
+        self,
+        joint_name: str,
+        angle: float,
+        axis: list[float] | None = None,
+    ) -> None:
+        """Log the joint angle to Rerun."""
+        joint = self._get_joint(joint_name)
+        joint_path = self._joint_entity_path(joint)
+
+        base_euler = joint.origin.rotation or [0.0, 0.0, 0.0]
+
+        # if we specify an axis override, use it; otherwise, use the joint's defined axis
+        effective_axis = (
+            np.array(axis)
+            if axis is not None
+            else np.array(joint.axis or [1.0, 0.0, 0.0])
+        )
+
+        target_euler = np.array(base_euler) + (effective_axis * angle)
+        target_translation = joint.origin.xyz or [0.0, 0.0, 0.0]
+
+        rr.log(
+            joint_path,
+            rr.Transform3D(
+                translation=target_translation,
+                quaternion=R.from_euler("xyz", target_euler).as_quat(),
+            ),
+            recording=self.recording,
+        )
+
+    def log_movements(self) -> None:
+        """Log the movement data to Rerun."""
+        url = f"http://{self._robot_ip}:8000/api/state/full"
+
+        params = {
+            "with_control_mode": "false",
+            "with_head_pose": "false",
+            "with_target_head_pose": "false",
+            "with_head_joints": "true",
+            "with_target_head_joints": "false",
+            "with_body_yaw": "false",  # already in head_joints
+            "with_target_body_yaw": "false",
+            "with_antenna_positions": "true",
+            "with_target_antenna_positions": "false",
+            "use_pose_matrix": "false",
+            "with_passive_joints": "true",
+        }
+
+        while not self.running.is_set():
+            msg = requests.get(url, params=params)
+
+            if msg.status_code != 200:
+                self.logger.error(
+                    f"Request failed with status {msg.status_code}: {msg.text}"
+                )
+                time.sleep(0.1)
+                continue
+            try:
+                data = json.loads(msg.text)
+                self.logger.debug(f"Data: {data}")
+            except Exception:
+                continue
+
+            rr.set_time("reachymini", timestamp=time.time(), recording=self.recording)
+
+            if "antennas_position" in data and data["antennas_position"] is not None:
+                antennas = data["antennas_position"]
+                if antennas is not None:
+                    self._log_joint_angle("left_antenna", antennas[0])
+                    self._log_joint_angle("right_antenna", antennas[1])
+
+            if "head_joints" in data and data["head_joints"] is not None:
+                head_joints = data["head_joints"]
+
+                # The joint axis definitions in the URDF do not match the real axis of rotation
+                # due to URDF not supporting ball joints properly.
+                self._log_joint_angle("yaw_body", -head_joints[0], axis=[0, 0, 1])
+                self._log_joint_angle("stewart_1", -head_joints[1], axis=[0, 1, 0])
+                self._log_joint_angle("stewart_2", head_joints[2], axis=[0, 1, 0])
+                self._log_joint_angle("stewart_3", -head_joints[3], axis=[0, 1, 0])
+                self._log_joint_angle("stewart_4", head_joints[4], axis=[0, 1, 0])
+                self._log_joint_angle("stewart_5", -head_joints[5], axis=[0, 1, 0])
+                self._log_joint_angle("stewart_6", head_joints[6], axis=[0, 1, 0])
+
+            if "passive_joints" in data and data["passive_joints"] is not None:
+                passive_joints = data["passive_joints"]
+
+                for axis_idx, axis in enumerate(["x", "y", "z"]):
+                    for i in range(1, 8):
+                        joint_name = f"passive_{i}_{axis}"
+                        value_index = (i - 1) * 3 + axis_idx
+
+                        override_axis = [0.0, 0.0, 0.0]
+                        override_axis[axis_idx] = 1.0
+                        self._log_joint_angle(
+                            joint_name,
+                            passive_joints[value_index],
+                            axis=override_axis,
+                        )
+
+            time.sleep(0.1)
+
+
+class UrdfEntityPaths:
+    """Helper for constructing link/joint entity paths that match the native URDF logger."""
+
+    def __init__(self, model: urdf.URDF, entity_path_prefix: Optional[str]) -> None:
+        """Construct a new `UrdfEntityPaths` instance.
+
+        Args:
+            model (urdf.URDF): The URDF model.
+            entity_path_prefix (Optional[str]): The prefix for entity paths.
+
+        """
+        self._model = model
+        self.prefix = entity_path_prefix
+        self.link_paths: Dict[str, str] = {}
+        self.joint_paths: Dict[str, str] = {}
+
+        base_parts = [part for part in (self.prefix, model.name) if part]
+        self._base_path = "/".join(base_parts)
+
+        children_map: Dict[str, List[urdf.Joint]] = {}
+        for joint in model.joints:
+            children_map.setdefault(joint.parent, []).append(joint)
+
+        root_link = model.get_root()
+        self._build_paths(root_link, self._base_path, children_map)
+
+    def _build_paths(
+        self,
+        link_name: str,
+        parent_path: str,
+        children_map: Dict[str, List[urdf.Joint]],
+    ) -> None:
+        link_path = self._join(parent_path, link_name)
+        self.link_paths[link_name] = link_path
+
+        for joint in children_map.get(link_name, []):
+            joint_path = self._join(link_path, joint.name)
+            self.joint_paths[joint.name] = joint_path
+            self._build_paths(joint.child, joint_path, children_map)
+
+    @staticmethod
+    def _join(parent: str, child: str) -> str:
+        if parent:
+            return f"{parent}/{child}"
+        return child

reachy_mini/utils/wireless_version/__init__.py

@@ -0,0 +1 @@
+"""Utility functions for working with wireless version."""

reachy_mini/utils/wireless_version/startup_check.py

@@ -0,0 +1,144 @@
+"""Check and fix ownership of files under /venvs directory.
+
+This module ensures that all files under /venvs are owned by the pollen user.
+If any files are not owned by pollen, it will recursively change ownership.
+Also checks and updates the bluetooth service if needed.
+"""
+
+import filecmp
+import logging
+import pwd
+import subprocess
+from pathlib import Path
+
+logger = logging.getLogger(__name__)
+USER = "pollen"
+
+
+def check_and_fix_venvs_ownership(
+    venvs_path: str = "/venvs", custom_logger: logging.Logger | None = None
+) -> None:
+    """Check if files under venvs_path are owned by user pollen and fix if needed.
+
+    Args:
+        venvs_path: Path to the virtual environments directory (default: /venvs)
+        custom_logger: Optional logger to use instead of the module logger
+
+    """
+    try:
+        # Get pollen user's UID
+        pollen_uid = pwd.getpwnam(USER).pw_uid
+    except KeyError:
+        print(f"User '{USER}' does not exist on this system")
+        return
+
+    venvs_dir = Path(venvs_path)
+
+    if not venvs_dir.exists():
+        print(f"Directory {venvs_path} does not exist")
+        return
+
+    if not venvs_dir.is_dir():
+        print(f"{venvs_path} exists but is not a directory")
+        return
+
+    # Check if any files are not owned by pollen
+    needs_fix = False
+    try:
+        for item in venvs_dir.rglob("*"):
+            try:
+                if item.stat().st_uid != pollen_uid:
+                    needs_fix = True
+                    print(f"Found file not owned by {USER}: {item}")
+                    break
+            except (PermissionError, OSError) as e:
+                print(f"Cannot check ownership of {item}: {e}")
+    except (PermissionError, OSError) as e:
+        print(f"Cannot access {venvs_path}: {e}")
+        return
+
+    if needs_fix:
+        print(f"Fixing ownership of {venvs_path} to {USER}:{USER}")
+        try:
+            # Run chown with sudo to fix ownership
+            subprocess.run(
+                ["sudo", "chown", f"{USER}:{USER}", "-R", venvs_path],
+                check=True,
+                capture_output=True,
+                text=True,
+            )
+            print(f"Successfully fixed ownership of {venvs_path}")
+        except subprocess.CalledProcessError as e:
+            print(f"Failed to fix ownership: {e.stderr}")
+        except Exception as e:
+            print(f"Unexpected error while fixing ownership: {e}")
+    else:
+        print(f"All files under {venvs_path} are owned by {USER}")
+
+
+def check_and_update_bluetooth_service() -> None:
+    """Check if bluetooth service needs updating and update if different.
+
+    Compares the source bluetooth_service.py with the installed version at
+    /bluetooth/bluetooth_service.py. If they differ, copies the new version
+    and restarts the bluetooth service.
+    """
+    # This file: src/reachy_mini/utils/wireless_version/startup_check.py
+    # Target:    src/reachy_mini/daemon/app/services/bluetooth/bluetooth_service.py
+    # From parent: ../../daemon/app/services/bluetooth/bluetooth_service.py
+    source = Path(__file__).parent / ".." / ".." / "daemon" / "app" / "services" / "bluetooth" / "bluetooth_service.py"
+    source = source.resolve()
+    target = Path("/bluetooth/bluetooth_service.py")
+
+    if not source.exists():
+        print(f"Source bluetooth service not found at {source}")
+        return
+
+    # Check if target exists
+    if not target.exists():
+        print(f"Bluetooth service not installed at {target}, copying...")
+        needs_update = True
+    else:
+        # Compare files
+        try:
+            if filecmp.cmp(str(source), str(target), shallow=False):
+                print("Bluetooth service is up to date")
+                return
+            else:
+                print("Bluetooth service has changed, updating...")
+                needs_update = True
+        except Exception as e:
+            print(f"Error comparing bluetooth service files: {e}")
+            return
+
+    if needs_update:
+        try:
+            # Copy the new version using sudo
+            print(f"Copying {source} to {target}")
+            subprocess.run(
+                ["sudo", "cp", str(source), str(target)],
+                check=True,
+                capture_output=True,
+                text=True,
+            )
+            print("Successfully copied bluetooth service")
+
+            # Restart the bluetooth service
+            print("Restarting bluetooth service...")
+            subprocess.run(
+                ["sudo", "systemctl", "restart", "reachy-mini-bluetooth"],
+                check=True,
+                capture_output=True,
+                text=True,
+            )
+            print("Successfully restarted bluetooth service")
+        except subprocess.CalledProcessError as e:
+            print(f"Failed to update bluetooth service: {e.stderr}")
+        except Exception as e:
+            print(f"Unexpected error while updating bluetooth service: {e}")
+
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO)
+    check_and_fix_venvs_ownership()
+    check_and_update_bluetooth_service()

reachy_mini/utils/wireless_version/update.py

@@ -0,0 +1,20 @@
+"""Module to handle software updates for the Reachy Mini wireless."""
+
+import logging
+
+from .utils import call_logger_wrapper
+
+
+async def update_reachy_mini(pre_release: bool, logger: logging.Logger) -> None:
+    """Perform a software update by upgrading the reachy_mini package and restarting the daemon."""
+    extra_args = []
+    if pre_release:
+        extra_args.append("--pre")
+
+    await call_logger_wrapper(
+        ["pip", "install", "--upgrade", "reachy_mini[wireless-version]"] + extra_args,
+        logger,
+    )
+    await call_logger_wrapper(
+        ["sudo", "systemctl", "restart", "reachy-mini-daemon"], logger
+    )

reachy_mini/utils/wireless_version/update_available.py

@@ -0,0 +1,61 @@
+"""Check if an update is available for Reachy Mini Wireless.
+
+For now, this only checks if a new version of "reachy_mini" is available on PyPI.
+"""
+
+from importlib.metadata import version
+
+import requests
+import semver
+
+
+def is_update_available(package_name: str, pre_release: bool) -> bool:
+    """Check if an update is available for the given package."""
+    pypi_version = get_pypi_version(package_name, pre_release)
+    local_version = get_local_version(package_name)
+
+    is_update_available = pypi_version > local_version
+    assert isinstance(is_update_available, bool)
+
+    return is_update_available
+
+
+def get_pypi_version(package_name: str, pre_release: bool) -> semver.Version:
+    """Get the latest version of a package from PyPI."""
+    url = f"https://pypi.org/pypi/{package_name}/json"
+    response = requests.get(url)
+    response.raise_for_status()
+    data = response.json()
+
+    version = data["info"]["version"]
+
+    if pre_release:
+        releases = list(data["releases"].keys())
+        pre_version = _semver_version(releases[-1])
+        if pre_version > version:
+            return pre_version
+
+    return _semver_version(version)
+
+
+def get_local_version(package_name: str) -> semver.Version:
+    """Get the currently installed version of a package."""
+    return _semver_version(version(package_name))
+
+
+def _semver_version(v: str) -> semver.Version:
+    """Convert a version string to a semver.Version object, handling pypi pre-release formats."""
+    try:
+        return semver.Version.parse(v)
+    except ValueError:
+        version_parts = v.split(".")
+        if len(version_parts) < 3:
+            raise ValueError(f"Invalid version string: {v}")
+
+        patch_part = version_parts[2]
+        if "rc" in patch_part:
+            patch, rc = patch_part.split("rc", 1)
+            v_clean = f"{version_parts[0]}.{version_parts[1]}.{patch}-rc.{rc}"
+            return semver.Version.parse(v_clean)
+
+    raise ValueError(f"Invalid version string: {v}")

reachy_mini/utils/wireless_version/utils.py

@@ -0,0 +1,39 @@
+"""Utility functions for running shell commands asynchronously with real-time logging."""
+
+import asyncio
+import logging
+from typing import Callable
+
+
+async def call_logger_wrapper(command: list[str], logger: logging.Logger) -> None:
+    """Run a command asynchronously, streaming stdout and stderr to logger in real time.
+
+    Args:
+        command: list or tuple of command arguments (not a string)
+        logger: logger object with .info and .error methods
+
+    """
+    process = await asyncio.create_subprocess_exec(
+        *command,
+        stdout=asyncio.subprocess.PIPE,
+        stderr=asyncio.subprocess.PIPE,
+    )
+
+    async def stream_output(
+        stream: asyncio.StreamReader,
+        log_func: Callable[[str], None],
+    ) -> None:
+        while True:
+            line = await stream.readline()
+            if not line:
+                break
+            log_func(line.decode().rstrip())
+
+    tasks = []
+    if process.stdout is not None:
+        tasks.append(asyncio.create_task(stream_output(process.stdout, logger.info)))
+    if process.stderr is not None:
+        tasks.append(asyncio.create_task(stream_output(process.stderr, logger.error)))
+
+    await asyncio.gather(*tasks)
+    await process.wait()