#!/usr/bin/env python3 """gamepad_teleop.py — ゲームパッドで mechArm 270 を手で動かす (依頼 68. 2026-11-02) PICK で爪の位置がずれたときに, エイイチがゲームパッドで左右・上げ下げ・爪の開閉を直す. 押した分だけそのまま動く. 自動で動く要素は入れない (自動の握り直し・失敗時の自動 HOME などはない). 押した操作と, 送った角度・届いた角度をすべて log.jsonl に残す (あとで「人が直した記録」として使う). 操作 (エイイチが決めた. 変えない): 十字キー 左 / 右 (13 / 14) j0 を 1° ずつ (2026-11-03 に 0.5° から変えた. 0.5° ではサーボが反応しなかった). どの関節を選んでいても いつも j0 (向き: 左 = J0_LEFT_SIGN. 依頼に向きの指定がないので, 右 = +, 左 = − にしてある) 十字キー 上 / 下 (11 / 12) 選んでいる関節を 1° ずつ. 上 = −, 下 = +. 始めは j1 十字キーの長押し 押している間, 止めずに動かし続ける (2026-11-03). 短く押せば 1 刻み. 押して HOLD_START_SEC (0.4 秒) たっても押していれば, HOLD_INTERVAL_SEC (0.1 秒) おきに 「いま読んだ角度から HOLD_AHEAD_DEG (3°) 先」を送り直す. アームは目標に着く前に次の目標を 受け取るので止まらず, speed 20 の速さで動き続ける (刻みごとに着いて止まるとぎこちなかった). 長押しの送信では届いたかを待たない. 先へ出るのは最大 3° (離すと, そこまで動いて止まる). R1 は短い押しの刻み (2°) にだけ効く. 長押しの速さは speed で決まる (20 のまま). 離したら, 送ると決める前の送信は取り消す. 押し始めに受け付けられなかった押し (動いている間・停止中) は, 押し続けても長押しにしない. 離した合図を取りこぼしても止まるよう, 毎コマ コントローラーのボタンの状態も見る. HOLD_STALL_SEC (1 秒) のあいだ動かした関節が進んでいなければ, その長押しは止める (当たっているなど). 離す・L2+R2・範囲の端・△・ほかのボタン・接続が切れる で長押しは止まる △ (3) 上下で動かす関節を切り替える: j1 → j2 → j3 → j4 → j5 → j1 R1 (10) を押しながら 1 回 2° ずつ (十字キーの左右・上下とも) × (0) / ○ (1) 爪を閉じる / 開く HOME (5) を 1 秒長押し HOME [0, 0, 0, 0, 0, -30] に戻る. 1 秒続けて押したときに 1 回だけ送る L2 と R2 (軸 4・5) の両方が 0.0 を超えたとき 停止. 以後の動く操作を受け付けない. OPTIONS (6) で再開 L2+R2 は新しい指令を送らないだけで, 送った 1 回分 (HOME を含む) は最後まで動く ほかのボタン (□・SHARE・L1・L3・R3・タッチパッド・スティック) は何もしない 守り: - j1 は 90.87 (arm_kinematics.md の PICK_J1_FIXED) を超える指令を送らない (fk_lookup.J1_SAFE_MAX = 88.0 は別の用途の値なので, ここでは使わない) - 各関節は物理可動域 (wr.JOINT_LIMITS) まで. j4 は J4_SAFE (−90〜+60) (基準の姿勢から j0 ±15°・j1〜j5 ±10° の枠は 2026-11-03 にやめた. HOME から手でブロックの上まで運ぶため) - 範囲で切られたら理由を画面とログに出す. 別の関節で補わない - HOME だけは範囲の検査を通さずに送れる (run_policy_client.py の h と同じ扱い) 送り方: - send_angles だけ (send_coords は使わない). speed 20 (j0_sweep_scan.SPEED) - 1 回押すごとに目標を 1 つ作って送り, 3° 以内に届いたかを読んで確かめる. 送った指令が無視されることがある (どの関節も動かない. 2026-11-03 の実機で 3 回, 8 秒止まった. waypoint_replay.py の「単発は無視されるが, 打ち直せば通る」と同じ). 送ってから IGNORED_CHECK_SEC (0.5 秒) たっても, 届いておらず, どの関節も IGNORED_MOVE_DEG (0.3°) 動いていなければ, 同じ目標を 送り直す (IGNORED_MAX_RESENDS 回まで). 送り直しも SendGate の中で行うので, 停止のあとは送らない - 送ってから届くまで (または待ちの上限まで) と, 爪のあとの GRIPPER_SLEEP の間は 次の動く操作を受け付けない (ためない). 停止はいつでも受け付ける (送る仕事は別のスレッドで行い, 画面とコントローラーの読み取りは止めない) - 停止が入ったあとは新しい指令を送らない. **動いている途中の動きは止められない** (daemon に止める命令がない. 依頼 68 の報告を参照) 1 回の出発点: 直前に送った目標に届いていて, 送る前に読んだ角度とも 3° 以内なら その目標. そうでなければ 読んだ角度. (j1 の重さによる 2〜3° の不足があっても, 0.5° の刻みが読んだ値に引きずられないように. 手で動かされたなど 3° を超えて離れていたら, 古い目標から大きく動かさない) 出発点の 6 関節のどれかが範囲の外なら送らない (範囲の端へ切って 1 回で大きく動かさない). 停止と送信の順番 (codex R1): 「送ると決める」と「停止・終了の取り消し」を 仕事ごとの鍵 (SendGate) で並べる (codex R1・R2). 停止のあとに送ると決める指令はない. 停止は待たされない (画面も止まらない). 停止より前に決めた送信は 通信の上限 SEND_RPC_TIMEOUT_SEC (8 秒) の中で終わる. 止める前に作った仕事は OPTIONS で再開しても生き返らない. 1 コマの中では L2+R2 と読み取りの失敗を ボタンより先に見る. 送ったあと確かめる途中で関節角を 読めなければ停止にする. log.jsonl に書けなくなったら停止し, 再開させない. 終わり方: Ctrl+C / ウィンドウを閉じる. 新しい指令は送らずに終わる. アームは動かさない (HOME 復帰もサーボ解放もしない. run_policy_client.py の finally と同じ) Usage: python3 tools/gamepad_teleop.py # ドライラン (daemon にもアームにも触らない. HOME から始まる仮の姿勢) python3 tools/gamepad_teleop.py --execute # daemon に送る 記録 (--execute のときだけ): /Volumes/KIOXIA/r2r2r-data/runs/experiments/gamepad_teleop/{run_stem}/ log.jsonl (押した操作 1 つにつき 1 行) / meta.json / gamepad_teleop.py (使ったスクリプトの写し) 通しの完走ではないので台帳には書かない. ドライランは記録しない (KIOXIA に何も書かない・フォルダも作らない. KIOXIA がなくても動く. 依頼 69) 読み込み (依頼 69): run_policy_client・j0_sweep_scan・visual_servo_pick は cv2 を読み, cv2 は自分の libSDL2 (site-packages/cv2/.dylibs/) を持っている. pygame の libSDL2 と 2 つ入ると objc の 「Class SDLApplication is implemented in both ...」が十数行出る. なのでこの 3 つは --execute のときだけ load_daemon_modules() で読む (ドライランでは cv2 も daemon の送り口も読まない). ドライランでも要る値 (HOME・J4_SAFE・GRIPPER_SLEEP・物理可動域) は, 読み込まずに元のファイルの 代入から読む (二重に書かない). --execute で読んだあと, 読み込んだ値と同じかを確かめる. --execute では cv2 を読むので 警告は今までどおり出る (動きには影響なし). """ import argparse import ast import hashlib import json import os import platform import queue import shutil import subprocess import sys import threading import time from datetime import datetime from pathlib import Path # ウィンドウが手前にないときもコントローラーの入力を受け取る (gamepad_check.py と同じ) os.environ.setdefault("SDL_JOYSTICK_ALLOW_BACKGROUND_EVENTS", "1") os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1") import numpy as np # noqa: E402 import pygame # noqa: E402 _HERE = Path(__file__).resolve().parent _ROOT = _HERE.parent sys.path.insert(0, str(_ROOT / "pi05")) _RPC_PATH = _ROOT / "pi05" / "run_policy_client.py" _WR_PATH = _ROOT / "dynamic_tracking_limits" / "drag_teaching" / "waypoint_replay.py" def read_constants(path, names): """path を読み込まずに (実行せずに), 一番外の `名前 = 値` の値を読む. 1 回だけ代入されている名前に限る. ファイルのどこかで ほかの形 (+= ・型注釈つき・連鎖・for・global の中など) で同じ名前に代入していれば止める (--execute では読み込んだ値とも比べる).""" found = {} tree = ast.parse(Path(path).read_text(encoding="utf-8")) simple = set() for node in tree.body: if isinstance(node, ast.Assign) and len(node.targets) == 1 and isinstance(node.targets[0], ast.Name): name = node.targets[0].id if name in names: if name in found: raise RuntimeError(f"{path} で {name} が 2 回代入されています") found[name] = ast.literal_eval(node.value) simple.add(id(node.targets[0])) for node in ast.walk(tree): if (isinstance(node, ast.Name) and node.id in names and isinstance(node.ctx, (ast.Store, ast.Del)) and id(node) not in simple): raise RuntimeError(f"{path} で {node.id} がほかの形でも代入されています (行 {node.lineno})") missing = [n for n in names if n not in found] if missing: raise RuntimeError(f"{path} に {missing} が見つかりません") return found # 爪 (gripper_cmd)・HOME・J4_SAFE・関節角の読み方 (read_angles) は run_policy_client と同じものを使う. # daemon の送り口と run_policy_client は --execute のときだけ load_daemon_modules() で読む (cv2 の SDL を避ける) _RPC_CONST = read_constants(_RPC_PATH, ("HOME", "J4_SAFE", "GRIPPER_SLEEP")) _WR_CONST = read_constants(_WR_PATH, ("JOINT_LIMITS",)) rpc = None wr = None SPEED = POLL_SLEEP = REACH_TIMEOUT_SEC = None # j0_sweep_scan の 20 / 0.3 / 8.0 (--execute で読む) close_daemon_socket = daemon_send_angles = daemon_set_gripper = daemon_set_gripper_value = get_daemon_socket = None def load_daemon_modules(): """--execute のときだけ呼ぶ. run_policy_client (パスと pymycobot のスタブも入る)・j0_sweep_scan・ visual_servo_pick の daemon の送り口を読む. 2 回目からは何もしない. 読み込まずに読んだ値 (HOME・J4_SAFE・GRIPPER_SLEEP・JOINT_LIMITS) と違えば止める.""" global rpc, wr, SPEED, POLL_SLEEP, REACH_TIMEOUT_SEC global close_daemon_socket, daemon_send_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket if rpc is not None: return import run_policy_client as _rpc import j0_sweep_scan as _j0 import visual_servo_pick as _vsp pairs = (("HOME", _rpc.HOME, HOME), ("J4_SAFE", _rpc.J4_SAFE, J4_SAFE), ("GRIPPER_SLEEP", _rpc.GRIPPER_SLEEP, GRIPPER_SLEEP), ("JOINT_LIMITS", _rpc.wr.JOINT_LIMITS, JOINT_LIMITS)) diff = [n for n, loaded, read in pairs if _norm(loaded) != _norm(read)] if diff: raise RuntimeError(f"読み込んだ値がファイルから読んだ値と違います: {diff}") SPEED, POLL_SLEEP, REACH_TIMEOUT_SEC = _j0.SPEED, _j0.POLL_SLEEP, _j0.REACH_TIMEOUT_SEC close_daemon_socket, daemon_send_angles = _vsp.close_daemon_socket, _vsp.daemon_send_angles daemon_set_gripper, daemon_set_gripper_value = _vsp.daemon_set_gripper, _vsp.daemon_set_gripper_value get_daemon_socket = _vsp.get_daemon_socket wr = _rpc.wr rpc = _rpc def _norm(v): if isinstance(v, (list, tuple)): return [_norm(x) for x in v] return float(v) # ---- ボタンと軸 (/Volumes/KIOXIA/r2r2r-data/diag/gamepad_check_20261102/ で確かめた番号) ---- BTN_CROSS = 0 BTN_CIRCLE = 1 BTN_SQUARE = 2 BTN_TRIANGLE = 3 BTN_SHARE = 4 BTN_HOME = 5 BTN_OPTIONS = 6 BTN_L3 = 7 BTN_R3 = 8 BTN_L1 = 9 BTN_R1 = 10 BTN_UP = 11 BTN_DOWN = 12 BTN_LEFT = 13 BTN_RIGHT = 14 BTN_TOUCHPAD = 15 BUTTON_NAMES = { BTN_CROSS: "cross", BTN_CIRCLE: "circle", BTN_SQUARE: "square", BTN_TRIANGLE: "triangle", BTN_SHARE: "share", BTN_HOME: "home", BTN_OPTIONS: "options", BTN_L3: "l3", BTN_R3: "r3", BTN_L1: "l1", BTN_R1: "r1", BTN_UP: "dpad_up", BTN_DOWN: "dpad_down", BTN_LEFT: "dpad_left", BTN_RIGHT: "dpad_right", BTN_TOUCHPAD: "touchpad", } AXIS_L2 = 4 AXIS_R2 = 5 TRIGGER_STOP_ABOVE = 0.0 # L2 と R2 の両方がこれを超えたら停止 (離して -1.0, 押し込んで +1.0) # ---- 動かし方 ---- STEP_FINE_DEG = 1.0 # 2026-11-03: 0.5° はサーボが反応しなかった (読んだ角度が変わらない). 1° にした STEP_COARSE_DEG = 2.0 # R1 を押しながら J0_LEFT_SIGN = -1.0 # 十字キー左で j0 を動かす向き (右はその逆). 依頼に向きの指定がない UP_SIGN = -1.0 # 十字キー上 = −, 下 = + (PICK の上空 → 床で j1・j2・j4 が増える向き) SELECT_ORDER = (1, 2, 3, 4, 5) # △ で回す順. 始めは j1 HOME_HOLD_SEC = 1.0 HOME = [float(v) for v in _RPC_CONST["HOME"]] # [0, 0, 0, 0, 0, -30] # ---- 守り ---- J1_MAX_DEG = 90.87 # arm_kinematics.md の PICK_J1_FIXED (エイイチと決めた. J1_SAFE_MAX 88 は使わない) J4_SAFE = tuple(float(v) for v in _RPC_CONST["J4_SAFE"]) # (-90, 60) JOINT_LIMITS = [tuple(float(v) for v in lim) for lim in _WR_CONST["JOINT_LIMITS"]] # 物理可動域 (wr.JOINT_LIMITS) GRIPPER_SLEEP = float(_RPC_CONST["GRIPPER_SLEEP"]) # 爪のあとの待ち (rpc.GRIPPER_SLEEP. ドライランと画面の表示で使う) LIMIT_EPS_DEG = 1e-6 REACH_TOL_DEG = 3.0 # 届いたかの判定 (推論の到着確認と同じ 3°) # ---- 長押し (2026-11-03) ---- HOLD_START_SEC = 0.4 # 押してからこれだけたっても押していれば, 長押しとして送り続ける HOLD_INTERVAL_SEC = 0.1 # 長押しで目標を送り直す間隔 HOLD_AHEAD_DEG = 3.0 # 長押しの目標 = 読んだ角度からこれだけ先 (アームが着く前に次を送る. 離すとこれだけ先で止まる) HOLD_STALL_SEC = 1.0 # このあいだ動かした関節が HOLD_STALL_DEG 進んでいなければ, 長押しを止める HOLD_STALL_DEG = 0.3 DRY_HOLD_MAX_DEG = 1.5 # ドライランの長押しで 1 回に動かす量 (0.1 秒で 1.5° = 15°/秒 のつもり) # ---- 無視された指令の送り直し (2026-11-03) ---- IGNORED_CHECK_SEC = 0.5 # 送ってからこれだけたっても動いていなければ, 無視されたとみなす IGNORED_MOVE_DEG = 0.3 # どの関節もこれより動いていなければ「動いていない」 IGNORED_MAX_RESENDS = 3 # 送り直す回数の上限 DRY_MOVE_SEC = 0.3 # ドライランで「動いている」状態を見せる時間 SEND_RPC_TIMEOUT_SEC = 8.0 # send_angles 1 回の通信 (接続・ping・送信・返事) の上限. 通信が詰まった # 送信が 停止のずっとあとに届かないように (daemon の返事は送信後の読み直し込みで最長 3.7 秒. LOG352) WORKER_JOIN_SEC = 12.0 # 終わるとき, 送っている途中の仕事が戻るのを待つ上限 (読み取り 1 回は最長 3.7 秒) DATA_ROOT = Path("/Volumes/KIOXIA/r2r2r-data") LOG_ROOT = DATA_ROOT / "runs" / "experiments" / "gamepad_teleop" SSD_MOUNT = "/Volumes/KIOXIA" STATE_IDLE = "IDLE" STATE_MOVING = "MOVING" STATE_STOPPED = "STOPPED" def ensure_ssd_mounted(): if not os.path.ismount(SSD_MOUNT): sys.exit("[ABORT] KIOXIA がマウントされていません。記録をローカルには逃がさず、ここで止めます。") def _r(a): return None if a is None else [round(float(v), 2) for v in a] def _fmt(a): return "None" if a is None else " ".join(f"{float(v):+.2f}" for v in a) # ============================================================================= # 守り (純関数. テストで直接呼ぶ) # ============================================================================= def joint_window(): """各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る. 出どころ: PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ.""" lo, hi, lo_src, hi_src = [], [], [], [] for k in range(6): cands_lo = [(JOINT_LIMITS[k][0], "PHYS")] cands_hi = [(JOINT_LIMITS[k][1], "PHYS")] if k == 4: cands_lo.append((J4_SAFE[0], "J4_SAFE")) cands_hi.append((J4_SAFE[1], "J4_SAFE")) if k == 1: cands_hi.append((J1_MAX_DEG, "J1_MAX")) v_lo, s_lo = max(cands_lo, key=lambda c: c[0]) v_hi, s_hi = min(cands_hi, key=lambda c: c[0]) lo.append(v_lo); hi.append(v_hi); lo_src.append(s_lo); hi_src.append(s_hi) return np.array(lo), np.array(hi), lo_src, hi_src LIMIT_JA = {"PHYS": "物理可動域", "J4_SAFE": "J4_SAFE", "J1_MAX": "j1 の上限 90.87 (PICK_J1_FIXED)"} def plan_step(start, joint, delta): """start (今回の出発点) の joint を delta 動かした目標を作り, 範囲を通す. 戻り値 dict: target 送る目標 (6) or None (送らない) clip 切られた中身 [{joint, want, cmd, limit, bound}] (切られて端まで送るとき) refused 送らない理由 [{joint, value, limit, lo, hi, ...}] (空なら送ってよい) 動かした関節は範囲の端で切る (端までは送る). 端にいて押した向きに 1 ° も進めないときは送らない. ほかの関節は変えない (補わない). 6 関節のどれかが範囲の外なら送らない. """ lo, hi, lo_src, hi_src = joint_window() want = np.asarray(start, dtype=float)[:6].copy() out = {"target": None, "clip": [], "refused": []} if not np.all(np.isfinite(want)): out["refused"].append({"joint": None, "why": "nonfinite_start", "value": _r(want)}) return out # 出発点の 6 関節がどれか範囲の外なら送らない (切ってから送ると, 範囲の外から 1 回で # 刻みより大きく動く. 例: j1=100 から −0.5 を押すと 90.87 へ −9.13° になる. codex 必須 2) for k in range(6): if want[k] < lo[k] - LIMIT_EPS_DEG or want[k] > hi[k] + LIMIT_EPS_DEG: src = hi_src[k] if want[k] > hi[k] else lo_src[k] out["refused"].append({"joint": k, "why": "start_out_of_window", "value": round(want[k], 2), "lo": round(lo[k], 2), "hi": round(hi[k], 2), "limit": src}) if out["refused"]: return out v = want[joint] + delta if lo[joint] > hi[joint]: out["refused"].append({"joint": joint, "why": "empty_window", "value": round(v, 2), "lo": round(lo[joint], 2), "hi": round(hi[joint], 2), "limit": f"{lo_src[joint]}/{hi_src[joint]}"}) return out c = min(max(v, lo[joint]), hi[joint]) if abs(c - v) > LIMIT_EPS_DEG: src = hi_src[joint] if v > hi[joint] else lo_src[joint] bound = hi[joint] if v > hi[joint] else lo[joint] out["clip"].append({"joint": joint, "want": round(v, 2), "cmd": round(c, 2), "limit": src, "bound": round(float(bound), 2)}) if (c - want[joint]) * np.sign(delta) <= LIMIT_EPS_DEG: # 端にいる (または出発点が範囲の外で, 押した向きに進めない) src = hi_src[joint] if delta > 0 else lo_src[joint] out["refused"].append({"joint": joint, "why": "at_limit", "value": round(v, 2), "lo": round(lo[joint], 2), "hi": round(hi[joint], 2), "limit": src}) return out want[joint] = c for k in range(6): if want[k] < lo[k] - LIMIT_EPS_DEG or want[k] > hi[k] + LIMIT_EPS_DEG: src = hi_src[k] if want[k] > hi[k] else lo_src[k] out["refused"].append({"joint": k, "why": "out_of_window", "value": round(want[k], 2), "lo": round(lo[k], 2), "hi": round(hi[k], 2), "limit": src}) if not out["refused"]: out["target"] = want return out def limit_text_ascii(plan): """画面用 (英数字だけ).""" parts = [] for c in plan.get("clip", []): parts.append(f"j{c['joint']} clipped {c['want']:+.2f}->{c['cmd']:+.2f} ({c['limit']} {c['bound']:+.2f})") for r in plan.get("refused", []): if r.get("joint") is None: parts.append(f"refused: {r['why']}") else: parts.append(f"j{r['joint']} refused {r['why']} {r['value']:+.2f} " f"[{r['lo']:+.2f}, {r['hi']:+.2f}] ({r['limit']})") return " / ".join(parts) def limit_text_ja(plan): """ターミナル用 (日本語).""" parts = [] for c in plan.get("clip", []): parts.append(f"j{c['joint']} を {c['want']:+.2f}° → {c['cmd']:+.2f}° に切った" f"（{LIMIT_JA.get(c['limit'], c['limit'])} {c['bound']:+.2f}°）") for r in plan.get("refused", []): if r.get("joint") is None: parts.append(f"送らない: {r['why']}") continue why = {"at_limit": "範囲の端にいて、押した向きに進めない", "out_of_window": "範囲の外", "start_out_of_window": "出発点が範囲の外（HOME の長押しで戻せます）", "empty_window": "範囲が空"}.get(r["why"], r["why"]) parts.append(f"j{r['joint']} {r['value']:+.2f}° は送らない：{why}" f"（範囲 [{r['lo']:+.2f}, {r['hi']:+.2f}]・{LIMIT_JA.get(r['limit'], r['limit'])}）") return " / ".join(parts) # ============================================================================= # アームへの送り口 # ============================================================================= class SendGate: """1 つの仕事の「送ると決める」と「停止・終了の取り消し」を順番に並べる鍵 (codex R1 必須 1 / R2). 送る側は 鍵の中で取り消しを確かめて「送ると決めた」(committed) を立て, 鍵の外で送る. 停止・終了は 鍵の中で取り消しを立て, そのときに送ると決めていたかを受け取る. 鍵を持つのは確かめる一瞬だけなので, 停止は待たされない (画面も止まらない). なので「停止のあとに送ると決めた指令」はない. 停止より前に決めた送信は, 通信が遅れても daemon_send_angles の timeout_override (SEND_RPC_TIMEOUT_SEC) の中で終わる. 取り消しは仕事ごとに作り, OPTIONS で再開しても戻さない (止める前の仕事が生き返らない). """ def __init__(self): self.lock = threading.Lock() self.cancel = threading.Event() self.committed = False def run(self, fn): """取り消されていなければ「送ると決めた」を立ててから fn() を呼ぶ. 戻り値: 呼んだか.""" with self.lock: if self.cancel.is_set(): return False self.committed = True fn() return True def cancel_now(self): """取り消しを立てる. 戻り値: それより前に送ると決めていたか (送信中・送信済み).""" with self.lock: self.cancel.set() return self.committed class DaemonArm: """--execute: daemon に送る. 送り方・読み方は visual_servo_pick / run_policy_client と同じ.""" execute = True def read(self): return rpc.read_angles() # 5 回まで読み直す. 読めなければ None def send_and_check(self, target, gate, quitting, check=True): """target を send_angles (speed 20) で送り, 3° 以内に届くかを読んで確かめる. gate が取り消されていれば送らない. 無視されたら送り直す (IGNORED_*). 確かめる途中で読めなければ error. check=False (長押し): 送るだけで, 届いたかは待たない (0.1 秒後に次の目標を送るため).""" target = np.asarray(target, dtype=float) if not check: t_send = time.time() try: sent = gate.run(lambda: daemon_send_angles([float(v) for v in target], speed=SPEED, wait=rpc.SEND_WAIT, timeout_override=SEND_RPC_TIMEOUT_SEC)) except Exception as e: return {"sent": "unknown", "error": f"send_failed: {type(e).__name__}: {e}", "t_send": t_send} if not sent: return {"sent": False, "why": "halted_before_send"} return {"sent": True, "reached": None, "no_check": True, "t_send": t_send} pre = self.read() if pre is None: return {"sent": False, "error": "read_failed_before_send"} pre_max = float(np.max(np.abs(target - pre))) timeout = max(REACH_TIMEOUT_SEC, wr._compute_arrival_timeout(pre_max)) t_send = time.time() try: sent = gate.run(lambda: daemon_send_angles([float(v) for v in target], speed=SPEED, wait=rpc.SEND_WAIT, timeout_override=SEND_RPC_TIMEOUT_SEC)) except Exception as e: # 届いたかどうか分からない. 呼び出し側で停止にする return {"sent": "unknown", "error": f"send_failed: {type(e).__name__}: {e}", "measured_before_send": _r(pre), "t_send": t_send} if not sent: return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(pre)} t0 = time.monotonic() t_last_send = t0 last, err = None, None resends = [] # 送り直した時刻 (送ってからの秒) base = {"sent": True, "timeout_sec": round(timeout, 2), "t_send": t_send, "measured_before_send": _r(pre), "resends": resends} while time.monotonic() - t0 < timeout: if quitting.is_set(): return dict(base, reached=None, interrupted=True, reached_angles=_r(last), reach_err=None if err is None else round(err, 3)) a = self.read() if a is None: # 動いているはずの間に読めない: 届いたか分からないまま次を受け付けない (codex 必須 6) return dict(base, reached=None, reached_angles=_r(last), reach_err=None if err is None else round(err, 3), error="read_failed_during_check", elapsed_sec=round(time.monotonic() - t0, 2)) last = a err = float(np.max(np.abs(a - target))) # 丸める前の値で判定する (codex 推奨 3) if err <= REACH_TOL_DEG: return dict(base, reached=True, reached_angles=_r(a), reach_err=round(err, 3), elapsed_sec=round(time.monotonic() - t0, 2)) moved = float(np.max(np.abs(a - pre))) if (len(resends) < IGNORED_MAX_RESENDS and moved < IGNORED_MOVE_DEG and time.monotonic() - t_last_send >= IGNORED_CHECK_SEC): # 無視された: 同じ目標を送り直す (停止・離したあとなら送らない) try: again = gate.run(lambda: daemon_send_angles([float(v) for v in target], speed=SPEED, wait=rpc.SEND_WAIT, timeout_override=SEND_RPC_TIMEOUT_SEC)) except Exception as e: return dict(base, reached=None, reached_angles=_r(a), reach_err=round(err, 3), error=f"resend_failed: {type(e).__name__}: {e}", elapsed_sec=round(time.monotonic() - t0, 2)) if not again: return dict(base, reached=None, halted_during_check=True, reached_angles=_r(a), reach_err=round(err, 3), elapsed_sec=round(time.monotonic() - t0, 2)) t_last_send = time.monotonic() resends.append(round(t_last_send - t0, 2)) continue time.sleep(POLL_SLEEP) return dict(base, reached=False, reached_angles=_r(last), reach_err=None if err is None else round(err, 3), elapsed_sec=round(time.monotonic() - t0, 2)) def gripper(self, state, gate): """run_policy_client.gripper_cmd と同じ中身 (閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ). 指令だけを gate の中で送り, 待ちは gate の外で行う (停止の受け付けを待ちの分だけ遅らせない). 戻り値: 送ったか.""" if state == 1: sent = gate.run(lambda: daemon_set_gripper_value(rpc.GRIP_VALUE)) else: sent = gate.run(lambda: daemon_set_gripper(0, wait=rpc.GRIP_OPEN_DAEMON_WAIT)) if sent: time.sleep(rpc.GRIPPER_SLEEP) return sent class DryArm: """ドライラン: daemon にもアームにも触らない. HOME から始まる仮の姿勢を動かす.""" execute = False def __init__(self, start=HOME): self.q = np.asarray(start, dtype=float).copy() def read(self): return self.q.copy() def send_and_check(self, target, gate, quitting, check=True): t_send = time.time() if not gate.run(lambda: None): return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(self.q)} if not check: # 長押し: 少しずつ目標へ寄せる (待たない) self.q = self.q + np.clip(np.asarray(target, dtype=float) - self.q, -DRY_HOLD_MAX_DEG, DRY_HOLD_MAX_DEG) return {"sent": True, "reached": None, "no_check": True, "t_send": t_send, "dry": True} time.sleep(DRY_MOVE_SEC) self.q = np.asarray(target, dtype=float).copy() return {"sent": True, "reached": True, "reached_angles": _r(self.q), "reach_err": 0.0, "t_send": t_send, "dry": True} def gripper(self, state, gate): if not gate.run(lambda: None): return False time.sleep(GRIPPER_SLEEP) # 実機と同じ間だけ受け付けない return True # ============================================================================= # 記録 # ============================================================================= class JsonlLog: def __init__(self, path): self.path = Path(path) self.f = open(self.path, "x", encoding="utf-8") def write(self, rec): self.f.write(json.dumps(rec, ensure_ascii=False) + "\n") self.f.flush() # 途中で止まっても残るように 1 行ごとに def close(self): if not self.f.closed: self.f.close() class NullLog: """ドライラン: 何も書かない (依頼 69).""" def write(self, rec): pass def close(self): pass def write_json_atomic(path, obj): tmp = Path(str(path) + ".tmp") with open(tmp, "w", encoding="utf-8") as f: json.dump(obj, f, ensure_ascii=False, indent=2) f.flush() os.fsync(f.fileno()) os.replace(tmp, path) # ============================================================================= # 操作の状態 (pygame に依存しない. テストで直接呼ぶ) # ============================================================================= class Teleop: """押したボタンを受けて, 送る仕事を別のスレッドで 1 つずつ行う. 送っている間 (busy) の動く操作は受け付けない (ためない). 停止はいつでも受け付ける. 状態を書き換えるのは呼び出し側のスレッド (pygame のループ) だけ. 送る仕事の結果は results に入り, poll() で受け取って状態とログに反映する. 送る直前の確かめは 仕事ごとの SendGate で行い, 停止・終了はその鍵を取ってから取り消す. """ def __init__(self, arm, start_pose, log, out=print, spawn=None): self.arm = arm self.log = log self.out = out self.spawn = spawn or self._spawn_thread self.cmd = np.asarray(start_pose, dtype=float).copy() # 直前の目標 (始めは起動時の姿勢) self.cmd_reached = True self.measured = np.asarray(start_pose, dtype=float).copy() # 最後に読めた角度 self.sel = 0 # SELECT_ORDER の何番目 (始めは j1) self.r1 = False self.stopped = False self.stop_reason = None self.busy = False self.pending = None # 送っている操作のログ行 self.thread = None self.grip = "unknown" self.last_limit = "" # 直前に範囲で切られた理由 (画面用. 英数字) self.last_msg = "" self.gate = None # 今の仕事の SendGate (停止・終了で取り消す) self.log_failed = False # log.jsonl に書けなくなったら 再開させない (終わらせる) self.quitting = threading.Event() self.results = queue.Queue() self.home_down_at = None self.home_handled = False self.triggers = [-1.0, -1.0] self.both_triggers = False self.read_error = False self.seq = 0 self.n_sent = 0 self.hold = None # 十字キーの長押し {id, button, joint, sign, down_at, n, last_req_at} self.hold_seq = 0 # ---- 見せる値 ---- @property def joint(self): return SELECT_ORDER[self.sel] @property def step(self): return STEP_COARSE_DEG if self.r1 else STEP_FINE_DEG @property def state(self): if self.stopped: return STATE_STOPPED return STATE_MOVING if self.busy else STATE_IDLE # ---- 記録 ---- def _entry(self, button, kind, **kw): self.seq += 1 e = {"seq": self.seq, "t": datetime.now().isoformat(timespec="milliseconds"), "t_unix": round(time.time(), 3), "button": button, "button_name": BUTTON_NAMES.get(button) if isinstance(button, int) else button, "kind": kind, "selected_joint": self.joint, "step_deg": self.step, "execute": bool(self.arm.execute)} e.update(kw) return e def _finish(self, e): e.update({"grip_state": self.grip, "stopped": self.stopped, "stop_reason": self.stop_reason}) try: self.log.write(e) except Exception as ex: # SSD が外れたなど. ローカルには逃がさない first = not self.log_failed self.log_failed = True if first: self.out(f"[記録エラー] log.jsonl に書けません: {ex}。停止にします" "（再開はできません。終わらせて KIOXIA を確かめてください）") if not self.stopped: self.stop_reason = "log_write_failed" self.stopped = True self._cancel_gate() self._drop_home_hold() # ---- 停止 ---- def _cancel_gate(self): """今の仕事をまだ送ると決めていなければ, もう送らせない (待たない). 戻り値: 停止より前に送ると決めていたか (None = 仕事なし).""" if self.gate is None: return None return self.gate.cancel_now() def _drop_home_hold(self): """HOME の長押しを無効にする (押し直しを求める. codex 必須 4).""" if self.home_down_at is not None and not self.home_handled: self.home_handled = True self.out("[HOME] 長押しの途中で受け付けられなくなったので取り消しました（押し直してください）") self._finish(self._entry(BTN_HOME, "home", accepted=False, reason="hold_cancelled")) def _end_hold(self, msg=None): h = self.hold if h is not None: if h.get("q0") is not None: self.out(f"[長押し] j{h['joint']} を {h['q0']:+.2f}° → {h['q_last']:+.2f}° まで" f"（{h['n_sent']} 回送信。離したあと最大 {HOLD_AHEAD_DEG:.0f}° 先まで動いて止まります）") if msg: self.out(msg) self.hold = None def _hold_progress(self, e, job): """長押しの 1 回の送信のあと: 進み具合を覚え, HOLD_STALL_SEC 進んでいなければ止める.""" h = self.hold if h is None or h["id"] != e.get("hold_id") or not e.get("measured_before"): return qj = float(e["measured_before"][job[1]]) t = h["last_req_at"] if h.get("q0") is None: h.update(q0=qj, n_sent=0, prog_q=qj, prog_t=t) h["q_last"] = qj h["n_sent"] += 1 if abs(qj - h["prog_q"]) >= HOLD_STALL_DEG: h["prog_q"], h["prog_t"] = qj, t elif t - h["prog_t"] >= HOLD_STALL_SEC: self._end_hold(f"[長押し] j{job[1]} が {HOLD_STALL_SEC:.0f} 秒進まないので止めました" "（どこかに当たっていれば、逆向きを1回押すか HOME で戻してください）") def enter_stop(self, reason, button=None, kind="stop"): was = self.stopped self.stopped = True self._end_hold() committed = self._cancel_gate() if not was: self.stop_reason = reason self.out(f"[停止] {reason}。新しい指令は送りません（OPTIONS で再開）" + ("。止める前に送った動きは止まらずに最後まで行きます" if committed else "") + ("。まだ送っていなかった操作は送りません" if committed is False else "")) self._finish(self._entry(button, kind, accepted=True, reason=reason, already_stopped=was, busy=self.busy, send_committed_before_stop=committed, triggers=[round(v, 3) for v in self.triggers])) self._drop_home_hold() def set_triggers(self, l2, r2): self.triggers = [float(l2), float(r2)] both = l2 > TRIGGER_STOP_ABOVE and r2 > TRIGGER_STOP_ABOVE if both and not self.both_triggers: self.both_triggers = True self.enter_stop("L2+R2", button="L2+R2") self.both_triggers = both def controller_lost(self, reason): self.enter_stop(reason, button=None, kind="controller_lost") self.r1 = False # 押しっぱなしの状態を持ち越さない self.home_down_at = None self.home_handled = False self.both_triggers = False def controller_read_failed(self, msg): if not self.read_error: self.read_error = True self.enter_stop(f"read_error: {msg}", button=None, kind="read_error") def controller_read_ok(self): self.read_error = False def _resume(self, e): if not self.stopped: e.update(accepted=False, reason="not_stopped") self.out("[再開] 止まっていないので何もしません") elif self.both_triggers: e.update(accepted=False, reason="triggers_still_pressed") self.out("[再開] L2 と R2 を離してから OPTIONS を押してください") elif self.read_error: e.update(accepted=False, reason="controller_read_error") self.out("[再開] コントローラーを読めていないので再開しません") elif self.log_failed: e.update(accepted=False, reason="log_write_failed") self.out("[再開] 記録が書けないので再開しません（終わらせて KIOXIA を確かめてください）") else: # 止める前に作った仕事の取り消しは戻さない (仕事ごとの SendGate. 再開で生き返らない) self.stopped = False self.stop_reason = None e.update(accepted=True) self.out("[再開] 操作を受け付けます") self._finish(e) # ---- ボタン ---- def press(self, button, now): if button == BTN_R1: self.r1 = True self._finish(self._entry(button, "modifier", accepted=True)) return self._end_hold() # 十字キーの長押しは, ほかのボタン (R1 以外) で止まる if button == BTN_TRIANGLE: self.sel = (self.sel + 1) % len(SELECT_ORDER) self.out(f"[関節] 上下で動かす関節を j{self.joint} にしました") self._finish(self._entry(button, "select_joint", accepted=True)) return if button == BTN_OPTIONS: self._resume(self._entry(button, "resume")) return if button == BTN_HOME: self.home_down_at = now self.home_handled = False if self.stopped or self.busy: # 押し始めに受け付けられないなら, 長押しを続けても送らない (押し直しを求める) self.home_handled = True why = "stopped" if self.stopped else "busy" self.out("[受付なし] HOME: " + ("停止中です" if self.stopped else "前の操作が終わっていません") + "（押し直してください）") self._finish(self._entry(button, "home", accepted=False, reason=why)) return # 1 秒続いたとき (tick) か 離したとき (release) に 1 行書く if button in (BTN_LEFT, BTN_RIGHT, BTN_UP, BTN_DOWN): if button in (BTN_LEFT, BTN_RIGHT): sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN j = 0 else: sign = UP_SIGN if button == BTN_UP else -UP_SIGN j = self.joint self.hold_seq += 1 self.hold = {"id": self.hold_seq, "button": button, "joint": j, "sign": sign, "down_at": now, "n": 0, "last_req_at": now} e = self._entry(button, "move", joint=j, delta=sign * self.step, hold_id=self.hold_seq, hold_n=0) self._request(e, ("move", j, sign * self.step, None)) if not e.get("accepted"): # 押し始めに受け付けられない (動いている間・停止中) なら, 押し続けても長押しにしない (HOME と同じ) self.hold = None return if button in (BTN_CROSS, BTN_CIRCLE): state = 1 if button == BTN_CROSS else 0 self._request(self._entry(button, "grip_close" if state else "grip_open", grip_cmd=state), ("grip", state)) return self._finish(self._entry(button, "ignored", accepted=False, reason="unassigned")) def release(self, button, now): if self.hold is not None and button == self.hold["button"]: hid = self.hold["id"] self._end_hold() # 離したら長押しを止める p = self.pending if (self.busy and self.gate is not None and p is not None and p.get("hold_id") == hid and (p.get("hold_n") or 0) >= 1): # 長押しの 2 回目からの刻みを, まだ送ると決めていなければ送らない (1 回目 = 短い押しは取り消さない) p["released_during_job"] = True self.gate.cancel_now() if button == BTN_R1: self.r1 = False elif button == BTN_HOME: if self.home_down_at is not None and not self.home_handled: held = now - self.home_down_at self.out(f"[HOME] {held:.2f} 秒で離したので送りません（{HOME_HOLD_SEC:.0f} 秒の長押しで送ります）") self._finish(self._entry(button, "home_short", accepted=False, reason="held_too_short", held_sec=round(held, 3))) self.home_down_at = None self.home_handled = False def tick(self, now): if self.home_down_at is not None and not self.home_handled and (self.busy or self.stopped): self._drop_home_hold() # 長押しの途中で別の操作が動いた・止まった if (self.home_down_at is not None and not self.home_handled and now - self.home_down_at >= HOME_HOLD_SEC): self.home_handled = True self._request(self._entry(BTN_HOME, "home", held_sec=round(now - self.home_down_at, 3)), ("home",)) h = self.hold if (h is not None and not self.stopped and not self.busy and now - h["down_at"] >= HOLD_START_SEC - 1e-6 and now - h["last_req_at"] >= HOLD_INTERVAL_SEC - 1e-6): h["n"] += 1 h["last_req_at"] = now d = h["sign"] * HOLD_AHEAD_DEG self._request(self._entry(h["button"], "move", joint=h["joint"], delta=d, hold_id=h["id"], hold_n=h["n"], held_sec=round(now - h["down_at"], 3)), ("move", h["joint"], d, h["id"])) def _hold_alive(self, hold_id): h = self.hold return h is not None and h["id"] == hold_id # ---- 送る仕事 ---- def _request(self, e, job): if self.stopped: e.update(accepted=False, reason="stopped") self.out("[受付なし] 停止中です（OPTIONS で再開）") self._finish(e) return if self.busy: e.update(accepted=False, reason="busy") self.out("[受付なし] 前の操作が終わっていません（ためません）") self._finish(e) return e["accepted"] = True self.busy = True self.pending = e gate = SendGate() self.gate = gate snap = {"cmd": self.cmd.copy(), "cmd_reached": self.cmd_reached} self.spawn(lambda: self._run_job(job, snap, gate)) def _spawn_thread(self, fn): self.thread = threading.Thread(target=fn, daemon=True) self.thread.start() def _run_job(self, job, snap, gate): res = {} try: if job[0] == "move": _, j, d, hold_id = job q = self.arm.read() res["measured_before"] = _r(q) if q is None: res["error"] = "read_failed" return if hold_id is not None: # 長押しの 2 回目から: 動かす関節は「いま読んだ角度 + 3° 先」. ほかの関節は直前の指令のまま # (読んだ角度で送り直すと, 重さで沈んだ分を毎回なぞって j1 などがずり下がる) cmd = np.asarray(snap["cmd"], dtype=float) others = [k for k in range(6) if k != j] gap_o = float(np.max(np.abs(cmd[others] - q[others]))) start = cmd.copy() if gap_o <= REACH_TOL_DEG else q.copy() start[j] = q[j] res["start_source"] = "hold" if gap_o <= REACH_TOL_DEG else "measured" res["cmd_gap_deg"] = round(gap_o, 3) res["hold_lead_deg"] = round(abs(float(cmd[j] - q[j])), 3) plan = plan_step(start, j, d) if (plan["target"] is not None and plan["clip"] and abs(float(plan["target"][j] - cmd[j])) < 0.01): # 範囲の端の目標をもう送ってある: 同じ目標を送り続けない (長押しを止める) c = plan["clip"][0] plan = {"target": None, "clip": [], "refused": [{ "joint": j, "why": "at_limit", "value": c["want"], "lo": c["bound"], "hi": c["bound"], "limit": c["limit"]}]} if (plan["target"] is not None and abs(float(cmd[j] - q[j])) <= HOLD_AHEAD_DEG + REACH_TOL_DEG and (plan["target"][j] - cmd[j]) * np.sign(d) < 0): # 読んだ角度が古いと「読んだ角度 + 3°」が前の目標より後ろになる. 後ろへは戻さない (引き戻さない) plan["target"][j] = cmd[j] res["held_previous_target"] = True res["clip"] = plan["clip"] res["refused"] = plan["refused"] res["limit_ja"] = limit_text_ja(plan) res["limit_ascii"] = limit_text_ascii(plan) if plan["target"] is None: res["sent"] = False return res["command"] = _r(plan["target"]) res["_target"] = plan["target"] res.update(self.arm.send_and_check(plan["target"], gate, self.quitting, check=False)) return # 出発点: 直前の目標に届いていて, 今読んだ角度とも 3° 以内ならその目標. # そうでなければ今読んだ角度 (手で動かされたなど. 古い目標から大きく動かさない. codex 必須 3) gap = float(np.max(np.abs(np.asarray(snap["cmd"]) - q))) use_cmd = snap["cmd_reached"] and gap <= REACH_TOL_DEG start = snap["cmd"] if use_cmd else q res["start_source"] = "command" if use_cmd else "measured" res["cmd_gap_deg"] = round(gap, 3) plan = plan_step(start, j, d) res["clip"] = plan["clip"] res["refused"] = plan["refused"] res["limit_ja"] = limit_text_ja(plan) res["limit_ascii"] = limit_text_ascii(plan) if plan["target"] is None: res["sent"] = False return res["command"] = _r(plan["target"]) res["_target"] = plan["target"] res.update(self.arm.send_and_check(plan["target"], gate, self.quitting)) elif job[0] == "home": q = self.arm.read() res["measured_before"] = _r(q) if q is None: # 読めなければ送らない (codex 必須 6) res["error"] = "read_failed" return res["command"] = _r(HOME) res["_target"] = np.array(HOME) res["range_check"] = "skipped_for_home" res.update(self.arm.send_and_check(np.array(HOME), gate, self.quitting)) elif job[0] == "grip": res["measured_before"] = None sent = self.arm.gripper(job[1], gate) res["sent"] = bool(sent) if not sent: res["why"] = "halted_before_send" except Exception as ex: res["error"] = f"{type(ex).__name__}: {ex}" finally: self.results.put((job, res)) def poll(self): """送る仕事の結果を受け取り, 状態とログに反映する (pygame のループから毎回呼ぶ).""" while True: try: job, res = self.results.get_nowait() except queue.Empty: return self._apply(job, res) def _apply(self, job, res): e = self.pending or self._entry(None, "orphan_result") self.pending = None self.busy = False target = res.pop("_target", None) e.update(res) if res.get("reached_angles") is not None: self.measured = np.asarray(res["reached_angles"], dtype=float) elif res.get("measured_before") is not None: self.measured = np.asarray(res["measured_before"], dtype=float) sent = res.get("sent") if job[0] in ("move", "home") and sent is True: self.n_sent += 1 self.cmd = np.asarray(target, dtype=float) # 長押しの送信は届くのを待たないが, アームは目標へ向かって動き続けるので「届く目標」として扱う # (次の短い押しは, 読んだ角度と 3° 以内ならこの目標から. 離れていれば読んだ角度から) self.cmd_reached = True if res.get("no_check") else bool(res.get("reached")) elif sent == "unknown": self.cmd_reached = False # 次は読んだ角度から if job[0] == "grip" and res.get("error"): self.grip = "unknown" if job[0] == "grip" and sent is True: self.grip = "closed" if job[1] == 1 else "open" if job[0] == "move": self.last_limit = res.get("limit_ascii", "") e["reached"] = res.get("reached") if sent is True else None # ---- ターミナル ---- if job[0] == "move": label = f"j{job[1]} {job[2]:+.1f}°" + (f"（長押し {e.get('hold_n')}）" if job[3] is not None else "") elif job[0] == "home": label = "HOME" else: label = "爪を" + ("閉じる" if job[1] == 1 else "開く") if res.get("limit_ja"): self.out(f"[範囲] {res['limit_ja']}") if res.get("error"): self.out(f"[エラー] {label}: {res['error']}。停止にします") elif sent is True and job[0] == "grip": self.out(f"[爪] {label}（待ち {GRIPPER_SLEEP:.1f} 秒のあと受付）") elif sent is True and res.get("no_check"): self._hold_progress(e, job) # 長押し: 1 回ずつは出さない (離したときに 1 行) elif sent is True: r = res.get("reached") how = "届いた" if r else ("確かめる途中で終了" if r is None else "届かなかった") if res.get("resends"): how += f"・無視されたので {len(res['resends'])} 回送り直した" self.out(f"[送信] {label} → {_fmt(res.get('command'))} {how}" f"（ずれ {res.get('reach_err')}°） 届いた角度 {_fmt(res.get('reached_angles'))}") elif res.get("why") == "halted_before_send" and e.get("released_during_job") and not self.stopped: pass # 長押しを離したので取り消した. 何も出さない elif res.get("why") == "halted_before_send": self.out(f"[送らない] {label}: 停止が入ったので送りませんでした") else: self.out(f"[送らない] {label}") # 長押しの中で送れなかったら (範囲の端・停止), その長押しは止める. 端で同じ行を繰り返さない if job[0] == "move" and sent is not True and e.get("hold_id") is not None and self._hold_alive(e["hold_id"]): self._end_hold() self.gate = None self._finish(e) if res.get("error"): self.enter_stop(f"arm_error: {res['error']}", kind="arm_error") # ---- 終わり ---- def shutdown(self): """新しい指令を送らずに終わる. 送っている途中の仕事が戻るのを WORKER_JOIN_SEC まで待つ. 戻り値: 仕事が終わっているか (終わっていなければ daemon のソケットを閉じない).""" self.quitting.set() committed = self._cancel_gate() if self.pending is not None: self.pending["send_committed_before_exit"] = committed if self.thread is not None: self.thread.join(WORKER_JOIN_SEC) self.poll() done = self.thread is None or not self.thread.is_alive() if self.pending is not None: e = self.pending self.pending = None e.update(reached=None, interrupted_at_exit=True) self._finish(e) return done def status_lines(self, q_now=None): """画面の文字 (英数字だけ).""" q = self.measured if q_now is None else q_now lines = [ ("big", f"JOINT j{self.joint} STEP {self.step:.1f} deg"), ("big", f"STATE {self.state}" + (" (moving)" if self.stopped and self.busy else "")), ("big", f"GRIP {self.grip}"), ] for k in range(6): lines.append(("mono", f"j{k} {q[k]:+8.2f} cmd {self.cmd[k]:+8.2f}")) lines.append(("small", "last limit: " + (self.last_limit or "-"))) if self.stopped: lines.append(("small", f"stop reason: {self.stop_reason} (OPTIONS = resume)")) return lines # ============================================================================= # pygame のループ # ============================================================================= WINDOW_SIZE = (900, 560) def controller_info(js): return {"name": js.get_name(), "guid": js.get_guid(), "instance_id": js.get_instance_id(), "axes": js.get_numaxes(), "buttons": js.get_numbuttons(), "hats": js.get_numhats(), "connected_at": datetime.now().isoformat(timespec="seconds")} def run_loop(tp, screen, fonts, mode_text, on_controller=None, on_frame=None): """ウィンドウを閉じるまで回す. 戻り値: 終わった理由.""" js = None clock = pygame.time.Clock() frame = 0 while True: if on_frame is not None: on_frame(frame, tp) frame += 1 now = time.monotonic() events = pygame.event.get() buttons = [] for ev in events: if ev.type == pygame.QUIT: return "window_closed" if ev.type == pygame.JOYDEVICEADDED: if js is None: js = pygame.joystick.Joystick(ev.device_index) info = controller_info(js) tp.out(f"[接続] {info['name']} GUID={info['guid']} 軸={info['axes']} ボタン={info['buttons']}") if on_controller is not None: on_controller(info) elif ev.type == pygame.JOYDEVICEREMOVED: if js is not None and ev.instance_id == js.get_instance_id(): js = None tp.controller_lost("controller_disconnected") elif ev.type in (pygame.JOYBUTTONDOWN, pygame.JOYBUTTONUP): if js is not None and ev.instance_id == js.get_instance_id(): buttons.append(ev) # 停止 (L2+R2) と 読み取りの失敗を ボタンより先に見る (同じコマのボタンで送らない. codex 必須 5) read_ok = js is not None if js is not None: try: l2, r2 = js.get_axis(AXIS_L2), js.get_axis(AXIS_R2) tp.controller_read_ok() tp.set_triggers(l2, r2) except Exception as ex: # pygame.error など read_ok = False tp.controller_read_failed(f"{type(ex).__name__}: {ex}") if read_ok: for ev in buttons: if ev.type == pygame.JOYBUTTONDOWN: tp.press(ev.button, now) else: tp.release(ev.button, now) # 長押しの最中は, 離した合図を取りこぼしても止まるよう, ボタンの状態も毎コマ見る if js is not None and read_ok and tp.hold is not None: hb = tp.hold["button"] try: held = js.get_button(hb) except Exception as ex: # pygame.error など tp.controller_read_failed(f"{type(ex).__name__}: {ex}") else: if not held: tp.release(hb, now) tp.tick(now) tp.poll() draw(screen, fonts, tp, mode_text, js) clock.tick(60) def draw(screen, fonts, tp, mode_text, js): screen.fill((20, 20, 24)) color_state = {STATE_IDLE: (120, 220, 140), STATE_MOVING: (240, 200, 80), STATE_STOPPED: (240, 90, 90)}[tp.state] y = 10 screen.blit(fonts["small"].render(f"{mode_text} controller: " + (js.get_name()[:40] if js is not None else "none"), True, (200, 220, 255)), (10, y)) y += 28 for kind, text in tp.status_lines(): col = color_state if text.startswith("STATE") else (235, 235, 235) screen.blit(fonts[kind].render(text[:110], True, col), (10, y)) y += {"big": 44, "mono": 26, "small": 24}[kind] y += 6 for t in ("dpad L/R: j0 dpad U/D: selected joint (up = -, down = +) hold: keep moving triangle: next joint", "R1 hold: 2 deg cross: close circle: open HOME hold 1s: home", "L2+R2: stop OPTIONS: resume window close / Ctrl+C: quit (arm is not moved)"): screen.blit(fonts["small"].render(t, True, (150, 150, 160)), (10, y)) y += 22 pygame.display.flip() # ============================================================================= # main # ============================================================================= def git_info(path): def run(*a): try: return subprocess.run(["git", *a], cwd=_ROOT, capture_output=True, text=True, timeout=5).stdout.strip() except Exception: return None rel = os.path.relpath(path, _ROOT) return {"commit": run("rev-parse", "HEAD"), "script_status": run("status", "--porcelain", "--", rel) or "clean"} def main(argv=None, on_frame=None, start_pose_dry=None): ap = argparse.ArgumentParser(description="ゲームパッドで mechArm を手で動かす (既定はドライラン)") ap.add_argument("--execute", action="store_true", help="daemon に送る (付けなければドライラン)") args = ap.parse_args(argv) stamp = datetime.now().strftime("%Y%m%d_%H%M%S") run_stem = f"teleop_{stamp}" + ("" if args.execute else "_dry") if args.execute: ensure_ssd_mounted() try: load_daemon_modules() except Exception as ex: # 読み込みの失敗・値の違い. 記録も daemon もまだ触っていない sys.exit(f"[ABORT] daemon の送り口を読めません: {type(ex).__name__}: {ex}。何も送らずに終わります") run_dir = LOG_ROOT / run_stem if run_dir.exists(): sys.exit(f"[ABORT] 同じ名前のフォルダがあります: {run_dir}") run_dir.mkdir(parents=True) script_copy = run_dir / Path(__file__).name shutil.copy2(__file__, script_copy) log = JsonlLog(run_dir / "log.jsonl") print(f"[記録] {run_dir}") else: # ドライランは記録しない: KIOXIA に何も書かない・フォルダも作らない (依頼 69) run_dir = script_copy = None log = NullLog() print("[記録] ドライランなので記録しません") tp = None meta = None end_reason = None daemon_used = False try: if args.execute: get_daemon_socket() daemon_used = True rpc.install_daemon_lock() arm = DaemonArm() start = arm.read() if start is None: end_reason = "start_read_failed" sys.exit("[ABORT] 起動したときの関節角を読めません（daemon を確かめてください）。何も送らずに終わります") print("[実機] daemon に送ります。起動したときの姿勢: " + _fmt(start)) else: start = np.asarray(start_pose_dry if start_pose_dry is not None else HOME, dtype=float) arm = DryArm(start) print("[ドライラン] daemon にもアームにも触りません。HOME から始まる仮の姿勢を動かします") lo, hi, _, _ = joint_window() bad = [k for k in range(6) if not (lo[k] - LIMIT_EPS_DEG <= start[k] <= hi[k] + LIMIT_EPS_DEG)] if bad: print("[注意] 起動したときの姿勢が守りの外: " + ", ".join(f"j{k}={start[k]:+.2f}" for k in bad) + "。この関節を含む操作は送りません（HOME の長押しは送れます）") meta = None if not args.execute else { "run_stem": run_stem, "started_at": datetime.now().isoformat(timespec="seconds"), "execute": bool(args.execute), "argv": sys.argv, "controllers": [], "python": sys.version, "pygame": pygame.version.ver, "sdl": ".".join(str(v) for v in pygame.get_sdl_version()), "platform": platform.platform(), "start_pose": _r(start), "guards": {"J1_MAX_DEG": J1_MAX_DEG, "range": "physical_limits (no base window. 2026-11-03)", "J4_SAFE": list(J4_SAFE), "JOINT_LIMITS": [list(x) for x in JOINT_LIMITS], "window": {"lo": _r(lo), "hi": _r(hi)}, "home_bypasses_range_check": True}, "motion": {"speed": SPEED, "step_fine_deg": STEP_FINE_DEG, "step_coarse_deg": STEP_COARSE_DEG, "reach_tol_deg": REACH_TOL_DEG, "reach_timeout_min_sec": REACH_TIMEOUT_SEC, "poll_sec": POLL_SLEEP, "gripper_sleep_sec": rpc.GRIPPER_SLEEP, "grip_close_value": rpc.GRIP_VALUE, "send_wait": rpc.SEND_WAIT, "home": HOME, "home_hold_sec": HOME_HOLD_SEC, "trigger_stop_above": TRIGGER_STOP_ABOVE, "hold_start_sec": HOLD_START_SEC, "hold_interval_sec": HOLD_INTERVAL_SEC, "hold_ahead_deg": HOLD_AHEAD_DEG, "hold_stall_sec": HOLD_STALL_SEC, "ignored_check_sec": IGNORED_CHECK_SEC, "ignored_max_resends": IGNORED_MAX_RESENDS, "j0_left_sign": J0_LEFT_SIGN, "up_sign": UP_SIGN, "select_order": list(SELECT_ORDER)}, "button_map": {str(k): v for k, v in BUTTON_NAMES.items()}, "git": git_info(__file__), "script_copy": script_copy.name, "script_sha256": hashlib.sha256(script_copy.read_bytes()).hexdigest(), } if meta is not None: write_json_atomic(run_dir / "meta.json", meta) def on_controller(info): if meta is None: return meta["controllers"].append(info) try: write_json_atomic(run_dir / "meta.json", meta) except Exception as ex: print(f"[記録エラー] meta.json に書けません: {ex}") tp = Teleop(arm, start, log) pygame.init() pygame.joystick.init() screen = pygame.display.set_mode(WINDOW_SIZE) pygame.display.set_caption("gamepad_teleop" + (" [EXECUTE]" if args.execute else " [DRY RUN]")) mono = "menlo,consolas,dejavusansmono" fonts = {"big": pygame.font.SysFont(mono, 34, bold=True), "mono": pygame.font.SysFont(mono, 20), "small": pygame.font.SysFont(mono, 16)} print("コントローラーを待っています（つないでいなければ、今つないでください）") end_reason = run_loop(tp, screen, fonts, "EXECUTE" if args.execute else "DRY RUN", on_controller=on_controller, on_frame=on_frame) except KeyboardInterrupt: end_reason = "ctrl_c" print("\n[終了] Ctrl+C") finally: # 終わるときにアームを動かさない (HOME 復帰もサーボ解放もしない). 新しい指令も送らない worker_done = True if tp is not None: worker_done = tp.shutdown() if not worker_done: print("[終了] 送っている途中の仕事が戻らないまま終わります（ソケットは閉じません）") if meta is not None: meta.update(ended_at=datetime.now().isoformat(timespec="seconds"), end_reason=end_reason or "error", n_sent=(tp.n_sent if tp is not None else 0)) try: write_json_atomic(run_dir / "meta.json", meta) except Exception as ex: print(f"[記録エラー] meta.json に書けません: {ex}") log.close() if daemon_used and worker_done: try: close_daemon_socket() except Exception: pass pygame.quit() print("[終了] アームは動かしていません。" + (f"記録: {run_dir}" if run_dir is not None else "記録なし（ドライラン）")) return 0 if __name__ == "__main__": sys.exit(main())