reachy-mini 1.2.5rc1__py3-none-any.whl

reachy_mini/media/webrtc_daemon.py

@@ -0,0 +1,334 @@
+"""WebRTC daemon.
+
+Starts a gstreamer webrtc pipeline to stream video and audio.
+"""
+
+import logging
+from threading import Thread
+from typing import Optional, Tuple, cast
+
+import gi
+
+from reachy_mini.media.camera_constants import (
+    ArducamSpecs,
+    CameraSpecs,
+    ReachyMiniLiteCamSpecs,
+    ReachyMiniWirelessCamSpecs,
+)
+
+gi.require_version("Gst", "1.0")
+gi.require_version("GstApp", "1.0")
+
+from gi.repository import GLib, Gst  # noqa: E402
+
+
+class GstWebRTC:
+    """WebRTC pipeline using GStreamer."""
+
+    def __init__(
+        self,
+        log_level: str = "INFO",
+    ) -> None:
+        """Initialize the GStreamer WebRTC pipeline."""
+        self._logger = logging.getLogger(__name__)
+        self._logger.setLevel(log_level)
+
+        # self._id_audio_card = get_respeaker_card_number()
+
+        Gst.init(None)
+        self._loop = GLib.MainLoop()
+        self._thread_bus_calls = Thread(target=lambda: self._loop.run(), daemon=True)
+        self._thread_bus_calls.start()
+
+        cam_path, self.camera_specs = self._get_video_device()
+
+        if self.camera_specs is None:
+            raise RuntimeError("Camera specs not set")
+        self._resolution = self.camera_specs.default_resolution
+        self.resized_K = self.camera_specs.K
+
+        if self._resolution is None:
+            raise RuntimeError("Failed to get default camera resolution.")
+
+        self._pipeline_sender = Gst.Pipeline.new("reachymini_webrtc_sender")
+        self._bus_sender = self._pipeline_sender.get_bus()
+        self._bus_sender.add_watch(
+            GLib.PRIORITY_DEFAULT, self._on_bus_message, self._loop
+        )
+
+        webrtcsink = self._configure_webrtc(self._pipeline_sender)
+
+        self._configure_video(cam_path, self._pipeline_sender, webrtcsink)
+        self._configure_audio(self._pipeline_sender, webrtcsink)
+
+        self._pipeline_receiver = Gst.Pipeline.new("reachymini_webrtc_receiver")
+        self._bus_receiver = self._pipeline_receiver.get_bus()
+        self._bus_receiver.add_watch(
+            GLib.PRIORITY_DEFAULT, self._on_bus_message, self._loop
+        )
+        self._configure_receiver(self._pipeline_receiver)
+
+    def __del__(self) -> None:
+        """Destructor to ensure gstreamer resources are released."""
+        self._loop.quit()
+        self._bus_sender.remove_watch()
+        self._bus_receiver.remove_watch()
+
+    def _configure_webrtc(self, pipeline: Gst.Pipeline) -> Gst.Element:
+        self._logger.debug("Configuring WebRTC")
+        webrtcsink = Gst.ElementFactory.make("webrtcsink")
+        if not webrtcsink:
+            raise RuntimeError(
+                "Failed to create webrtcsink element. Is the GStreamer webrtc rust plugin installed?"
+            )
+
+        meta_structure = Gst.Structure.new_empty("meta")
+        meta_structure.set_value("name", "reachymini")
+        webrtcsink.set_property("meta", meta_structure)
+        webrtcsink.set_property("run-signalling-server", True)
+
+        pipeline.add(webrtcsink)
+
+        return webrtcsink
+
+    def _configure_receiver(self, pipeline: Gst.Pipeline) -> None:
+        udpsrc = Gst.ElementFactory.make("udpsrc")
+        udpsrc.set_property("port", 5000)
+        caps = Gst.Caps.from_string(
+            "application/x-rtp,media=audio,encoding-name=OPUS,payload=96"
+        )
+        capsfilter = Gst.ElementFactory.make("capsfilter")
+        capsfilter.set_property("caps", caps)
+        rtpjitterbuffer = Gst.ElementFactory.make("rtpjitterbuffer")
+        rtpjitterbuffer.set_property(
+            "latency", 200
+        )  # configure latency depending on network conditions
+        rtpopusdepay = Gst.ElementFactory.make("rtpopusdepay")
+        opusdec = Gst.ElementFactory.make("opusdec")
+        queue = Gst.ElementFactory.make("queue")
+        audioconvert = Gst.ElementFactory.make("audioconvert")
+        audioresample = Gst.ElementFactory.make("audioresample")
+        alsasink = Gst.ElementFactory.make("alsasink")
+        alsasink.set_property(
+            "device", "reachymini_audio_sink"
+        )  # f"hw:{self._id_audio_card},0")
+        alsasink.set_property("sync", False)
+
+        pipeline.add(udpsrc)
+        pipeline.add(capsfilter)
+        pipeline.add(rtpjitterbuffer)
+        pipeline.add(rtpopusdepay)
+        pipeline.add(opusdec)
+        pipeline.add(queue)
+        pipeline.add(audioconvert)
+        pipeline.add(audioresample)
+        pipeline.add(alsasink)
+
+        udpsrc.link(capsfilter)
+        capsfilter.link(rtpjitterbuffer)
+        rtpjitterbuffer.link(rtpopusdepay)
+        rtpopusdepay.link(opusdec)
+        opusdec.link(queue)
+        queue.link(audioconvert)
+        audioconvert.link(audioresample)
+        audioresample.link(alsasink)
+
+    @property
+    def resolution(self) -> tuple[int, int]:
+        """Get the current camera resolution as a tuple (width, height)."""
+        return (self._resolution.value[0], self._resolution.value[1])
+
+    @property
+    def framerate(self) -> int:
+        """Get the current camera framerate."""
+        return self._resolution.value[2]
+
+    def _configure_video(
+        self, cam_path: str, pipeline: Gst.Pipeline, webrtcsink: Gst.Element
+    ) -> None:
+        self._logger.debug(f"Configuring video {cam_path}")
+        camerasrc = Gst.ElementFactory.make("libcamerasrc")
+        caps = Gst.Caps.from_string(
+            f"video/x-raw,width={self.resolution[0]},height={self.resolution[1]},framerate={self.framerate}/1,format=YUY2,colorimetry=bt709,interlace-mode=progressive"
+        )
+        capsfilter = Gst.ElementFactory.make("capsfilter")
+        capsfilter.set_property("caps", caps)
+        tee = Gst.ElementFactory.make("tee")
+        # make camera accessible to other applications via unixfdsrc/sink
+        unixfdsink = Gst.ElementFactory.make("unixfdsink")
+        unixfdsink.set_property("socket-path", "/tmp/reachymini_camera_socket")
+        queue_unixfd = Gst.ElementFactory.make("queue", "queue_unixfd")
+        queue_encoder = Gst.ElementFactory.make("queue", "queue_encoder")
+        v4l2h264enc = Gst.ElementFactory.make("v4l2h264enc")
+        extra_controls_structure = Gst.Structure.new_empty("extra-controls")
+        extra_controls_structure.set_value("repeat_sequence_header", 1)
+        extra_controls_structure.set_value("video_bitrate", 5_000_000)
+        v4l2h264enc.set_property("extra-controls", extra_controls_structure)
+        caps_h264 = Gst.Caps.from_string(
+            "video/x-h264,stream-format=byte-stream,alignment=au,level=(string)4"
+        )
+        capsfilter_h264 = Gst.ElementFactory.make("capsfilter")
+        capsfilter_h264.set_property("caps", caps_h264)
+
+        if not all(
+            [
+                camerasrc,
+                capsfilter,
+                tee,
+                queue_unixfd,
+                unixfdsink,
+                queue_encoder,
+                v4l2h264enc,
+                capsfilter_h264,
+            ]
+        ):
+            raise RuntimeError("Failed to create GStreamer video elements")
+
+        pipeline.add(camerasrc)
+        pipeline.add(capsfilter)
+        pipeline.add(tee)
+        pipeline.add(queue_unixfd)
+        pipeline.add(unixfdsink)
+        pipeline.add(queue_encoder)
+        pipeline.add(v4l2h264enc)
+        pipeline.add(capsfilter_h264)
+
+        camerasrc.link(capsfilter)
+        capsfilter.link(tee)
+        tee.link(queue_unixfd)
+        queue_unixfd.link(unixfdsink)
+        tee.link(queue_encoder)
+        queue_encoder.link(v4l2h264enc)
+        v4l2h264enc.link(capsfilter_h264)
+        capsfilter_h264.link(webrtcsink)
+
+    def _configure_audio(self, pipeline: Gst.Pipeline, webrtcsink: Gst.Element) -> None:
+        self._logger.debug("Configuring audio")
+
+        alsasrc = Gst.ElementFactory.make("alsasrc")
+        alsasrc.set_property("device", "reachymini_audio_src")
+
+        if not all([alsasrc]):
+            raise RuntimeError("Failed to create GStreamer audio elements")
+
+        pipeline.add(alsasrc)
+        alsasrc.link(webrtcsink)
+
+    def _get_audio_input_device(self) -> Optional[str]:
+        """Use Gst.DeviceMonitor to find the pipeire audio card.
+
+        Returns the device ID of the found audio card, None if not.
+        """
+        monitor = Gst.DeviceMonitor()
+        monitor.add_filter("Audio/Source")
+        monitor.start()
+
+        snd_card_name = "Reachy Mini Audio"
+
+        devices = monitor.get_devices()
+        for device in devices:
+            name = device.get_display_name()
+            device_props = device.get_properties()
+
+            if snd_card_name in name:
+                if device_props and device_props.has_field("object.serial"):
+                    serial = device_props.get_string("object.serial")
+                    self._logger.debug(f"Found audio input device with serial {serial}")
+                    monitor.stop()
+                    return str(serial)
+
+        monitor.stop()
+        self._logger.warning("No source audio card found.")
+        return None
+
+    def _get_video_device(self) -> Tuple[str, Optional[CameraSpecs]]:
+        """Use Gst.DeviceMonitor to find the unix camera path /dev/videoX.
+
+        Returns the device path (e.g., '/dev/video2'), or '' if not found.
+        """
+        monitor = Gst.DeviceMonitor()
+        monitor.add_filter("Video/Source")
+        monitor.start()
+
+        cam_names = ["Reachy", "Arducam_12MP", "imx708"]
+
+        devices = monitor.get_devices()
+        for cam_name in cam_names:
+            for device in devices:
+                name = device.get_display_name()
+                device_props = device.get_properties()
+
+                if cam_name in name:
+                    if device_props and device_props.has_field("api.v4l2.path"):
+                        device_path = device_props.get_string("api.v4l2.path")
+                        camera_specs = (
+                            cast(CameraSpecs, ArducamSpecs)
+                            if cam_name == "Arducam_12MP"
+                            else cast(CameraSpecs, ReachyMiniLiteCamSpecs)
+                        )
+                        self._logger.debug(f"Found {cam_name} camera at {device_path}")
+                        monitor.stop()
+                        return str(device_path), camera_specs
+                    elif cam_name == "imx708":
+                        camera_specs = cast(CameraSpecs, ReachyMiniWirelessCamSpecs)
+                        self._logger.debug(f"Found {cam_name} camera")
+                        monitor.stop()
+                        return cam_name, camera_specs
+        monitor.stop()
+        self._logger.warning("No camera found.")
+        return "", None
+
+    def _on_bus_message(self, bus: Gst.Bus, msg: Gst.Message, loop) -> bool:  # type: ignore[no-untyped-def]
+        t = msg.type
+        if t == Gst.MessageType.EOS:
+            self._logger.warning("End-of-stream")
+            return False
+
+        elif t == Gst.MessageType.ERROR:
+            err, debug = msg.parse_error()
+            self._logger.error(f"Error: {err} {debug}")
+            return False
+
+        else:
+            # self._logger.warning(f"Unhandled message type: {t}")
+            pass
+
+        return True
+
+    def start(self) -> None:
+        """Start the WebRTC pipeline."""
+        self._logger.debug("Starting WebRTC")
+        self._pipeline_sender.set_state(Gst.State.PLAYING)
+        self._pipeline_receiver.set_state(Gst.State.PLAYING)
+
+    def pause(self) -> None:
+        """Pause the WebRTC pipeline."""
+        self._logger.debug("Pausing WebRTC")
+        self._pipeline_sender.set_state(Gst.State.PAUSED)
+        self._pipeline_receiver.set_state(Gst.State.PAUSED)
+
+    def stop(self) -> None:
+        """Stop the WebRTC pipeline."""
+        self._logger.debug("Stopping WebRTC")
+
+        self._pipeline_sender.set_state(Gst.State.NULL)
+        self._pipeline_receiver.set_state(Gst.State.NULL)
+
+
+if __name__ == "__main__":
+    import time
+
+    logging.basicConfig(
+        level=logging.DEBUG,
+        format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
+    )
+
+    webrtc = GstWebRTC(log_level="DEBUG")
+    webrtc.start()
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        logging.info("User interrupted")
+    finally:
+        webrtc.stop()

reachy_mini/motion/__init__.py

@@ -0,0 +1,4 @@
+"""Motion module for Reachy Mini.
+
+This module contains both utilities to create and play moves, as well as utilities to download datasets of recorded moves.
+"""

reachy_mini/motion/goto.py

@@ -0,0 +1,71 @@
+"""A goto move to a target head pose and/or antennas position."""
+
+import numpy as np
+import numpy.typing as npt
+
+from reachy_mini.utils.interpolation import (
+    InterpolationTechnique,
+    linear_pose_interpolation,
+    time_trajectory,
+)
+
+from .move import Move
+
+
+class GotoMove(Move):
+    """A goto move to a target head pose and/or antennas position."""
+
+    def __init__(
+        self,
+        start_head_pose: npt.NDArray[np.float64],
+        target_head_pose: npt.NDArray[np.float64] | None,
+        start_antennas: npt.NDArray[np.float64],
+        target_antennas: npt.NDArray[np.float64] | None,
+        start_body_yaw: float,
+        target_body_yaw: float | None,
+        duration: float,
+        method: InterpolationTechnique,
+    ):
+        """Set up the goto move."""
+        self.start_head_pose = start_head_pose
+        self.target_head_pose = (
+            target_head_pose if target_head_pose is not None else start_head_pose
+        )
+        self.start_antennas = start_antennas
+        self.target_antennas = (
+            target_antennas if target_antennas is not None else start_antennas
+        )
+        self.start_body_yaw = start_body_yaw
+        self.target_body_yaw = (
+            target_body_yaw if target_body_yaw is not None else start_body_yaw
+        )
+
+        self._duration = duration
+        self.method = method
+
+    @property
+    def duration(self) -> float:
+        """Duration of the goto in seconds."""
+        return self._duration
+
+    def evaluate(
+        self, t: float
+    ) -> tuple[
+        npt.NDArray[np.float64] | None, npt.NDArray[np.float64] | None, float | None
+    ]:
+        """Evaluate the goto at time t."""
+        interp_time = time_trajectory(t / self.duration, method=self.method)
+
+        interp_head_pose = linear_pose_interpolation(
+            self.start_head_pose, self.target_head_pose, interp_time
+        )
+        interp_antennas_joint = (
+            self.start_antennas
+            + (self.target_antennas - self.start_antennas) * interp_time
+        )
+        interp_body_yaw_joint = (
+            self.start_body_yaw
+            + (self.target_body_yaw - self.start_body_yaw) * interp_time
+        )
+
+        return interp_head_pose, interp_antennas_joint, interp_body_yaw_joint

reachy_mini/motion/move.py

@@ -0,0 +1,43 @@
+"""Module for defining motion moves on the ReachyMini robot."""
+
+from abc import ABC, abstractmethod
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+import numpy.typing as npt
+
+
+class Move(ABC):
+    """Abstract base class for defining a move on the ReachyMini robot."""
+
+    @property
+    def sound_path(self) -> Optional[Path]:
+        """Get the sound path associated with the move, if any."""
+        return None
+
+    @property
+    @abstractmethod
+    def duration(self) -> float:
+        """Duration of the move in seconds."""
+        pass
+
+    @abstractmethod
+    def evaluate(
+        self,
+        t: float,
+    ) -> tuple[
+        npt.NDArray[np.float64] | None, npt.NDArray[np.float64] | None, float | None
+    ]:
+        """Evaluate the move at time t, typically called at a high-frequency (eg. 100Hz).
+
+        Arguments:
+            t: The time at which to evaluate the move (in seconds). It will always be between 0 and duration.
+
+        Returns:
+            head: The head position (4x4 homogeneous matrix).
+            antennas: The antennas positions (rad).
+            body_yaw: The body yaw angle (rad).
+
+        """
+        pass

reachy_mini/motion/recorded_move.py

@@ -0,0 +1,146 @@
+import bisect  # noqa: D100
+import json
+import os
+from glob import glob
+from pathlib import Path
+from typing import Any, Dict, List, Optional
+
+import numpy as np
+import numpy.typing as npt
+from huggingface_hub import snapshot_download
+
+from reachy_mini.motion.move import Move
+from reachy_mini.utils.interpolation import linear_pose_interpolation
+
+
+def lerp(v0: float, v1: float, alpha: float) -> float:
+    """Linear interpolation between two values."""
+    return v0 + alpha * (v1 - v0)
+
+
+class RecordedMove(Move):
+    """Represent a recorded move."""
+
+    def __init__(self, move: Dict[str, Any], sound_path: Optional[Path] = None) -> None:
+        """Initialize RecordedMove."""
+        self.move = move
+        self._sound_path = sound_path
+
+        self.description: str = self.move["description"]
+        self.timestamps: List[float] = self.move["time"]
+        self.trajectory: List[Dict[str, List[List[float]] | List[float] | float]] = (
+            self.move["set_target_data"]
+        )
+
+        self.dt: float = (self.timestamps[-1] - self.timestamps[0]) / len(
+            self.timestamps
+        )
+
+    @property
+    def duration(self) -> float:
+        """Get the duration of the recorded move."""
+        return len(self.trajectory) * self.dt
+
+    @property
+    def sound_path(self) -> Optional[Path]:
+        """Get the sound path associated with the move, if any."""
+        return self._sound_path
+
+    def evaluate(
+        self, t: float
+    ) -> tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], float]:
+        """Evaluate the move at time t.
+
+        Returns:
+            head: The head position (4x4 homogeneous matrix).
+            antennas: The antennas positions (rad).
+            body_yaw: The body yaw angle (rad).
+
+        """
+        # Under is Remi's emotions code, adapted
+        if t >= self.timestamps[-1]:
+            raise Exception("Tried to evaluate recorded move beyond its duration.")
+
+        # Locate the right interval in the recorded time array.
+        # 'index' is the insertion point which gives us the next timestamp.
+        index = bisect.bisect_right(self.timestamps, t)
+        # print(f"index: {index}, expected index: {t / self.dt:.0f}")
+        idx_prev = index - 1 if index > 0 else 0
+        idx_next = index if index < len(self.timestamps) else idx_prev
+
+        t_prev = self.timestamps[idx_prev]
+        t_next = self.timestamps[idx_next]
+
+        # Avoid division by zero (if by any chance two timestamps are identical).
+        if t_next == t_prev:
+            alpha = 0.0
+        else:
+            alpha = (t - t_prev) / (t_next - t_prev)
+
+        head_prev = np.array(self.trajectory[idx_prev]["head"], dtype=np.float64)
+        head_next = np.array(self.trajectory[idx_next]["head"], dtype=np.float64)
+        antennas_prev: List[float] = self.trajectory[idx_prev]["antennas"]  # type: ignore[assignment]
+        antennas_next: List[float] = self.trajectory[idx_next]["antennas"]  # type: ignore[assignment]
+        body_yaw_prev: float = self.trajectory[idx_prev].get("body_yaw", 0.0)  # type: ignore[assignment]
+        body_yaw_next: float = self.trajectory[idx_next].get("body_yaw", 0.0)  # type: ignore[assignment]
+        # check_collision = self.trajectory[idx_prev].get("check_collision", False)
+
+        # Interpolate to infer a better position at the current time.
+        # Joint interpolations are easy:
+
+        antennas_joints = np.array(
+            [
+                lerp(pos_prev, pos_next, alpha)
+                for pos_prev, pos_next in zip(antennas_prev, antennas_next)
+            ],
+            dtype=np.float64,
+        )
+
+        body_yaw = lerp(body_yaw_prev, body_yaw_next, alpha)
+
+        # Head position interpolation is more complex:
+        head_pose = linear_pose_interpolation(head_prev, head_next, alpha)
+
+        return head_pose, antennas_joints, body_yaw
+
+
+class RecordedMoves:
+    """Load a library of recorded moves from a HuggingFace dataset."""
+
+    def __init__(self, hf_dataset_name: str):
+        """Initialize RecordedMoves."""
+        self.hf_dataset_name = hf_dataset_name
+        self.local_path = snapshot_download(self.hf_dataset_name, repo_type="dataset")
+        self.moves: Dict[str, Any] = {}
+        self.sounds: Dict[str, Optional[Path]] = {}
+
+        self.process()
+
+    def process(self) -> None:
+        """Populate recorded moves and sounds."""
+        move_paths_tmp = glob(f"{self.local_path}/*.json")
+        move_paths = [Path(move_path) for move_path in move_paths_tmp]
+        for move_path in move_paths:
+            move_name = move_path.stem
+
+            move = json.load(open(move_path, "r"))
+            self.moves[move_name] = move
+
+            sound_path = move_path.with_suffix(".wav")
+            self.sounds[move_name] = None
+
+            if os.path.exists(sound_path):
+                self.sounds[move_name] = sound_path
+
+    def get(self, move_name: str) -> RecordedMove:
+        """Get a recorded move by name."""
+        if move_name not in self.moves:
+            raise ValueError(
+                f"Move {move_name} not found in recorded moves library {self.hf_dataset_name}"
+            )
+
+        return RecordedMove(self.moves[move_name], self.sounds[move_name])
+
+    def list_moves(self) -> List[str]:
+        """List all moves in the loaded library."""
+        return list(self.moves.keys())