#!/usr/bin/env python3 """gamepad_teleop.py — ゲームパッドで mechArm 270 を手で動かす (依頼 68. 2026-11-02) PICK で爪の位置がずれたときに, エイイチがゲームパッドで左右・上げ下げ・爪の開閉を直す. 押した分だけそのまま動く. 自動で動く要素は入れない (自動の握り直し・失敗時の自動 HOME などはない). 押した操作と, 送った角度・届いた角度をすべて log.jsonl に残す (あとで「人が直した記録」として使う). 操作 (エイイチが決めた. 変えない): 十字キー 左 / 右 (13 / 14) j0 を 0.5° ずつ. どの関節を選んでいても いつも j0 (向き: 左 = J0_LEFT_SIGN. 依頼に向きの指定がないので, 右 = +, 左 = − にしてある) 十字キー 上 / 下 (11 / 12) 選んでいる関節を 0.5° ずつ. 上 = −, 下 = +. 始めは j1 △ (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) で再開 ほかのボタン (□・SHARE・L1・L3・R3・タッチパッド・スティック) は何もしない 守り: - j1 は 90.87 (arm_kinematics.md の PICK_J1_FIXED) を超える指令を送らない (fk_lookup.J1_SAFE_MAX = 88.0 は別の用途の値なので, ここでは使わない) - j0 は 基準の姿勢 ±15°, j1〜j5 は ±10°. j4 は J4_SAFE (−90〜+60). 物理可動域 (wr.JOINT_LIMITS) も通す - 基準の姿勢 = 起動したときに読んだ姿勢. HOME を送ったあとは HOME を基準にし直す - 範囲で切られたら理由を画面とログに出す. 別の関節で補わない - HOME だけは範囲の検査を通さずに送れる (run_policy_client.py の h と同じ扱い) 送り方: - send_angles だけ (send_coords は使わない). speed 20 (j0_sweep_scan.SPEED) - 1 回押すごとに目標を 1 つ作って 1 回だけ送り, 3° 以内に届いたかを読んで確かめる. 送り直しはしない (_send_and_wait_arrival は届かないと 5 秒後に同じ目標を送り直すので, その間に停止が押されても送ってしまう. ここでは使わない) - 送ってから届くまで (または待ちの上限まで) と, 爪のあとの 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 に送る 記録: /Volumes/KIOXIA/r2r2r-data/runs/experiments/gamepad_teleop/{run_stem}/ log.jsonl (押した操作 1 つにつき 1 行) / meta.json / gamepad_teleop.py (使ったスクリプトの写し) 通しの完走ではないので台帳には書かない. """ import argparse 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")) # run_policy_client を読むと, パス (place_modules / drag_teaching) と pymycobot のスタブが入る. # 爪 (gripper_cmd)・HOME・J4_SAFE・関節角の読み方 (read_angles) は それと同じものを使う import run_policy_client as rpc # noqa: E402 from j0_sweep_scan import POLL_SLEEP, REACH_TIMEOUT_SEC, SPEED # noqa: E402 20 / 0.3 / 8.0 from visual_servo_pick import ( # noqa: E402 close_daemon_socket, daemon_send_angles, daemon_set_gripper, daemon_set_gripper_value, get_daemon_socket, ) wr = rpc.wr # ---- ボタンと軸 (/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 = 0.5 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.HOME] # [0, 0, 0, 0, 0, -30] # ---- 守り ---- J1_MAX_DEG = 90.87 # arm_kinematics.md の PICK_J1_FIXED (エイイチと決めた. J1_SAFE_MAX 88 は使わない) J0_RANGE_DEG = 15.0 # 基準の姿勢から j0 は ±15° JOINT_RANGE_DEG = 10.0 # j1〜j5 は ±10° J4_SAFE = tuple(float(v) for v in rpc.J4_SAFE) # (-90, 60) JOINT_LIMITS = [tuple(float(v) for v in lim) for lim in wr.JOINT_LIMITS] # 物理可動域 LIMIT_EPS_DEG = 1e-6 REACH_TOL_DEG = 3.0 # 届いたかの判定 (推論の到着確認と同じ 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(base): """基準の姿勢 base から, 各関節に送ってよい範囲 (lo, hi, 各端の出どころ) を作る. 出どころ: BASE (基準 ±15/±10) / PHYS (物理可動域) / J4_SAFE / J1_MAX. 狭い方が勝つ.""" lo, hi, lo_src, hi_src = [], [], [], [] for k in range(6): r = J0_RANGE_DEG if k == 0 else JOINT_RANGE_DEG cands_lo = [(float(base[k]) - r, "BASE"), (JOINT_LIMITS[k][0], "PHYS")] cands_hi = [(float(base[k]) + r, "BASE"), (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 = {"BASE": "基準の姿勢からの範囲", "PHYS": "物理可動域", "J4_SAFE": "J4_SAFE", "J1_MAX": "j1 の上限 90.87 (PICK_J1_FIXED)"} def plan_step(start, joint, delta, base): """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(base) 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): """target を 1 回だけ send_angles (speed 20) で送り, 3° 以内に届くかを読んで確かめる. gate が取り消されていれば送らない. 送り直しはしない. 確かめる途中で読めなければ error.""" target = np.asarray(target, dtype=float) 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() last, err = None, None base = {"sent": True, "timeout_sec": round(timeout, 2), "t_send": t_send, "measured_before_send": _r(pre)} 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)) 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): t_send = time.time() if not gate.run(lambda: None): return {"sent": False, "why": "halted_before_send", "measured_before_send": _r(self.q)} 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(rpc.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() 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.base = np.asarray(start_pose, dtype=float).copy() # 基準の姿勢 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 # ---- 見せる値 ---- @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, "base_pose": _r(self.base)}) 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 enter_stop(self, reason, button=None, kind="stop"): was = self.stopped self.stopped = True 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 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): sign = J0_LEFT_SIGN if button == BTN_LEFT else -J0_LEFT_SIGN self._request(self._entry(button, "move", joint=0, delta=sign * self.step), ("move", 0, sign * self.step)) return if button in (BTN_UP, BTN_DOWN): sign = UP_SIGN if button == BTN_UP else -UP_SIGN j = self.joint self._request(self._entry(button, "move", joint=j, delta=sign * self.step), ("move", j, sign * self.step)) 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 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",)) # ---- 送る仕事 ---- 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, "base": self.base.copy()} 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 = job q = self.arm.read() res["measured_before"] = _r(q) if q is None: res["error"] = "read_failed" 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, snap["base"]) 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) self.cmd_reached = bool(res.get("reached")) if job[0] == "home": self.base = np.array(HOME, dtype=float) # HOME を送ったあとは HOME を基準にし直す 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}°" 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}（待ち {rpc.GRIPPER_SLEEP:.1f} 秒のあと受付）") elif sent is True: r = res.get("reached") how = "届いた" if r else ("確かめる途中で終了" if r is None else "届かなかった") 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": self.out(f"[送らない] {label}: 停止が入ったので送りませんでした") else: self.out(f"[送らない] {label}") 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} base {self.base[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) 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 = +) 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) ensure_ssd_mounted() stamp = datetime.now().strftime("%Y%m%d_%H%M%S") run_stem = f"teleop_{stamp}" + ("" if args.execute else "_dry") 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}") 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(start) 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 = { "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, "J0_RANGE_DEG": J0_RANGE_DEG, "JOINT_RANGE_DEG": JOINT_RANGE_DEG, "J4_SAFE": list(J4_SAFE), "JOINT_LIMITS": [list(x) for x in JOINT_LIMITS], "window_at_start": {"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, "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(), } write_json_atomic(run_dir / "meta.json", meta) def on_controller(info): 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}") return 0 if __name__ == "__main__": sys.exit(main())