diff --git a/src/presa_mano/api/right_arm_routes.py b/src/presa_mano/api/right_arm_routes.py index 4b869cc..e8837a9 100644 --- a/src/presa_mano/api/right_arm_routes.py +++ b/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")} diff --git a/tests/conftest.py b/tests/conftest.py index c675284..67cf78c 100644 --- a/tests/conftest.py +++ b/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 diff --git a/tests/test_right_arm_routes.py b/tests/test_right_arm_routes.py index b7bec1c..7ba96ed 100644 --- a/tests/test_right_arm_routes.py +++ b/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}