126 lines
4.5 KiB
Python
126 lines
4.5 KiB
Python
"""Motion endpoints — arm actions, replay management."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import asyncio
|
|
|
|
from fastapi import APIRouter, HTTPException, Query
|
|
from pydantic import BaseModel
|
|
|
|
router = APIRouter()
|
|
|
|
|
|
def _block_if_movement_armed():
|
|
"""409 if locomotion movement is armed — arm actions are mutually exclusive
|
|
with walking. The arm controller's own motion-block is the safety net; this
|
|
just gives the dashboard a clear message instead of a silent no-op."""
|
|
try:
|
|
from Project.Sanad.main import loco_controller # type: ignore
|
|
armed = loco_controller is not None and loco_controller.is_armed()
|
|
except HTTPException:
|
|
raise
|
|
except Exception:
|
|
return
|
|
if armed:
|
|
raise HTTPException(
|
|
409, "Arm actions are disabled while movement is enabled. "
|
|
"Disable movement in the Controller tab first.")
|
|
|
|
|
|
@router.get("/status")
|
|
async def motion_status():
|
|
from Project.Sanad.main import arm
|
|
return arm.status() if arm else {"error": "Arm not attached"}
|
|
|
|
|
|
@router.get("/actions")
|
|
async def list_actions():
|
|
from Project.Sanad.main import arm
|
|
return {"actions": arm.list_actions() if arm else []}
|
|
|
|
|
|
class TriggerPayload(BaseModel):
|
|
action_id: int | None = None
|
|
action_name: str | None = None
|
|
speed: float = 1.0
|
|
|
|
|
|
@router.post("/trigger")
|
|
async def trigger_action(payload: TriggerPayload):
|
|
from Project.Sanad.main import arm
|
|
if arm is None:
|
|
raise HTTPException(503, "Arm controller not attached.")
|
|
|
|
# Only the legacy JSONL replays (direct rt/arm_sdk streaming) conflict with
|
|
# the walker. R1 preset gestures go through the arm service, which moves the
|
|
# arms while the firmware balances — so those are allowed while movement is
|
|
# armed. Gate JSONL only.
|
|
acts = arm.list_actions() if hasattr(arm, "list_actions") else []
|
|
resolved = None
|
|
if payload.action_id is not None:
|
|
resolved = next((a for a in acts if a.get("id") == payload.action_id), None)
|
|
elif payload.action_name:
|
|
resolved = next((a for a in acts if a.get("name") == payload.action_name), None)
|
|
if resolved and resolved.get("file"):
|
|
_block_if_movement_armed()
|
|
|
|
speed = max(0.1, min(payload.speed, 5.0))
|
|
|
|
# NOTE: TOCTOU on arm.is_busy is unavoidable from the route layer.
|
|
# The internal arm controller has its own _lock + _is_busy guard inside
|
|
# _execute() that returns silently if busy. We rely on that.
|
|
if payload.action_id is not None:
|
|
try:
|
|
await asyncio.to_thread(arm.trigger_by_id, payload.action_id, speed)
|
|
except KeyError as exc:
|
|
raise HTTPException(404, str(exc))
|
|
return {"ok": True, "action_id": payload.action_id, "speed": speed}
|
|
elif payload.action_name:
|
|
try:
|
|
await asyncio.to_thread(arm.trigger_by_name, payload.action_name, speed)
|
|
except KeyError as exc:
|
|
raise HTTPException(404, str(exc))
|
|
return {"ok": True, "action_name": payload.action_name, "speed": speed}
|
|
else:
|
|
raise HTTPException(400, "Provide action_id or action_name.")
|
|
|
|
|
|
# ── R1 head-look menu (recorded teach motions via the arm service) ──────────
|
|
|
|
@router.get("/head-actions")
|
|
async def head_actions():
|
|
"""The recorded head-look names for the Controller-tab HEAD menu."""
|
|
from Project.Sanad.main import arm
|
|
return {"looks": arm.head_looks() if arm else []}
|
|
|
|
|
|
@router.post("/head")
|
|
async def head_look(name: str = Query(...)):
|
|
"""Replay a recorded head rotation (R1 arm-service teach action). NOT gated by
|
|
the movement lock: the R1 arm service moves the head while the firmware keeps
|
|
the body balanced (that is the whole point of the canteen pattern), so head
|
|
looks are safe — and useful — while locomotion/teleop is armed."""
|
|
from Project.Sanad.main import arm
|
|
if arm is None:
|
|
raise HTTPException(503, "Arm controller not attached.")
|
|
res = await asyncio.to_thread(arm.run_head_look, name)
|
|
if not res.get("ok"):
|
|
raise HTTPException(502, res.get("error") or f"head look failed (rc={res.get('rc')})")
|
|
return res
|
|
|
|
|
|
@router.post("/cancel")
|
|
async def cancel_motion():
|
|
from Project.Sanad.main import arm
|
|
if arm is None:
|
|
raise HTTPException(503, "Arm controller not attached.")
|
|
arm.cancel()
|
|
return {"ok": True, "cancelled": True}
|
|
|
|
|
|
@router.post("/gestural-speaking")
|
|
async def toggle_gestural(enabled: bool = True):
|
|
from Project.Sanad.main import brain
|
|
brain.set_gestural_speaking(enabled)
|
|
return {"gestural_speaking": brain.gestural_speaking}
|