reachy-mini 1.0.0__py3-none-any.whl

reachy_mini/daemon/app/routers/move.py

@@ -0,0 +1,257 @@
+"""Movement-related API routes.
+
+This exposes:
+- goto
+- play (wake_up, goto_sleep)
+- stop running moves
+- set_target and streaming set_target
+"""
+
+import asyncio
+import json
+from enum import Enum
+from typing import Any, Coroutine
+from uuid import UUID, uuid4
+
+import numpy as np
+from fastapi import APIRouter, Depends, HTTPException, WebSocket, WebSocketDisconnect
+from huggingface_hub.errors import RepositoryNotFoundError
+from pydantic import BaseModel
+
+from reachy_mini.motion.recorded_move import RecordedMoves
+
+from ....daemon.backend.abstract import Backend
+from ..dependencies import get_backend, ws_get_backend
+from ..models import AnyPose, FullBodyTarget
+
+move_tasks: dict[UUID, asyncio.Task[None]] = {}
+move_listeners: list[WebSocket] = []
+
+
+router = APIRouter(prefix="/move")
+
+
+class InterpolationMode(str, Enum):
+    """Interpolation modes for movement."""
+
+    # TODO: This should be the same as for the backend
+
+    LINEAR = "linear"
+    MINJERK = "minjerk"
+    EASE = "ease"
+    CARTOON = "cartoon"
+
+
+class GotoModelRequest(BaseModel):
+    """Request model for the goto endpoint."""
+
+    head_pose: AnyPose | None = None
+    antennas: tuple[float, float] | None = None
+    duration: float
+    interpolation: InterpolationMode = InterpolationMode.MINJERK
+
+    model_config = {
+        "json_schema_extra": {
+            "examples": [
+                {
+                    "head_pose": {
+                        "x": 0.0,
+                        "y": 0.0,
+                        "z": 0.0,
+                        "roll": 0.0,
+                        "pitch": 0.0,
+                        "yaw": 0.0,
+                    },
+                    "antennas": [0.0, 0.0],
+                    "duration": 2.0,
+                    "interpolation": "minjerk",
+                },
+                {
+                    "antennas": [0.0, 0.0],
+                    "duration": 1.0,
+                    "interpolation": "linear",
+                },
+            ],
+        }
+    }
+
+
+class MoveUUID(BaseModel):
+    """Model representing a unique identifier for a move task."""
+
+    uuid: UUID
+
+
+def create_move_task(coro: Coroutine[Any, Any, None]) -> MoveUUID:
+    """Create a new move task using async task coroutine."""
+    uuid = uuid4()
+
+    async def notify_listeners(message: str, details: str = "") -> None:
+        for ws in move_listeners:
+            try:
+                await ws.send_json(
+                    {
+                        "type": message,
+                        "uuid": str(uuid),
+                        "details": details,
+                    }
+                )
+            except (RuntimeError, WebSocketDisconnect):
+                move_listeners.remove(ws)
+
+    async def wrap_coro() -> None:
+        try:
+            await notify_listeners("move_started")
+            await coro
+            await notify_listeners("move_completed")
+        except Exception as e:
+            await notify_listeners("move_failed", details=str(e))
+        except asyncio.CancelledError:
+            await notify_listeners("move_cancelled")
+        finally:
+            move_tasks.pop(uuid, None)
+
+    task = asyncio.create_task(wrap_coro())
+    move_tasks[uuid] = task
+
+    return MoveUUID(uuid=uuid)
+
+
+async def stop_move_task(uuid: UUID) -> dict[str, str]:
+    """Stop a running move task by cancelling it."""
+    if uuid not in move_tasks:
+        raise KeyError(f"Running move with UUID {uuid} not found")
+
+    task = move_tasks.pop(uuid, None)
+    assert task is not None
+
+    if task:
+        if task.cancel():
+            try:
+                await task
+            except asyncio.CancelledError:
+                pass
+
+    return {
+        "message": f"Stopped move with UUID: {uuid}",
+    }
+
+
+@router.get("/running")
+async def get_running_moves() -> list[MoveUUID]:
+    """Get a list of currently running move tasks."""
+    return [MoveUUID(uuid=uuid) for uuid in move_tasks.keys()]
+
+
+@router.post("/goto")
+async def goto(
+    goto_req: GotoModelRequest, backend: Backend = Depends(get_backend)
+) -> MoveUUID:
+    """Request a movement to a specific target."""
+    return create_move_task(
+        backend.goto_target(
+            head=goto_req.head_pose.to_pose_array() if goto_req.head_pose else None,
+            antennas=np.array(goto_req.antennas) if goto_req.antennas else None,
+            duration=goto_req.duration,
+        )
+    )
+
+
+@router.post("/play/wake_up")
+async def play_wake_up(backend: Backend = Depends(get_backend)) -> MoveUUID:
+    """Request the robot to wake up."""
+    return create_move_task(backend.wake_up())
+
+
+@router.post("/play/goto_sleep")
+async def play_goto_sleep(backend: Backend = Depends(get_backend)) -> MoveUUID:
+    """Request the robot to go to sleep."""
+    return create_move_task(backend.goto_sleep())
+
+
+@router.get("/recorded-move-datasets/list/{dataset_name:path}")
+async def list_recorded_move_dataset(
+    dataset_name: str,
+) -> list[str]:
+    """List available recorded moves in a dataset."""
+    try:
+        moves = RecordedMoves(dataset_name)
+    except RepositoryNotFoundError as e:
+        raise HTTPException(status_code=404, detail=str(e))
+
+    return moves.list_moves()
+
+
+@router.post("/play/recorded-move-dataset/{dataset_name:path}/{move_name}")
+async def play_recorded_move_dataset(
+    dataset_name: str,
+    move_name: str,
+    backend: Backend = Depends(get_backend),
+) -> MoveUUID:
+    """Request the robot to play a predefined recorded move from a dataset."""
+    try:
+        recorded_moves = RecordedMoves(dataset_name)
+    except RepositoryNotFoundError as e:
+        raise HTTPException(status_code=404, detail=str(e))
+    try:
+        move = recorded_moves.get(move_name)
+    except ValueError as e:
+        raise HTTPException(status_code=404, detail=str(e))
+    return create_move_task(backend.play_move(move))
+
+
+@router.post("/stop")
+async def stop_move(uuid: MoveUUID) -> dict[str, str]:
+    """Stop a running move task."""
+    return await stop_move_task(uuid.uuid)
+
+
+@router.websocket("/ws/updates")
+async def ws_move_updates(
+    websocket: WebSocket,
+) -> None:
+    """WebSocket route to stream move updates."""
+    await websocket.accept()
+    try:
+        move_listeners.append(websocket)
+        while True:
+            _ = await websocket.receive_text()
+    except WebSocketDisconnect:
+        move_listeners.remove(websocket)
+
+
+# --- FullBodyTarget streaming and single set_target ---
+@router.post("/set_target")
+async def set_target(
+    target: FullBodyTarget,
+    backend: Backend = Depends(get_backend),
+) -> dict[str, str]:
+    """POST route to set a single FullBodyTarget."""
+    backend.set_target(
+        head=target.target_head_pose.to_pose_array()
+        if target.target_head_pose
+        else None,
+        antennas=np.array(target.target_antennas) if target.target_antennas else None,
+    )
+    return {"status": "ok"}
+
+
+@router.websocket("/ws/set_target")
+async def ws_set_target(
+    websocket: WebSocket, backend: Backend = Depends(ws_get_backend)
+) -> None:
+    """WebSocket route to stream FullBodyTarget set_target calls."""
+    await websocket.accept()
+    try:
+        while True:
+            data = await websocket.receive_text()
+            try:
+                target = FullBodyTarget.model_validate_json(data)
+                await set_target(target, backend)
+
+            except Exception as e:
+                await websocket.send_text(
+                    json.dumps({"status": "error", "detail": str(e)})
+                )
+    except WebSocketDisconnect:
+        pass

reachy_mini/daemon/app/routers/state.py

@@ -0,0 +1,146 @@
+"""State-related API routes.
+
+This exposes:
+- basic get routes to retrieve most common fields
+- full state and streaming state updates
+"""
+
+import asyncio
+from datetime import datetime, timezone
+from typing import Any
+
+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
+
+router = APIRouter(prefix="/state")
+
+
+@router.get("/present_head_pose")
+async def get_head_pose(
+    use_pose_matrix: bool = False,
+    backend: Backend = Depends(get_backend),
+) -> AnyPose:
+    """Get the present head pose.
+
+    Arguments:
+        use_pose_matrix (bool): Whether to use the pose matrix representation (4x4 flattened) or the translation + Euler angles representation (x, y, z, roll, pitch, yaw).
+        backend (Backend): The backend instance.
+
+    Returns:
+        AnyPose: The present head pose.
+
+    """
+    return as_any_pose(backend.get_present_head_pose(), use_pose_matrix)
+
+
+@router.get("/present_body_yaw")
+async def get_body_yaw(
+    backend: Backend = Depends(get_backend),
+) -> float:
+    """Get the present body yaw (in radians)."""
+    return backend.get_present_body_yaw()
+
+
+@router.get("/present_antenna_joint_positions")
+async def get_antenna_joint_positions(
+    backend: Backend = Depends(get_backend),
+) -> tuple[float, float]:
+    """Get the present antenna joint positions (in radians) - (left, right)."""
+    pos = backend.get_present_antenna_joint_positions()
+    assert len(pos) == 2
+    return (pos[0], pos[1])
+
+
+@router.get("/full")
+async def get_full_state(
+    with_control_mode: bool = True,
+    with_head_pose: bool = True,
+    with_target_head_pose: bool = False,
+    with_head_joints: bool = False,
+    with_target_head_joints: bool = False,
+    with_body_yaw: bool = True,
+    with_target_body_yaw: bool = False,
+    with_antenna_positions: bool = True,
+    with_target_antenna_positions: bool = False,
+    with_passive_joints: bool = False,
+    use_pose_matrix: bool = False,
+    backend: Backend = Depends(get_backend),
+) -> FullState:
+    """Get the full robot state, with optional fields."""
+    result: dict[str, Any] = {}
+
+    if with_control_mode:
+        result["control_mode"] = backend.get_motor_control_mode().value
+
+    if with_head_pose:
+        pose = backend.get_present_head_pose()
+        result["head_pose"] = as_any_pose(pose, use_pose_matrix)
+    if with_target_head_pose:
+        target_pose = backend.target_head_pose
+        assert target_pose is not None
+        result["target_head_pose"] = as_any_pose(target_pose, use_pose_matrix)
+    if with_head_joints:
+        result["head_joints"] = backend.get_present_head_joint_positions()
+    if with_target_head_joints:
+        result["target_head_joints"] = backend.target_head_joint_positions
+    if with_body_yaw:
+        result["body_yaw"] = backend.get_present_body_yaw()
+    if with_target_body_yaw:
+        result["target_body_yaw"] = backend.target_body_yaw
+    if with_antenna_positions:
+        result["antennas_position"] = backend.get_present_antenna_joint_positions()
+    if with_target_antenna_positions:
+        result["target_antennas_position"] = backend.target_antenna_joint_positions
+    if with_passive_joints:
+        joints = backend.get_present_passive_joint_positions()
+        if joints is not None:
+            result["passive_joints"] = list(joints.values())
+        else:
+            result["passive_joints"] = None
+
+    result["timestamp"] = datetime.now(timezone.utc)
+    return FullState.model_validate(result)
+
+
+@router.websocket("/ws/full")
+async def ws_full_state(
+    websocket: WebSocket,
+    frequency: float = 10.0,
+    with_head_pose: bool = True,
+    with_target_head_pose: bool = False,
+    with_head_joints: bool = False,
+    with_target_head_joints: bool = False,
+    with_body_yaw: bool = True,
+    with_target_body_yaw: bool = False,
+    with_antenna_positions: bool = True,
+    with_target_antenna_positions: bool = False,
+    with_passive_joints: bool = False,
+    use_pose_matrix: bool = False,
+    backend: Backend = Depends(ws_get_backend),
+) -> None:
+    """WebSocket endpoint to stream the full state of the robot."""
+    await websocket.accept()
+    period = 1.0 / frequency
+
+    try:
+        while True:
+            full_state = await get_full_state(
+                with_head_pose=with_head_pose,
+                with_target_head_pose=with_target_head_pose,
+                with_head_joints=with_head_joints,
+                with_target_head_joints=with_target_head_joints,
+                with_body_yaw=with_body_yaw,
+                with_target_body_yaw=with_target_body_yaw,
+                with_antenna_positions=with_antenna_positions,
+                with_target_antenna_positions=with_target_antenna_positions,
+                with_passive_joints=with_passive_joints,
+                use_pose_matrix=use_pose_matrix,
+                backend=backend,
+            )
+            await websocket.send_text(full_state.model_dump_json())
+            await asyncio.sleep(period)
+    except WebSocketDisconnect:
+        pass

reachy_mini/daemon/backend/__init__.py

@@ -0,0 +1 @@
+"""Backend module for Reachy Mini Daemon."""