commit cd2fb4602ad724108e7c3b4f9b118b862801012e Author: Eiichi Shimazu Date: Thu Nov 5 10:42:07 2026 +0000 run_policy_client: 推論のあとの確認に p (ゲームパッドで補正) を足す. gamepad_teleop を同じプロセスで動かし □ で推論に戻る (LOG450) Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01LevEXVQCng1HbyW9QDciA6 diff --git a/pi05/run_policy_client.py b/pi05/run_policy_client.py index 599b090..f233c69 100644 --- a/pi05/run_policy_client.py +++ b/pi05/run_policy_client.py @@ -61,6 +61,13 @@ お手本と同じ形). 1 コマごとに状態 (model / rewind / correct / home / wait) を付ける. wait = 推論のあとの確認で入力を待っている間 (人が手でブロックを置き直すので学習データに使わない) h / q で終えるときに 結果 (y / n / Enter=記録しない) を聞き, {run_stem}.json に残す + - p = ゲームパッドで補正 (2026-11-05 LOG450). tools/gamepad_teleop.py と同じ操作・送り方 (1 関節だけ送る 1 刻み・ + 15° 先へ 1 回送る長押し・離したら stop) のウィンドウが開き, □ で推論に戻る. 推論と同じプロセスで動かし, + daemon のつながりと鍵を共有する (daemon は 1 本ずつしか受けない). 守りは gamepad_teleop と同じ + (物理可動域・J4_SAFE・j1 ≤ 90.87. 学習データの範囲ではない). 範囲の外で終えたら, 次の推論で止まりうると出す. + 間の状態は correct (学習データに使う). 爪の指令は送る瞬間に記録へ入れる. 押した操作は log.jsonl に + {"type": "pad", ...} で 1 つずつ, 終わりに {"type": "correction", "action": "accept", "device": "gamepad"}. + b で巻き戻せる (送った目標を逆にたどる. 爪を動かした補正は今までどおり断る) - 推論のあとの確認 (2026-10-31 依頼 40): 実行するのは何も打たずに Enter を押したときだけ. 知らない入力 (R・c] ・空白だけ など) は実行せずに聞き直す. r は補正の中と同じく爪を締め直す (閉じているときだけ. チャンクは実行しない. log.jsonl に type=confirm / action=regrip_tighten). @@ -891,6 +898,111 @@ def run_correction(arm, keys, lo, hi, max_move, grip, corr_id, chunk, log_f, rec return res, grip +class PadLog: + """ゲームパッドの補正 (p) の記録: gamepad_teleop の 1 行を log.jsonl に {"type": "pad", ...} で書き, 手元にも残す.""" + + def __init__(self, log_f, corr_id, chunk): + self.log_f, self.corr_id, self.chunk = log_f, corr_id, chunk + self.rows = [] + + def write(self, rec): + row = {"type": "pad", "correction_id": self.corr_id, "after_chunk": self.chunk} + row.update(rec) + self.rows.append(row) + self.log_f.write(json.dumps(row, ensure_ascii=False, default=float) + "\n") + self.log_f.flush() + + def close(self): + pass + + +def _load_gamepad_teleop(): + """tools/gamepad_teleop.py を読む (p を押したときだけ. pygame を使うので). 読めなければ None.""" + # gamepad_teleop は run_policy_client を import して値と送り口を使う. 別の写しを読まないよう, 今動いているこの + # モジュールを同じ名前で登録する (--grip-wait などで変えた待ちを補正でも使う) + sys.modules.setdefault("run_policy_client", sys.modules[__name__]) + tools = str(_ROOT / "tools") + if tools not in sys.path: + sys.path.insert(0, tools) + try: + import gamepad_teleop as gt + except Exception as ex: + print(f"[PAD ] gamepad_teleop を読めません ({type(ex).__name__}: {ex}). c (キー) で補正してください") + return None + return gt + + +def run_pad_correction(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): + """p = ゲームパッドで補正 (2026-11-05 LOG450). 戻り値は run_correction と同じ (結果 dict, 補正後のグリッパー状態). + 結果の action はいつも accept (取り消しは, 推論に戻ってから b で巻き戻す).""" + gt = _load_gamepad_teleop() + if gt is None: + return None, grip + start_q = arm.read() + if start_q is None: + print("[PAD ] 関節角を読めないので補正に入れません") + return None, grip + start_q = np.asarray(start_q, dtype=float)[:6].copy() + if arm.execute: + gt.load_daemon_modules() + + class PadArm(gt.DaemonArm): + """爪の指令を送る瞬間に記録へ入れる (build_correction_episodes は指令の時刻で 0/1 を切り替える).""" + + def gripper(self, state, gate): + def _send(): + if recorder is not None: + recorder.gripper(state) + if state == 1: + daemon_set_gripper_value(GRIP_VALUE) + else: + daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT) + sent = gate.run(_send) + if sent: + time.sleep(GRIPPER_SLEEP) + return sent + pad_arm = PadArm() + else: + pad_arm = gt.DryArm(start_q) + plog = PadLog(log_f, corr_id, chunk) + if recorder is not None: + recorder.set_mode(MODE_CORRECT) + t_start = time.time() + print(f"[PAD ] 補正 {corr_id} (推論 {chunk} のあと). ゲームパッドで直して, □ で推論に戻ります") + title = f"PAD CORRECTION {corr_id} [{'EXECUTE' if arm.execute else 'DRY'}] square: back to policy" + end_reason, tp, worker_done = gt.run_embedded(pad_arm, start_q, plog, grip=grip, mode_text=title) + t_end = time.time() + if recorder is not None: + recorder.set_mode(MODE_MODEL) + if not arm.execute: + arm.sim_q = np.asarray(pad_arm.q, dtype=float).copy() + sent = [np.asarray(r["command"], dtype=float) for r in plog.rows + if r.get("kind") in ("move", "home") and r.get("sent") is True and r.get("command") is not None] + grip_rows = [r for r in plog.rows if r.get("kind") in ("grip_close", "grip_open") and r.get("sent") is True] + new_grip = int(grip_rows[-1]["grip_cmd"]) if grip_rows else grip + qa = arm.read() + res = {"type": "correction", "action": "accept", "device": "gamepad", "correction_id": corr_id, + "after_chunk": chunk, "t_start": t_start, "t_end": t_end, "duration_s": round(t_end - t_start, 2), + "start_pose": _fmt6(start_q), "final_target": _fmt6(sent[-1]) if sent else _fmt6(start_q), + "measured_after": _fmt6(qa), "steps_sent": len(sent), "pad_rows": len(plog.rows), + "grip_before": grip, "grip_after": new_grip, "grip_used": bool(grip_rows), "regrip_used": False, + "sent_targets": [_fmt6(x) for x in sent], "pad_end_reason": end_reason, + "pad_stopped": bool(tp.stopped), "pad_stop_reason": tp.stop_reason, "pad_worker_done": bool(worker_done), + "pad_ignored": sum(1 for r in plog.rows if r.get("accepted") is False)} + print(f"[PAD ] 補正 {corr_id} を終えました ({t_end - t_start:.1f} 秒, 送った目標 {len(sent)} 回" + + (", 爪を動かした" if grip_rows else "") + "). この姿勢から推論に戻ります") + if qa is not None: + out = [k for k in range(6) if not (lo[k] - 0.5 <= qa[k] <= hi[k] + 0.5)] + if out: + res["outside_envelope"] = out + print("[PAD ] [注意] 学習データの範囲の外にいます: " + + ", ".join(f"j{k}={qa[k]:+.1f} (範囲 {lo[k]:+.0f}〜{hi[k]:+.0f})" for k in out) + + ". 次の推論は範囲の端へ切られるか, 止まります") + if end_reason == "ctrl_c": + res["ctrl_c"] = True + return res, new_grip + + def write_trial_json(run_dir, run_stem, args, recorded, end_reason, outcome, history_log, n_rewinds, n_corrections, exit_code): """{run_stem}.json. お手本の試行 JSON と同じキー (recording_enabled / result / judgments) を持つ. @@ -928,9 +1040,9 @@ def flush_stdin(): print(f" [WARN] たまった入力を捨てられませんでした ({ex})") -CONFIRM_PROMPT = (" Enter=実行 / s=送らず次へ / b=巻き戻し / c=補正 / r=爪の締め直し / " - "h=HOMEに戻して終了 / q=終了 > ") -CONFIRM_KEYS = ("s", "b", "c", "r", "h", "q") +CONFIRM_PROMPT = (" Enter=実行 / s=送らず次へ / b=巻き戻し / p=ゲームパッドで補正 / c=キーで補正 / " + "r=爪の締め直し / h=HOMEに戻して終了 / q=終了 > ") +CONFIRM_KEYS = ("s", "b", "c", "p", "r", "h", "q") def ask_outcome(): @@ -1352,13 +1464,17 @@ def main(): if recorder is not None: recorder.set_mode(MODE_MODEL) continue - if ans == "c": + if ans in ("c", "p"): rec["correction_requested"] = True + rec["correction_device"] = "gamepad" if ans == "p" else "keys" log_f.write(json.dumps(rec) + "\n"); log_f.flush() n_corrections += 1 - with KeyReader() as keys: - cr, grip = run_correction(arm, keys, lo, hi, args.max_move_deg, grip, - n_corrections, i, log_f, recorder) + if ans == "p": + cr, grip = run_pad_correction(arm, grip, n_corrections, i, log_f, recorder, lo, hi) + else: + with KeyReader() as keys: + cr, grip = run_correction(arm, keys, lo, hi, args.max_move_deg, grip, + n_corrections, i, log_f, recorder) if cr is not None: log_f.write(json.dumps(cr) + "\n"); log_f.flush() if cr["grip_used"] or cr.get("regrip_used"): @@ -1379,6 +1495,8 @@ def main(): "grip_changed": cr["grip_used"], "regrip": cr["regrip_used"], "t_start": cr["t_start"], "t_end": cr["t_end"]}) + if cr.get("ctrl_c"): + raise KeyboardInterrupt continue if not unsafe and args.dry_keys: # --dry-keys の Enter: 送らずに仮の姿勢を目標へ移す (b / c の流れを試すため) diff --git a/pi05/test_pad_correction.py b/pi05/test_pad_correction.py new file mode 100644 index 0000000..54b132a --- /dev/null +++ b/pi05/test_pad_correction.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +"""test_pad_correction.py — run_policy_client.py の p (ゲームパッドで補正. 2026-11-05 LOG450) の確かめ. + +本物の pygame のループ (SDL_VIDEODRIVER=dummy) に偽のコントローラーの押しを流し, 補正の結果の記録・ +爪の記録・巻き戻し用の目標・ドライランの仮の姿勢・実機の送り口 (daemon を偽物にする) を見る. + SDL_VIDEODRIVER=dummy SDL_AUDIODRIVER=dummy python3 pi05/test_pad_correction.py +""" +import functools +import io +import json +import os +import sys +import unittest +from pathlib import Path +from unittest import mock + +os.environ.setdefault("SDL_VIDEODRIVER", "dummy") +os.environ.setdefault("SDL_AUDIODRIVER", "dummy") +_HERE = Path(__file__).resolve().parent +sys.path.insert(0, str(_HERE)) +sys.path.insert(0, str(_HERE.parent / "tools")) + +import numpy as np # noqa: E402 + +import run_policy_client as rpc # noqa: E402 +import gamepad_teleop as gt # noqa: E402 + + +class FakeJoystick: + """test_gamepad_teleop.FakeJoystick と同じ (あちらは読み込むと刻みを 0.5° に書き換えるので写した).""" + + def __init__(self, idx): + self.axes = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0] + + def get_instance_id(self): + return 7 + + def get_name(self): + return "PS4 Controller" + + def get_guid(self): + return "fake-guid" + + def get_numaxes(self): + return 6 + + def get_numbuttons(self): + return 16 + + def get_numhats(self): + return 0 + + def get_axis(self, a): + return self.axes[a] + + def get_button(self, b): + return 0 + +HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] + + +class FakeRecorder: + def __init__(self): + self.events = [] + + def set_mode(self, m): + self.events.append(("mode", m)) + + def gripper(self, s): + self.events.append(("gripper", s)) + + +def script_frames(script): + import pygame + + def btn(b, down=True): + t = pygame.JOYBUTTONDOWN if down else pygame.JOYBUTTONUP + pygame.event.post(pygame.event.Event(t, instance_id=7, button=b, joy=0)) + + def on_frame(n, tp): + if n == 1: + pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)) + for b in script.get(n, ()): + btn(b); btn(b, False) + if n == 600: + pygame.event.post(pygame.event.Event(pygame.QUIT)) + return on_frame + + +LO = np.array([-60.0, -10, -10, -60, -60, -60]) +HI = np.array([60.0, 60, 60, 60, 60, 0]) + + +class TestPadDry(unittest.TestCase): + def test_dry_flow(self): + arm = rpc.ArmIO(False, None, sim_q=HOME) + rec = FakeRecorder() + log_f = io.StringIO() + on_frame = script_frames({3: [gt.BTN_RIGHT], 40: [gt.BTN_RIGHT], 80: [gt.BTN_CROSS], 140: [gt.BTN_SQUARE]}) + orig = gt.run_embedded + with mock.patch.object(gt, "run_embedded", functools.partial(orig, on_frame=on_frame)), \ + mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch.object(gt, "GRIPPER_SLEEP", 0.0): + res, grip = rpc.run_pad_correction(arm, 0, 1, 4, log_f, rec, LO, HI) + self.assertEqual(res["action"], "accept") + self.assertEqual(res["device"], "gamepad") + self.assertEqual(res["pad_end_reason"], "finish_button") + self.assertEqual(res["steps_sent"], 2) + self.assertEqual(res["sent_targets"][-1][0], 2.0) + self.assertTrue(res["grip_used"]) + self.assertEqual((res["grip_before"], res["grip_after"], grip), (0, 1, 1)) + self.assertAlmostEqual(arm.sim_q[0], 2.0) # 推論に戻る姿勢 = 直した姿勢 + self.assertEqual(rec.events[0], ("mode", rpc.MODE_CORRECT)) + self.assertEqual(rec.events[-1], ("mode", rpc.MODE_MODEL)) + rows = [json.loads(x) for x in log_f.getvalue().splitlines()] + self.assertTrue(rows and all(r["type"] == "pad" and r["correction_id"] == 1 and r["after_chunk"] == 4 + for r in rows)) + self.assertGreaterEqual(res["duration_s"], 0.0) + self.assertNotIn("outside_envelope", res) + + def test_outside_envelope_warns(self): + arm = rpc.ArmIO(False, None, sim_q=[59.5, 0, 0, 0, 0, -30]) + on_frame = script_frames({3: [gt.BTN_RIGHT], 60: [gt.BTN_RIGHT], 120: [gt.BTN_SQUARE]}) + orig = gt.run_embedded + out = [] + with mock.patch.object(gt, "run_embedded", functools.partial(orig, on_frame=on_frame)), \ + mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch("builtins.print", lambda *a, **k: out.append(" ".join(map(str, a)))): + res, _ = rpc.run_pad_correction(arm, 0, 2, 0, io.StringIO(), None, LO, HI) + self.assertEqual(res["outside_envelope"], [0]) + self.assertTrue(any("学習データの範囲の外" in x for x in out)) + + +class TestPadExecute(unittest.TestCase): + """実機の送り口: daemon を偽物にして, 1 関節の send_angle と, 爪の指令の瞬間の記録を見る.""" + + def test_execute_sends_and_records_gripper(self): + q = {"now": np.array(HOME, dtype=float)} + calls = [] + + def req(c, **k): + calls.append(c) + if c["cmd"] == "send_angle": + q["now"][c["joint_id"] - 1] = c["angle"] + return {"ok": True} + if c["cmd"] == "get_angles": + return {"ok": True, "angles": list(q["now"])} + return {"ok": True, "result": None} + + rec = FakeRecorder() + arm = rpc.ArmIO(True, rec) + on_frame = script_frames({3: [gt.BTN_LEFT], 60: [gt.BTN_CROSS], 120: [gt.BTN_SQUARE]}) + orig = gt.run_embedded + gt.load_daemon_modules() + self.assertIs(gt.rpc, sys.modules["run_policy_client"]) + with mock.patch.object(gt, "run_embedded", functools.partial(orig, on_frame=on_frame)), \ + mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch.object(rpc, "read_angles", lambda: q["now"].copy()), \ + mock.patch.object(gt, "daemon_request", req), \ + mock.patch.object(rpc, "daemon_set_gripper_value", lambda v: calls.append({"cmd": "grip", "v": v})), \ + mock.patch.object(rpc, "GRIPPER_SLEEP", 0.0): + res, grip = rpc.run_pad_correction(arm, 0, 3, 7, io.StringIO(), rec, LO, HI) + self.assertEqual(res["pad_end_reason"], "finish_button") + self.assertIn({"cmd": "send_angle", "joint_id": 1, "angle": -1.0, "speed": gt.SPEED, "wait": rpc.SEND_WAIT}, + calls) + self.assertIn({"cmd": "grip", "v": rpc.GRIP_VALUE}, calls) + self.assertIn(("gripper", 1), rec.events) + self.assertEqual(grip, 1) + self.assertEqual(res["sent_targets"], [[-1.0, 0.0, 0.0, 0.0, 0.0, -30.0]]) + + +class TestConfirmP(unittest.TestCase): + """確認で p を押すと補正に入り, 終えたあとの b でその補正を巻き戻せる (アームなし --dry-keys).""" + + def test_p_then_b(self): + import tempfile + import time + from test_rewind_correct import run_client + + def fake_pad(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): + start = arm.read()[:6].copy() + t0 = time.time() + tgt = start.copy(); tgt[0] += 3.0 + arm.sim_q = tgt.copy() + return ({"type": "correction", "action": "accept", "device": "gamepad", "correction_id": corr_id, + "after_chunk": chunk, "t_start": t0, "t_end": time.time(), "start_pose": rpc._fmt6(start), + "final_target": rpc._fmt6(tgt), "measured_after": rpc._fmt6(tgt), "steps_sent": 1, + "grip_before": grip, "grip_after": grip, "grip_used": False, "regrip_used": False, + "sent_targets": [rpc._fmt6(tgt)]}, grip) + td = Path(tempfile.mkdtemp()) + with mock.patch.object(rpc, "run_pad_correction", fake_pad): + code, world, run, lines, out = run_client( + rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "4"], ["", "p", "b", "q"], td) + self.assertEqual(code, 0) + req = [x for x in lines if x.get("correction_requested")] + self.assertEqual([x["correction_device"] for x in req], ["gamepad"]) + rw = [x for x in lines if x.get("type") == "rewind"] + self.assertEqual([(x["kind"], x["correction_id"]) for x in rw], [("correction", 1)]) + self.assertIn("p=ゲームパッドで補正", rpc.CONFIRM_PROMPT) + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/gamepad_teleop.py b/tools/gamepad_teleop.py index e987267..158614f 100644 --- a/tools/gamepad_teleop.py +++ b/tools/gamepad_teleop.py @@ -1386,8 +1386,11 @@ def controller_info(js): "connected_at": datetime.now().isoformat(timespec="seconds")} -def run_loop(tp, screen, fonts, mode_text, on_controller=None, on_frame=None): - """ウィンドウを閉じるまで回す. 戻り値: 終わった理由.""" +def run_loop(tp, screen, fonts, mode_text, on_controller=None, on_frame=None, finish_buttons=None): + """ウィンドウを閉じるまで回す. 戻り値: 終わった理由. + finish_buttons (run_policy_client の補正 p から. 2026-11-05 LOG450): このボタンを押したら, 止まっていれば + "finish_button" で戻る (推論に戻る). 動いている間 (送っている・長押し・止める待ち) は断って続ける. + このボタンは Teleop には渡さない.""" js = None clock = pygame.time.Clock() frame = 0 @@ -1425,6 +1428,14 @@ def run_loop(tp, screen, fonts, mode_text, on_controller=None, on_frame=None): except Exception as ex: # pygame.error など read_ok = False tp.controller_read_failed(f"{type(ex).__name__}: {ex}") + if finish_buttons and read_ok: + for ev in buttons: + if ev.type == pygame.JOYBUTTONDOWN and ev.button in finish_buttons: + if tp.busy or tp.hold is not None or tp.halt_pending is not None: + tp.out("[補正] 動いている間は終われません。止まってから □ を押してください") + else: + return "finish_button" + buttons = [ev for ev in buttons if ev.button not in finish_buttons] if read_ok: for ev in buttons: if ev.type == pygame.JOYBUTTONDOWN: @@ -1473,6 +1484,42 @@ def draw(screen, fonts, tp, mode_text, js): # main # ============================================================================= +def make_fonts(): + mono = "menlo,consolas,dejavusansmono" + return {"big": pygame.font.SysFont(mono, 34, bold=True), "mono": pygame.font.SysFont(mono, 20), + "small": pygame.font.SysFont(mono, 16)} + + +def run_embedded(arm, start, log, grip=None, mode_text="PAD CORRECTION", finish_buttons=(BTN_SQUARE,), out=print, + on_frame=None): + """run_policy_client.py の推論のあとの確認で p を押したときに呼ぶ (2026-11-05 LOG450). + このファイルの main と同じ Teleop と run_loop を, 推論のプロセスの中で動かす (daemon のつながりと鍵は + 推論と共有する. daemon は 1 本ずつしか受けないので, 別のプロセスでは動かせない). + □ (finish_buttons) で推論に戻る. ウィンドウを閉じても戻る. 操作・送り方・守りは main と同じ. + arm は DaemonArm (かその子) か DryArm. log は write(dict) / close() を持つもの. + grip: 推論の側の爪の状態 (1 = 閉 / 0 = 開). 画面の GRIP に出すだけ. + 戻り値: (終わった理由, Teleop, 送る仕事が終わっているか).""" + tp = Teleop(arm, start, log, out=out) + if grip is not None: + tp.grip = "closed" if int(grip) == 1 else "open" + end_reason = None + worker_done = True + pygame.init() + pygame.joystick.init() + try: + screen = pygame.display.set_mode(WINDOW_SIZE) + pygame.display.set_caption(mode_text) + out("コントローラーを待っています（つないでいなければ、今つないでください）。□ で推論に戻ります") + end_reason = run_loop(tp, screen, make_fonts(), mode_text, finish_buttons=finish_buttons, + on_frame=on_frame) + except KeyboardInterrupt: + end_reason = "ctrl_c" + finally: + worker_done = tp.shutdown() + pygame.quit() + return end_reason, tp, worker_done + + def git_info(path): def run(*a): try: diff --git a/tools/test_gamepad_teleop.py b/tools/test_gamepad_teleop.py index cc07f07..4c44327 100644 --- a/tools/test_gamepad_teleop.py +++ b/tools/test_gamepad_teleop.py @@ -1403,6 +1403,57 @@ class TestDryRunMain(unittest.TestCase): self.assertTrue(all(r["execute"] is False for r in rows)) +class TestEmbeddedFinish(unittest.TestCase): + """run_policy_client の p から呼ぶ run_embedded (2026-11-05 LOG450): □ で戻る. 動いている間の □ は断る.""" + + def test_square_finishes_only_when_idle(self): + import pygame + + def btn(b, down=True): + t = pygame.JOYBUTTONDOWN if down else pygame.JOYBUTTONUP + pygame.event.post(pygame.event.Event(t, instance_id=7, button=b, joy=0)) + + seen = {} + + def on_frame(n, tp): + if n == 1: + pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)) + elif n == 3: + btn(gt.BTN_RIGHT); btn(gt.BTN_RIGHT, False) + elif n == 5: + seen["busy_at_square"] = tp.busy + btn(gt.BTN_SQUARE); btn(gt.BTN_SQUARE, False) + elif n == 80: + btn(gt.BTN_SQUARE); btn(gt.BTN_SQUARE, False) + elif n == 400: + pygame.event.post(pygame.event.Event(pygame.QUIT)) + + rows, out = [], [] + + class L: + def write(self, r): + json.dumps(r, ensure_ascii=False) + rows.append(r) + + def close(self): + pass + + arm = gt.DryArm(HOME) + with mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch.object(gt, "GRIPPER_SLEEP", 0.0): + reason, tp, done = gt.run_embedded(arm, np.array(HOME, dtype=float), L(), grip=1, + out=out.append, on_frame=on_frame) + self.assertEqual(reason, "finish_button") + self.assertTrue(done) + self.assertTrue(seen["busy_at_square"]) + self.assertTrue(any("動いている間は終われません" in x for x in out)) + self.assertEqual(tp.grip, "closed") + sent = [r for r in rows if r["kind"] == "move" and r.get("sent") is True] + self.assertEqual(len(sent), 1) + self.assertNotIn("square", [r.get("button_name") for r in rows]) # □ は Teleop に渡さない + self.assertAlmostEqual(arm.q[0], HOME[0] + gt.STEP_FINE_DEG) + + class TestExecuteMainRecords(unittest.TestCase): """--execute は今までどおり KIOXIA (ここでは一時フォルダ) に log.jsonl・meta.json・写しを書く."""