---
作業フォルダ: /Users/shimazueiichi/r2r2r-scripts
上限時間: 180
---
# 依頼68：ゲームパッドで mechArm を手で動かすスクリプト（tools/gamepad_teleop.py）を作る。実機には流さない（2026-11-02）

## ねらい

PICK で爪の位置がずれたときに、エイイチがゲームパッドで左右・上げ下げ・爪の開閉を直す。押した分だけそのまま動く単純な作りにする。**自動で動く要素は入れない**（自動の握り直し・失敗時の自動 HOME などは作らない）。押した操作と、送った角度・届いた角度をすべて記録し、あとで「人が直した記録」として使えるようにする。

## 前提（確かめ済み）

- コントローラー：Yommor の互換品。Mac に Bluetooth でつなぐと pygame では `PS4 Controller`（軸6・ボタン16・hat 0）として見える
- 読み取りの確認は `tools/gamepad_check.py` で済み。記録は `/Volumes/KIOXIA/r2r2r-data/diag/gamepad_check_20261102/`（README.md と button_map_confirmed.md にボタン番号の表がある）
- ボタン番号：0=× 1=○ 2=□ 3=△ 4=SHARE 5=HOME 6=OPTIONS 7=L3 8=R3 9=L1 10=R1 11=十字上 12=十字下 13=十字左 14=十字右 15=タッチパッド押し込み
- 軸：0/1 左スティック、2/3 右スティック、4=L2、5=R2（L2・R2 は離して −1.0、押し込んで +1.0）
- 左スティックを強く回すと L3（7）が一瞬入ることがある。スティックと L3・R3 は使わない

## 操作（エイイチが決めた。**変えない**）

| ボタン | 動き |
|---|---|
| 十字キー 左 / 右（13 / 14） | j0 を 0.5°ずつ。どの関節を選んでいても、いつも j0 |
| 十字キー 上 / 下（11 / 12） | 選んでいる関節を 0.5°ずつ。上 = −、下 = ＋（PICK の上空 → 床で j1・j2・j4 が増えるのと同じ向き。j1 では下を押すと爪が下がる）。始めは 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回だけ送る。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）は別の用途の値なので、ここでは使わない（エイイチと 90.87 で決めた）
- j0 は「基準の姿勢」から ±15° まで。j1〜j5 は ±10° まで
- j4 は J4_SAFE（−90〜+60）の中だけ。各関節の物理的な可動域（今のコードにある値）も通す
- 基準の姿勢 = 起動したときに読んだ姿勢。HOME に戻ったあとは HOME を基準にし直す
- 範囲で切られたら、切られた理由を画面とログに出す。勝手に別の関節で補わない
- HOME だけは範囲の検査を通さずに送れる（run_policy_client.py の h と同じ扱い。範囲の外にいるアームを戻す手段として残す）

## 送り方

- `send_angles` だけを使う（`send_coords` 禁止）。speed 20
- daemon への送り方・読み方・爪の開閉は、今あるものを使う：`visual_servo_pick` の `get_daemon_socket` / `daemon_send_angles` / `daemon_get_angles`、爪は `pi05/run_policy_client.py` の `gripper_cmd`（閉 = `daemon_set_gripper_value(20)`、開 = `daemon_set_gripper(0, ...)`）と同じ。他のスクリプトと同時に daemon を使わないための仕組み（`install_daemon_lock` など）があれば同じように使う
- 1回押すごとに目標を1つ作って送り、届いたか（3° 以内）を確かめる。`pi05/run_policy_client.py` の補正（`plan_correction_step`・`apply_limits`・`check_send_safe`）の作りを参考にしてよい
- 送ってから届くまで（または待ちの上限まで）は、次の動く操作を受け付けない（ためておかない）。停止だけはいつでも受け付ける
- 爪の開閉のあとは、run_policy_client.py と同じ待ち（GRIPPER_SLEEP）を置き、その間は動く操作を受け付けない

## 停止について

- この依頼でやるのは「新しい指令を送らない＋ OPTIONS まで受け付けない」まで
- **動いている途中（特に HOME のような大きい動き）を止める方法は作らずに調べて報告する**：Pi で動いている `/home/ubuntu/mecharm_daemon.py`（リポジトリの写しには stop の命令がない）に止める命令があるか、pymycobot の `stop()`、今の角度を読んで送り直す案の得失。daemon は変えない
- 次のときも停止と同じ状態にする：コントローラーの接続が切れたとき（JOYDEVICEREMOVED）、読み取りに失敗したとき
- Ctrl+C・ウィンドウを閉じたときは、新しい指令を送らずに終わる。終わるときにアームを動かさない（HOME 復帰もサーボ解放もしない。run_policy_client.py の finally と同じ）

## 画面

- pygame のウィンドウに大きく出す：今動かす関節（j1 など）・刻み（0.5° / 2°）・状態（待機 / 動いている / 停止中）・6関節の今の角度・爪の状態・直前に範囲で切られた理由
- ウィンドウの文字は英数字だけ（日本語フォントがなくても化けないように。gamepad_check.py と同じ）。ターミナルには日本語で1行ずつ出す。関節を切り替えたときもターミナルに出す

## 記録

- 置き場所：`/Volumes/KIOXIA/r2r2r-data/runs/experiments/gamepad_teleop/{run_stem}/`（MASTER の保存先テーブルの「実機の単体実験」。通しの完走ではないので台帳には書かない）
- 書く前に KIOXIA のマウントを確かめる（`ensure_ssd_mounted()` などの今ある仕組み）。マウントされていなければ止めて知らせる。ローカルに逃がさない
- `log.jsonl`：押した操作1つにつき1行。時刻・ボタン・操作の種類・選んでいる関節・刻み・送る前に読んだ角度・送った角度（command）・届いた角度（reached）・届いたか・爪の状態・範囲で切られた中身・停止の状態
- `meta.json`：コントローラーの名前と GUID・Python / pygame / SDL の版・起動したときの姿勢・守りの値・git のコミット・`--execute` の有無
- 測定データと対になるコードとして、使ったスクリプトの写しを同じフォルダに置く
- 1行書くたびに flush する（途中で止まっても残るように）

## 実行のしかた

- 既定はドライラン：daemon にもアームにも触らず、HOME から始まる仮の姿勢を動かす。エイイチがアームなしでコントローラーの操作を試せるようにする
- `--execute` を付けたときだけ daemon に送る

## 確かめ（実機なし）

テストを足す：
- ボタンと操作の対応が上の表のとおりか（番号の取り違えがないか）
- 十字キー上下の向き（上 = −、下 = ＋）と、△ で j1 → j5 → j1 と回ること
- 守り：j1 が 90.87 を超えない、j0 ±15°・j1〜j5 ±10°、J4_SAFE。HOME のあと基準が HOME になること
- 停止：L2・R2 の両方が 0.0 を超えたら止まる。片方だけでは止まらない。停止中は動く操作を受け付けず、OPTIONS で戻る。接続が切れたら停止になる
- HOME：1秒に満たない長押しでは送らない。1秒続けたら1回だけ送る
- 動いている間の押しは受け付けない（ためない）
- ドライランで流れがひととおり動くこと

## 変えないもの

- daemon（Pi 側も写しも）、run_policy_client.py・visual_servo_pick.py などの既存のスクリプトと定数、tools/gamepad_check.py
- 既存のデータ

## 終わったら

- 関節を動かすコードなので、codex のレビューを通してから報告する（code_review_request_template.md の形）。指摘は直すか、直さない理由を書く
- **実機には流さない**。報告に、エイイチが最初に試す手順を書く：①ドライランでコントローラーの操作を一通り試す ②アームを HOME に置いて `--execute`、j0 を 0.5° ずつ数回 ③停止（L2+R2）と OPTIONS での再開 ④爪の開閉 ⑤HOME の長押し
- 停止の調べた結果（上の「停止について」）を「決めてほしいこと」に書く
- 個別のファイルを指定して add・commit する（`git add .` は使わない）