reachy-mini 1.0.0rc1__py3-none-any.whl

reachy_mini/daemon/app/models.py

@@ -0,0 +1,146 @@
+"""Common pydantic models definitions."""
+
+from datetime import datetime
+
+import numpy as np
+from numpy.typing import NDArray
+from pydantic import BaseModel
+from scipy.spatial.transform import Rotation as R
+
+from reachy_mini.daemon.backend.abstract import MotorControlMode
+
+
+class Matrix4x4Pose(BaseModel):
+    """Represent a 3D pose by its 4x4 transformation matrix (translation is expressed in meters)."""
+
+    m: tuple[
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+        float,
+    ]
+
+    @classmethod
+    def from_pose_array(cls, arr: NDArray[np.float64]) -> "Matrix4x4Pose":
+        """Create a Matrix4x4 pose representation from a 4x4 pose array."""
+        assert arr.shape == (4, 4), "Array must be of shape (4, 4)"
+        m: tuple[
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+            float,
+        ] = tuple(arr.flatten().tolist())  # type: ignore [assignment]
+        return cls(m=m)
+
+    def to_pose_array(self) -> NDArray[np.float64]:
+        """Convert the Matrix4x4Pose to a 4x4 numpy array."""
+        return np.array(self.m).reshape((4, 4))
+
+
+class XYZRPYPose(BaseModel):
+    """Represent a 3D pose using position (x, y, z) in meters and orientation (roll, pitch, yaw) angles in radians."""
+
+    x: float = 0.0
+    y: float = 0.0
+    z: float = 0.0
+    roll: float = 0.0
+    pitch: float = 0.0
+    yaw: float = 0.0
+
+    @classmethod
+    def from_pose_array(cls, arr: NDArray[np.float64]) -> "XYZRPYPose":
+        """Create an XYZRPYPose representation from a 4x4 pose array."""
+        assert arr.shape == (4, 4), "Array must be of shape (4, 4)"
+
+        x, y, z = arr[0, 3], arr[1, 3], arr[2, 3]
+        roll, pitch, yaw = R.from_matrix(arr[:3, :3]).as_euler("xyz")
+
+        return cls(
+            x=x,
+            y=y,
+            z=z,
+            roll=roll,
+            pitch=pitch,
+            yaw=yaw,
+        )
+
+    def to_pose_array(self) -> NDArray[np.float64]:
+        """Convert the XYZRPYPose to a 4x4 numpy array."""
+        rotation = R.from_euler("xyz", [self.roll, self.pitch, self.yaw])
+        pose_matrix = np.eye(4)
+        pose_matrix[:3, 3] = [self.x, self.y, self.z]
+        pose_matrix[:3, :3] = rotation.as_matrix()
+        return pose_matrix
+
+
+AnyPose = XYZRPYPose | Matrix4x4Pose
+
+
+def as_any_pose(pose: NDArray[np.float64], use_matrix: bool) -> AnyPose:
+    """Convert a numpy array to an AnyPose representation."""
+    return (
+        Matrix4x4Pose.from_pose_array(pose)
+        if use_matrix
+        else XYZRPYPose.from_pose_array(pose)
+    )
+
+
+class FullBodyTarget(BaseModel):
+    """Represent the full body including the head pose and the joints for antennas."""
+
+    target_head_pose: AnyPose | None = None
+    target_antennas: tuple[float, float] | None = None
+    timestamp: datetime | None = None
+
+    model_config = {
+        "json_schema_extra": {
+            "examples": [
+                {
+                    "target_head_pose": {
+                        "x": 0.0,
+                        "y": 0.0,
+                        "z": 0.0,
+                        "roll": 0.0,
+                        "pitch": 0.0,
+                        "yaw": 0.0,
+                    },
+                    "target_antennas": [0.0, 0.0],
+                }
+            ]
+        }
+    }
+
+
+class FullState(BaseModel):
+    """Represent the full state of the robot including all joint positions and poses."""
+
+    control_mode: MotorControlMode | None = None
+    head_pose: AnyPose | None = None
+    head_joints: list[float] | None = None
+    body_yaw: float | None = None
+    antennas_position: list[float] | None = None
+    timestamp: datetime | None = None

reachy_mini/daemon/app/routers/apps.py

@@ -0,0 +1,220 @@
+"""Apps router for apps management."""
+
+import asyncio
+import logging
+import uuid
+from dataclasses import dataclass
+from typing import Any, Awaitable, Callable
+
+from fastapi import (
+    APIRouter,
+    BackgroundTasks,
+    Depends,
+    HTTPException,
+    WebSocket,
+    WebSocketDisconnect,
+)
+from pydantic import BaseModel
+
+from reachy_mini.apps import AppInfo, SourceKind
+from reachy_mini.apps.manager import AppManager, AppStatus
+from reachy_mini.daemon.app.dependencies import get_app_manager
+
+router = APIRouter(prefix="/apps")
+
+
+class JobStatus(BaseModel):
+    """Pydantic model for install job status."""
+
+    command: str
+    status: str
+    logs: list[str]
+
+
+@dataclass
+class JobHandler:
+    """Handler for background jobs."""
+
+    status: JobStatus
+    new_log_evt: dict[str, asyncio.Event]
+
+
+jobs: dict[str, JobHandler] = {}
+
+
+@router.get("/list-available/{source_kind}")
+async def list_available_apps(
+    source_kind: SourceKind,
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> list[AppInfo]:
+    """List available apps (including not installed)."""
+    return await app_manager.list_available_apps(source_kind)
+
+
+@router.get("/list-available")
+async def list_all_available_apps(
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> list[AppInfo]:
+    """List all available apps (including not installed)."""
+    return await app_manager.list_all_available_apps()
+
+
+@router.post("/install")
+async def install_app(
+    app_info: AppInfo,
+    background_tasks: BackgroundTasks,
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> dict[str, str]:
+    """Install a new app by its info (background, returns job_id)."""
+    job_id = start_bg_job(
+        "install", background_tasks, app_manager.install_new_app, app_info
+    )
+    return {"job_id": job_id}
+
+
+@router.post("/remove/{app_name}")
+async def remove_app(
+    app_name: str,
+    background_tasks: BackgroundTasks,
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> dict[str, str]:
+    """Remove an installed app by its name (background, returns job_id)."""
+    job_id = start_bg_job("remove", background_tasks, app_manager.remove_app, app_name)
+    return {"job_id": job_id}
+
+
+@router.get("/job-status/{job_id}")
+async def job_status(job_id: str) -> JobStatus:
+    """Get status/logs for a job."""
+    job = jobs.get(job_id)
+    if not job:
+        raise HTTPException(status_code=404, detail="Job not found")
+    return job.status
+
+
+# WebSocket route for live job status/logs
+@router.websocket("/ws/apps-manager/{job_id}")
+async def ws_apps_manager(websocket: WebSocket, job_id: str) -> None:
+    """WebSocket route to stream live job status/logs for a job, sending updates as soon as new logs are available."""
+    await websocket.accept()
+    last_log_len = 0
+
+    job = jobs.get(job_id)
+    if not job:
+        await websocket.send_json({"error": "Job not found"})
+        await websocket.close()
+        return
+
+    assert job is not None  # for mypy
+
+    ws_uuid = str(uuid.uuid4())
+
+    try:
+        job.new_log_evt[ws_uuid] = asyncio.Event()
+        while True:
+            await job.new_log_evt[ws_uuid].wait()
+            job.new_log_evt[ws_uuid].clear()
+            new_logs = job.status.logs[last_log_len:]
+            last_log_len = len(job.status.logs)
+            await websocket.send_json(
+                {
+                    "command": job.status.command,
+                    "status": job.status.model_dump_json(),
+                    "logs": new_logs,
+                }
+            )
+            if job.status.status.startswith("done") or job.status.status.startswith(
+                "error"
+            ):
+                await websocket.close()
+                break
+    except WebSocketDisconnect:
+        pass
+
+    finally:
+        if ws_uuid in job.new_log_evt:
+            del job.new_log_evt[ws_uuid]
+
+
+@router.post("/start-app/{app_name}")
+async def start_app(
+    app_name: str,
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> AppStatus:
+    """Start an app by its name."""
+    try:
+        return await app_manager.start_app(app_name)
+    except RuntimeError as e:
+        raise HTTPException(status_code=400, detail=str(e))
+
+
+@router.post("/restart-current-app")
+async def restart_app(
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> AppStatus:
+    """Restart the currently running app."""
+    try:
+        return await app_manager.restart_current_app()
+    except RuntimeError as e:
+        raise HTTPException(status_code=400, detail=str(e))
+
+
+@router.post("/stop-current-app")
+async def stop_app(
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> None:
+    """Stop the currently running app."""
+    try:
+        return await app_manager.stop_current_app()
+    except RuntimeError as e:
+        raise HTTPException(status_code=400, detail=str(e))
+
+
+@router.get("/current-app-status")
+async def current_app_status(
+    app_manager: "AppManager" = Depends(get_app_manager),
+) -> AppStatus | None:
+    """Get the status of the currently running app, if any."""
+    return await app_manager.current_app_status()
+
+
+def start_bg_job(
+    command: str,
+    background_tasks: BackgroundTasks,
+    coro_func: Callable[..., Awaitable[None]],
+    *args: Any,
+) -> str:
+    """Start a background job, with a custom logger and return its job_id."""
+    job_id = str(uuid.uuid4())
+    jobs[job_id] = JobHandler(
+        status=JobStatus(
+            command=command,
+            status="pending",
+            logs=[],
+        ),
+        new_log_evt={},
+    )
+
+    async def run_command() -> None:
+        jobs[job_id].status.status = "running"
+
+        class JobLogger(logging.Handler):
+            def emit(self, record: logging.LogRecord) -> None:
+                jobs[job_id].status.logs.append(self.format(record))
+                for ws in jobs[job_id].new_log_evt.values():
+                    ws.set()
+
+        logger = logging.getLogger(f"logs_job_{job_id}")
+        logger.setLevel(logging.INFO)
+        logger.handlers.clear()
+        logger.addHandler(JobLogger())
+        try:
+            await coro_func(*args, logger=logger)
+            jobs[job_id].status.status = "done"
+            logger.info(f"Job '{command}' completed successfully")
+        except Exception as e:
+            jobs[job_id].status.status = f"error: {e}"
+            logger.error(f"Job '{command}' failed with error: {e}")
+
+    background_tasks.add_task(run_command)
+    return job_id

reachy_mini/daemon/app/routers/daemon.py

@@ -0,0 +1,53 @@
+"""Daemon-related API routes."""
+
+from fastapi import APIRouter, Depends, Request
+
+from ...daemon import Daemon, DaemonStatus
+from ..dependencies import get_daemon
+
+router = APIRouter(
+    prefix="/daemon",
+)
+
+
+@router.post("/start")
+async def start_daemon(
+    request: Request,
+    wake_up: bool,
+    daemon: Daemon = Depends(get_daemon),
+) -> DaemonStatus:
+    """Start the daemon."""
+    await daemon.start(
+        sim=request.app.state.args.sim,
+        scene=request.app.state.args.scene,
+        headless=request.app.state.args.headless,
+        wake_up_on_start=wake_up,
+    )
+    return daemon.status()
+
+
+@router.post("/stop")
+async def stop_daemon(
+    goto_sleep: bool, daemon: Daemon = Depends(get_daemon)
+) -> DaemonStatus:
+    """Stop the daemon, optionally putting the robot to sleep."""
+    await daemon.stop(goto_sleep_on_stop=goto_sleep)
+    return daemon.status()
+
+
+@router.post("/restart")
+async def restart_daemon(
+    request: Request, daemon: Daemon = Depends(get_daemon)
+) -> DaemonStatus:
+    """Restart the daemon."""
+    await daemon.restart(
+        sim=request.app.state.args.sim,
+        scene=request.app.state.args.scene,
+    )
+    return daemon.status()
+
+
+@router.get("/status")
+async def get_daemon_status(daemon: Daemon = Depends(get_daemon)) -> DaemonStatus:
+    """Get the current status of the daemon."""
+    return daemon.status()

reachy_mini/daemon/app/routers/kinematics.py

@@ -0,0 +1,36 @@
+"""Kinematics router for handling kinematics-related requests.
+
+This module defines the API endpoints for interacting with the kinematics
+subsystem of the robot. It provides endpoints for retrieving URDF representation,
+and other kinematics-related information.
+"""
+
+from typing import Any
+
+from fastapi import APIRouter, Depends
+
+from ....daemon.backend.abstract import Backend
+from ..dependencies import get_backend
+
+router = APIRouter(
+    prefix="/kinematics",
+)
+
+
+@router.get("/info")
+async def get_kinematics_info(
+    backend: Backend = Depends(get_backend),
+) -> dict[str, Any]:
+    """Get the current information of the kinematics."""
+    return {
+        "info": {
+            "engine": backend.kinematics_engine,
+            "collision check": backend.check_collision,
+        }
+    }
+
+
+@router.get("/urdf")
+async def get_urdf(backend: Backend = Depends(get_backend)) -> dict[str, str]:
+    """Get the URDF representation of the robot."""
+    return {"urdf": backend.get_urdf()}

reachy_mini/daemon/app/routers/motors.py

@@ -0,0 +1,41 @@
+"""Motors router.
+
+Provides endpoints to get and set the motor control mode.
+"""
+
+from fastapi import APIRouter, Depends
+from pydantic import BaseModel
+
+from ....daemon.backend.abstract import Backend, MotorControlMode
+from ..dependencies import get_backend
+
+router = APIRouter(
+    prefix="/motors",
+)
+
+
+class MotorStatus(BaseModel):
+    """Represents the status of the motors.
+
+    Exposes
+    - mode: The current motor control mode (enabled, disabled, gravity_compensation).
+    """
+
+    mode: MotorControlMode
+
+
+@router.get("/status")
+async def get_motor_status(backend: Backend = Depends(get_backend)) -> MotorStatus:
+    """Get the current status of the motors."""
+    return MotorStatus(mode=backend.get_motor_control_mode())
+
+
+@router.post("/set_mode/{mode}")
+async def set_motor_mode(
+    mode: MotorControlMode,
+    backend: Backend = Depends(get_backend),
+) -> dict[str, str]:
+    """Set the motor control mode."""
+    backend.set_motor_control_mode(mode)
+
+    return {"status": f"motors changed to {mode} mode"}

reachy_mini/daemon/app/routers/move.py

@@ -0,0 +1,200 @@
+"""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, WebSocket, WebSocketDisconnect
+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
+
+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
+
+
+move_tasks: dict[UUID, asyncio.Task[None]] = {}
+
+
+def create_move_task(coro: Coroutine[Any, Any, None]) -> MoveUUID:
+    """Create a new move task using async task coroutine."""
+    uuid = uuid4()
+
+    task = asyncio.create_task(coro)
+
+    task.add_done_callback(lambda t: move_tasks.pop(uuid, None))
+    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.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."""
+    move = RecordedMoves(dataset_name).get(move_name)
+    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)
+
+
+# --- 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