feat(right_arm): /home, /custom_pose, PUT /custom_pose, PUT /offset + fixture conftest

src/presa_mano/api/right_arm_routes.py

@@ -12,7 +12,7 @@ import numpy as np
 
 from ..clients import G1Unavailable
 from ..config import update_config
-from ..geometry import quaternion_xyzw_to_rpy_deg
+from ..geometry import quaternion_xyzw_to_rpy_deg, rpy_deg_to_quaternion_xyzw
 from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21
 from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist
 from ..timing import Stopwatch
@@ -274,3 +274,87 @@ def move_toward(req: MoveTowardReq) -> dict[str, Any]:
         "n_sam21_points": det["n_sam21_points"],
         "timings_ms": sw.as_dict(),
     }
+
+
+class PoseExecReq(BaseModel):
+    dry_run: bool = False
+
+
+class PoseUpdateReq(BaseModel):
+    position: Optional[list[float]] = None
+    rpy_deg: Optional[list[float]] = None
+
+
+class OffsetUpdateReq(BaseModel):
+    move_offset_camera_mm: Optional[dict[str, float]] = None
+    axis_sign_cam_to_wrist: Optional[dict[str, int]] = None
+
+
+def _exec_named_pose(pose_key: str, dry_run: bool, sw: Stopwatch) -> dict[str, Any]:
+    _guard_g1_right()
+    cfg = _deps.tab_right_arm_cfg()
+    pose = cfg.get(pose_key) or {}
+    pos = pose.get("position")
+    rpy = pose.get("rpy_deg")
+    if not (pos and rpy and len(pos) == 3 and len(rpy) == 3):
+        raise HTTPException(status_code=422, detail=f"{pose_key} malformed in config")
+    quat = rpy_deg_to_quaternion_xyzw(rpy[0], rpy[1], rpy[2])
+    try:
+        with sw.step("wrist_absolute"):
+            res = _deps.g1(dry_run=dry_run).wrist_absolute(
+                side="right", position=list(pos), quaternion=quat.tolist(),
+            )
+    except G1Unavailable as e:
+        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
+    return {"pose_key": pose_key, "position": pos, "rpy_deg": rpy,
+            "quaternion": quat.tolist(), "result": res, "dry_run": dry_run,
+            "applied": not dry_run, "timings_ms": sw.as_dict()}
+
+
+@router.post("/home")
+def post_home(req: PoseExecReq) -> dict[str, Any]:
+    return _exec_named_pose("home_pose", req.dry_run, Stopwatch())
+
+
+@router.post("/custom_pose")
+def post_custom_pose(req: PoseExecReq) -> dict[str, Any]:
+    return _exec_named_pose("custom_pose", req.dry_run, Stopwatch())
+
+
+@router.put("/custom_pose")
+def put_custom_pose(req: PoseUpdateReq) -> dict[str, Any]:
+    patch: dict[str, Any] = {}
+    if req.position is not None:
+        if len(req.position) != 3:
+            raise HTTPException(status_code=422, detail="position must be 3 floats")
+        patch["position"] = list(req.position)
+    if req.rpy_deg is not None:
+        if len(req.rpy_deg) != 3:
+            raise HTTPException(status_code=422, detail="rpy_deg must be 3 floats")
+        patch["rpy_deg"] = list(req.rpy_deg)
+    if not patch:
+        raise HTTPException(status_code=422, detail="nothing to update")
+    update_config({"tab_right_arm": {"custom_pose": patch}})
+    return {"custom_pose": _deps.tab_right_arm_cfg().get("custom_pose")}
+
+
+@router.put("/offset")
+def put_offset(req: OffsetUpdateReq) -> dict[str, Any]:
+    patch: dict[str, Any] = {}
+    if req.move_offset_camera_mm is not None:
+        for k in ("x", "y", "z"):
+            if k not in req.move_offset_camera_mm:
+                raise HTTPException(status_code=422, detail=f"move_offset_camera_mm missing '{k}'")
+        patch["move_offset_camera_mm"] = {k: float(req.move_offset_camera_mm[k]) for k in ("x", "y", "z")}
+    if req.axis_sign_cam_to_wrist is not None:
+        for k in ("x", "y", "z"):
+            v = req.axis_sign_cam_to_wrist.get(k)
+            if v not in (-1, 1):
+                raise HTTPException(status_code=422, detail=f"axis_sign_cam_to_wrist['{k}'] must be +/-1")
+        patch["axis_sign_cam_to_wrist"] = {k: int(req.axis_sign_cam_to_wrist[k]) for k in ("x", "y", "z")}
+    if not patch:
+        raise HTTPException(status_code=422, detail="nothing to update")
+    update_config({"tab_right_arm": patch})
+    cfg = _deps.tab_right_arm_cfg()
+    return {"move_offset_camera_mm": cfg.get("move_offset_camera_mm"),
+            "axis_sign_cam_to_wrist": cfg.get("axis_sign_cam_to_wrist")}