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")}

tests/conftest.py

@@ -7,3 +7,29 @@ from pathlib import Path
 _SRC = Path(__file__).resolve().parent.parent / "src"
 if str(_SRC) not in sys.path:
     sys.path.insert(0, str(_SRC))
+
+import pytest
+
+_CFG_PATH = Path(__file__).resolve().parent.parent / "config.yaml"
+
+
+@pytest.fixture(autouse=True)
+def _restore_config_yaml():
+    """Snapshot/restore config.yaml + in-memory singleton.
+    Any test that triggers update_config() would otherwise leave the file
+    cosmetically reformatted (compact flow-style -> block-style) AND/OR
+    semantically mutated. This fixture keeps the working tree clean."""
+    if not _CFG_PATH.exists():
+        yield
+        return
+    snapshot = _CFG_PATH.read_bytes()
+    try:
+        yield
+    finally:
+        _CFG_PATH.write_bytes(snapshot)
+        # Reset in-memory singleton so subsequent tests re-read disk
+        try:
+            from presa_mano import config as _cfg_mod
+            _cfg_mod._STATE["config"] = None  # type: ignore[attr-defined]
+        except Exception:
+            pass  # if internals change, we still get the disk reset

tests/test_right_arm_routes.py

@@ -326,3 +326,56 @@ def test_move_toward_412_when_not_g1_right():
         r = client.post("/api/right_arm/move/toward",
                         json={"class_name": "pestello", "dry_run": True})
         assert r.status_code == 412
+
+
+def test_home_calls_wrist_absolute_with_home_pose():
+    calls = []
+    def _abs(**kw):
+        calls.append(kw)
+        return {"ok": True}
+    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
+         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
+        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1
+        r = client.post("/api/right_arm/home", json={"dry_run": True})
+        assert r.status_code == 200
+        assert len(calls) == 1
+        assert calls[0]["side"] == "right"
+        assert len(calls[0]["position"]) == 3
+        assert len(calls[0]["quaternion"]) == 4
+
+
+def test_custom_pose_executes_pose_with_dry_run_default_false():
+    calls = []
+    def _abs(**kw):
+        calls.append(kw); return {"ok": True}
+    with patch("presa_mano.api.right_arm_routes._deps.g1") as g1_f, \
+         patch("presa_mano.api.right_arm_routes._deps.active_robot_key", return_value="g1"), \
+         patch("presa_mano.api.right_arm_routes._deps.default_side", return_value="right"):
+        g1 = MagicMock(); g1.wrist_absolute.side_effect = _abs; g1_f.return_value = g1
+        r = client.post("/api/right_arm/custom_pose", json={})
+        assert r.status_code == 200
+        assert len(calls) == 1
+
+
+def test_put_custom_pose_updates_config():
+    new_pos = [0.40, -0.15, 0.25]
+    new_rpy = [10.0, -5.0, 90.0]
+    r = client.put("/api/right_arm/custom_pose",
+                   json={"position": new_pos, "rpy_deg": new_rpy})
+    assert r.status_code == 200
+    r2 = client.get("/api/config")
+    cp = r2.json()["tab_right_arm"]["custom_pose"]
+    assert cp["position"] == new_pos
+    assert cp["rpy_deg"] == new_rpy
+
+
+def test_put_offset_updates_config():
+    r = client.put("/api/right_arm/offset",
+                   json={"move_offset_camera_mm": {"x": 5, "y": -3, "z": 75},
+                         "axis_sign_cam_to_wrist": {"x": -1, "y": 1, "z": -1}})
+    assert r.status_code == 200
+    r2 = client.get("/api/config")
+    body = r2.json()["tab_right_arm"]
+    assert body["move_offset_camera_mm"] == {"x": 5, "y": -3, "z": 75}
+    assert body["axis_sign_cam_to_wrist"] == {"x": -1, "y": 1, "z": -1}