commit d6f5823896197ca1a924069988015a2a50fc487e Author: Eiichi Shimazu Date: Wed Nov 11 11:34:03 2026 +0900 爪の閉じる値を変えられるようにし, 締め直し (r) を前に送った値と違う値にし, 位置5 のお手本を入れ替える準備をする (依頼 152・Log30). run_policy_client.py に --grip-value (0〜100. 既定 20 のまま) を足し, 推論の閉じる指令・補正の g / r / R・巻き戻し・補正の取り消し・ゲームパッドの補正の閉じる指令をすべてこの値にする. log.jsonl の頭 (type=model) に grip_value. 推論の終わりに閉じて 1〜2 秒の爪の電流の中央値を閉じた命令ごとに 1 行で出す (grip_current.after_close. 表示だけ). r は開かずに (閉じる値 − 3) と閉じる値を交互に送る (補正の中と外. 0 より下にしない. regrip_tighten に grip_value_sent・prev_close_value). R は今のまま. gamepad_teleop.py を単独で動かすときも --grip-value. lab_start.sh に --grip-value N を足しクライアントに渡す ([GRIP ] で使う値を出す). pi05/excludes/deep_even10_v2.json (deep_even10_v1 から位置5 の補正入り 6 本を外す. count_by_position.py で位置5・補正入りを確かめた) と DEEP_DEMOS.md の 5 節 (位置5 を教えた動きの再生で 6 本撮り, deep_even10_v2 を作る手順). テストを足す (test_rewind_correct 81・test_pad_correction 24・test_gamepad_teleop 93・test_teach_currents 27・lab_start 197). record_teach_demo.py の run --grip-value は git の外 (依頼 152. codex 2 回) Co-Authored-By: Claude Opus 5.5 diff --git a/lab_start.sh b/lab_start.sh index 12dfc79..606745f 100755 --- a/lab_start.sh +++ b/lab_start.sh @@ -34,6 +34,8 @@ # トンネルを閉じて終わる (Pod は消さない. 片付けは ./lab_stop.sh). 録画の画像で答えを出させる # pi05/tilt_ab_test.py などのため (依頼 146) # --no-tilt-fix 傾きをソフトでそろえない (camera_align.py と run_policy_client.py に --no-tilt-fix を渡す. 依頼 144) +# --grip-value N 爪の閉じる値 (0〜100. run_policy_client.py に --grip-value を渡す. 付けなければ今までどおり +# run_policy_client.py の既定 20. 起動時に [GRIP ] で使う値を出す. 依頼 152) # つなぎ直しで, 残っている Pod のサーバーが別のモデルで動いていれば, そのことを出して止まる # # 1. Pod を作る (A40×1・Runpod Pytorch 2.8.0・コンテナのディスク 200GB・22/tcp を外へ出す) @@ -75,6 +77,8 @@ DRY_RUN=0 SERVE_ONLY=0 # 依頼 146: 1〜5 (サーバーとトンネル) だけ. アームと daemon の段は通らない MODEL=trim TILT_FIX_ARGS=() # 依頼 144: 既定は傾きをソフトでそろえる. --no-tilt-fix で回さない +GRIP_ARGS=() # 依頼 152: --grip-value N のときだけ run_policy_client.py に --grip-value N を渡す +GRIP_VALUE_SHOWN="20 (既定. run_policy_client.py の GRIP_VALUE)" while [ $# -gt 0 ]; do case "$1" in --gpu) lab_need_val A40 "$@"; GPU="$2"; shift 2 ;; @@ -83,9 +87,14 @@ while [ $# -gt 0 ]; do --dry-run) DRY_RUN=1; shift ;; --serve-only) SERVE_ONLY=1; shift ;; --no-tilt-fix) TILT_FIX_ARGS=(--no-tilt-fix); shift ;; + --grip-value) [ $# -ge 2 ] || { echo "[ERROR] --grip-value には 0〜100 の整数を付ける" >&2; exit 2; } + case "$2" in ''|*[!0-9]*) echo "[ERROR] --grip-value は 0〜100 の整数: $2" >&2; exit 2 ;; esac + # 桁数を先に見る (大きな数は算術展開であふれて小さな値になる. codex R1) + [ ${#2} -le 3 ] && [ "$((10#$2))" -le 100 ] || { echo "[ERROR] --grip-value は 0〜100 の整数: $2" >&2; exit 2; } + GRIP_ARGS=(--grip-value "$((10#$2))"); GRIP_VALUE_SHOWN="$((10#$2)) (--grip-value)"; shift 2 ;; --model) [ $# -ge 2 ] || { echo "[ERROR] --model には ${LAB_MODELS// / か } を付ける" >&2; exit 2; } # 値がないと shift 2 が失敗して回り続ける (codex R1) MODEL="$2"; shift 2 ;; - -h|--help) sed -n '2,52p' "$0"; exit 0 ;; + -h|--help) sed -n '2,54p' "$0"; exit 0 ;; *) echo "[ERROR] 知らない引数: $1 (./lab_start.sh --help)" >&2; exit 2 ;; esac done @@ -133,6 +142,7 @@ if [ ${#TILT_FIX_ARGS[@]} = 0 ]; then else echo "[TILT ] 上カメラの傾きのお手本 (各エピソードの 1 枚目の中央値) は $RAW_RANGE から (安全範囲と同じ. --no-tilt-fix なので回さない. 依頼 142)" fi +echo "[GRIP ] 爪の閉じる値は $GRIP_VALUE_SHOWN. 締め直し r はこの値−3 とこの値を交互に送る (依頼 152)" lab_ssh_opts HF_REPO=$MODEL_REPO # --model で選んだモデル (lab_lib.sh の lab_model_def) @@ -493,13 +503,13 @@ while :; do stage "8. 実機推論 ($round 回目)" python3 pi05/run_policy_client.py --raw-dir "$RAW_RANGE" --steps-per-chunk 10 --via-stride 3 \ --settle-retries 0 --state-from-target --execute --ignore-framing --model "$MODEL_TAG" --framing-raw-dir "$RAW" \ - --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} rc=$? else stage "8. 推論のクライアント (ドライラン・--dry-keys. アームには触らない)" python3 pi05/run_policy_client.py --raw-dir "$RAW_RANGE" --steps-per-chunk 10 --via-stride 3 \ --settle-retries 0 --state-from-target --dry-keys --ignore-framing --model "$MODEL_TAG" --framing-raw-dir "$RAW" \ - --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} + --tilt-raw-dir "$RAW_RANGE" ${TILT_FIX_ARGS[@]+"${TILT_FIX_ARGS[@]}"} ${GRIP_ARGS[@]+"${GRIP_ARGS[@]}"} rc=$? fi echo "[CLIENT] run_policy_client.py が終わった (終了コード $rc. モデル $MODEL_TAG)" diff --git a/pi05/DEEP_DEMOS.md b/pi05/DEEP_DEMOS.md index b95de27..8f1aef8 100644 --- a/pi05/DEEP_DEMOS.md +++ b/pi05/DEEP_DEMOS.md @@ -140,3 +140,56 @@ python3 pi05/check_dataset.py /Volumes/KIOXIA/r2r2r-data/datasets/<新しい名 - 放す(開く)と −50 前後になる。閉じたあとの値に負の値が混じっていたら、開いたあとの値を拾っている(爪の命令の時刻を `states.csv` で見る) - 線 100 は空(15 前後)とつかんだ値(数百)の間に置いた目安で、再生の記録ではまだ実機で確かめていない。最初の数本で実際の値を見て、合わなければ `pi05/grip_current.py` の `GRASP_LINE` を見直す(外す決めには使っていない) + +## 5. 位置5 を撮り足す(6 本。2026-11-11 依頼 152。**データはまだ撮っていない**) + +**なぜ**: deep1 の実機推論(2026-11-11 Log30)で位置5 は 2/9。つかんだあとに落とすものが多かった。deep_even10_v1 の位置5 の 10 本のうち 6 本は補正入り(人が途中で直した本)で、持ち上げ方が教えた動きの 4 本とそろっていない。エイイチの決め(2026-11-11): 位置5 の補正入り 6 本を、教えた動きの再生で撮る深いお手本に入れ替える(持ち上げをそろえるため)。 + +**外す 6 本**(`pi05/excludes/deep_even10_v2.json`。理由と、`count_by_position.py` で確かめた位置を一緒に書いてある): + +| エピソード | 種類 | 位置(`count_by_position.py mecharm_redblock_box_deep_even10_v1`、2026-11-11) | +| --- | --- | --- | +| pi05_20261031_175527 | 補正入り | 位置 5(位置 5 まで 16.3mm) | +| pi05_20261031_180435 | 補正入り | 位置 5(13.8mm) | +| pi05_20261031_184016 | 補正入り | 位置 5(6.1mm) | +| pi05_20261101_190347 | 補正入り | 位置 5(13.6mm) | +| pi05_20261106_183926 | 補正入り | 位置 5(4.0mm) | +| pi05_20261106_184650 | 補正入り | 位置 5(0.5mm) | + +### 5-1. 撮る(位置5・教えた記録 `drag_v2_20221013_233323.json`) + +撮る前は 1. と同じ(recorder.py を常駐・印に合わせて置く・daemon に read_servo)。**爪の閉じる値**は、実機推論で使うと決めた値(依頼 152 の実機の試しで決める。25〜30 あたりの見込み)を `--grip-value` に付ける。付けなければ今までどおり 20。使った値は試行 JSON(`{run}/{run}.json`)の `grip_value` と `constants.GRIP_VALUE` に残る。 + +``` +# ブロックを位置 5 の印に置く +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py measure # 1 本ごとに置き直したら測り直す +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json --grip-value N # ドライラン(計画だけ。閉じる値 N が出る) +python3 dynamic_tracking_limits/drag_teaching/record_teach_demo.py run --teach drag_v2_20221013_233323.json --grip-value N --execute # 再生+記録 +``` + +**1 本目を撮ったら、深さを見てから続ける**(2-0 と同じ。24px を超えたら止めて置き直し): + +``` +python3 pi05/grasp_depth.py /Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_YYYYMMDD_HHMMSS +``` + +- 2 行目(爪の値)で、持ち上げたあとに力が抜けていないか(中央値が 0 付近に落ちていないか)も見る。落ちていたら `top.mp4` で持ち上げの間にブロックが残っているかを見る +- 24px 以下・つかめていそう なら残り 5 本を撮る(浅くて外れた分は撮り足して、合わせて 6 本) + +### 5-2. データにする(新しい 6 本 → deep_even10_v1 の残り 44 本と合わせて deep_even10_v2) + +``` +# 新しい 6 本だけのデータセット(24px を超えた本は外し、index.json の grasp_depth に値と理由を残す) +python3 pi05/build_demo_episodes.py --dataset mecharm_redblock_box_teach_pos5_deep_v1 --trim-mid-idle \ + --max-protrude-px 24 --run run_0001_YYYYMMDD_HHMMSS --run ... +# deep_even10_v1 から補正入り 6 本を外し (pi05/excludes/deep_even10_v2.json), 新しい 6 本と合わせる +python3 pi05/merge_raw_episodes.py --src mecharm_redblock_box_deep_even10_v1 \ + --src mecharm_redblock_box_teach_pos5_deep_v1 --dataset mecharm_redblock_box_deep_even10_v2 \ + --exclude-json pi05/excludes/deep_even10_v2.json --tar +python3 pi05/check_dataset.py /Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_deep_even10_v2/raw_episodes +# 位置ごとに 10 本 (位置 5 は教えた動き 10 本・補正入り 0 本) になったかを見る (読むだけ) +python3 pi05/count_by_position.py mecharm_redblock_box_deep_even10_v2 +``` + +- `count_by_position.py` の表で、位置 1〜5 がどれも 10 本・「目標 10 本まで」が +0・位置 5 の補正入りが 0・合計 50 本(教えた動き 50)になっていればよい。位置 5 が 10 本に足りなければ 5-1 に戻って撮り足し、5-2 を `--force` で作り直す +- 学習は、データができてから `lab_train.sh` に新しい `--model` を足す(この依頼では設定を作らない) diff --git a/pi05/excludes/deep_even10_v2.json b/pi05/excludes/deep_even10_v2.json new file mode 100644 index 0000000..aa0358b --- /dev/null +++ b/pi05/excludes/deep_even10_v2.json @@ -0,0 +1,98 @@ +{ + "pi05_20261031_175527": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 1182) → 位置 5 まで 16.3mm", + "block_mm": [ + -53.5, + 230.8 + ], + "dist_mm": 16.3 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + }, + "pi05_20261031_180435": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 516) → 位置 5 まで 13.8mm", + "block_mm": [ + -61.9, + 242.1 + ], + "dist_mm": 13.8 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + }, + "pi05_20261031_184016": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 387) → 位置 5 まで 6.1mm", + "block_mm": [ + -52.8, + 241.7 + ], + "dist_mm": 6.1 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + }, + "pi05_20261101_190347": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 1245) → 位置 5 まで 13.6mm", + "block_mm": [ + -62.4, + 246.8 + ], + "dist_mm": 13.6 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + }, + "pi05_20261106_183926": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 951) → 位置 5 まで 4.0mm", + "block_mm": [ + -47.2, + 242.7 + ], + "dist_mm": 4.0 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + }, + "pi05_20261106_184650": { + "reason": "位置5 の持ち上げをそろえるため、教えた動きの再生に入れ替える(エイイチの決め 2026-11-11)", + "kind": "補正入り", + "position": 5, + "position_check": { + "by": "pi05/count_by_position.py mecharm_redblock_box_deep_even10_v1 (2026-11-11 依頼 152)", + "how": "top.mp4 のブロック (コマ 174) → 位置 5 まで 0.5mm", + "block_mm": [ + -48.3, + 246.2 + ], + "dist_mm": 0.5 + }, + "replaced_by": "位置5 の教えた動き drag_v2_20221013_233323.json の再生 (pi05/DEEP_DEMOS.md「位置5 を撮り足す(6 本)」)", + "from_dataset": "mecharm_redblock_box_deep_even10_v1" + } +} diff --git a/pi05/grip_current.py b/pi05/grip_current.py index a215405..2215d73 100644 --- a/pi05/grip_current.py +++ b/pi05/grip_current.py @@ -16,11 +16,16 @@ python3 pi05/grip_current.py RUN_DIR [RUN_DIR ...] (grasp_depth.py も同じ run で深さの行の下にこの 1 行を出す) +閉じて 1 秒後の値 (依頼 152): after_close_line は, 開いた状態 (または始め) から閉じた命令ごとに, + AFTER_CLOSE_FROM_SEC〜+AFTER_CLOSE_SPAN_SEC 秒 (次に開く命令まで) の gripper_current の中央値を 1 行で出す. + run_policy_client.py が推論の終わりに出す (表示だけ). + 読むだけ (記録には何も書かない). """ import argparse import csv import json +import statistics import sys from pathlib import Path @@ -28,6 +33,8 @@ import grasp_depth as gd GRASP_LINE = 100 # これ以上なら「つかめていそう」(空 15 前後と つかんだ 数百 の間. 目安) HELD_FROM_SEC = 1.0 # 閉じる命令からこの秒数より後の値を見る (閉じている途中の値を混ぜない) +AFTER_CLOSE_FROM_SEC = 1.0 # 閉じて 1 秒後の値 (依頼 152): 閉じる命令からこの秒数〜 +AFTER_CLOSE_SPAN_SEC = 1.0 # + この秒数 の間の中央値 def summarize(run_dir): @@ -85,6 +92,65 @@ def summarize(run_dir): return out +def _read_currents(cur): + """currents.csv の (時刻, gripper_current) の並び (空の値・読めない行は飛ばす).""" + out = [] + with Path(cur).open(newline="") as f: + for row in csv.DictReader(f): + try: + t, v = float(row["ts"]), row.get("gripper_current") or "" + if v != "": + out.append((t, int(v))) + except (KeyError, TypeError, ValueError): + continue + return out + + +def after_close(run_dir, from_sec=AFTER_CLOSE_FROM_SEC, span_sec=AFTER_CLOSE_SPAN_SEC): + """閉じて from_sec 秒後から span_sec 秒の gripper_current の中央値を, 閉じた命令ごとに返す (依頼 152). + 閉じた命令 = 開いた状態 (または始め) から閉じた命令 (閉じたままの締め直しは数えない). + 戻り値 dict: status (ok / no_currents / no_close / error) と, ok なら closes = [{t_close, median, n}, ...] + (n = 0 なら median は None).""" + rec = Path(run_dir) / "recording" + out = {"status": None, "closes": [], "reason": None} + cur = rec / "currents.csv" + if not cur.exists(): + out.update(status="no_currents", reason="currents.csv がない") + return out + try: + gt, gs = gd.load_gripper_cmds(rec / "states.csv") + vals = _read_currents(cur) + except Exception as e: + out.update(status="error", reason=f"読めない ({type(e).__name__})") + return out + prev = 0.0 + for t, s in zip(gt, gs): + if s == 1 and prev == 0: + opens = [float(u) for u, w in zip(gt, gs) if w == 0 and u > t] + lo, hi = float(t) + from_sec, min([float(t) + from_sec + span_sec] + opens) + vs = [v for ts, v in vals if lo <= ts < hi] + # 偶数行は真ん中の 2 つの平均 (片方が力の抜けた 0 付近でも高い方だけを見せない. codex R1) + out["closes"].append({"t_close": round(float(t), 3), "n": len(vs), + "median": statistics.median(vs) if vs else None}) + prev = s + if not out["closes"]: + out.update(status="no_close", reason="閉じる命令がない") + else: + out["status"] = "ok" + return out + + +def after_close_line(run_dir, a=None): + """推論の終わりに出す 1 行 (依頼 152).""" + a = a if a is not None else after_close(run_dir) + head = f"閉じて {AFTER_CLOSE_FROM_SEC:.0f} 秒後の爪の電流 (中央値)" + if a["status"] != "ok": + return f"{head}: 値なし ({a['reason']})" + parts = [f"{k + 1} 回目 " + ("値なし" if c["median"] is None else f"{c['median']:g} ({c['n']} 行)") + for k, c in enumerate(a["closes"])] + return f"{head}: " + " / ".join(parts) + + def line(run_dir, s=None): """表示の 1 行.""" s = s if s is not None else summarize(run_dir) diff --git a/pi05/rewind_correct.py b/pi05/rewind_correct.py index b1ec69e..262b35b 100644 --- a/pi05/rewind_correct.py +++ b/pi05/rewind_correct.py @@ -9,6 +9,7 @@ build_correction_episodes.py の両方から使い, test_rewind_correct.py で - parse_correction_key: 補正モードのキー 1 文字を操作に読み替える - correction_reach_tol / correction_joint_reached: 補正の 1 刻みが着いたかの判定 (動かした関節だけ) - correction_shortfall / correction_carry_used: 届かなかった分を次の 1 刻みに足す量 + - tighten_value: 締め直し (r) で送る閉じる値 (前に送った値と違う値. 依頼 152) - excluded_windows / step_keep_mask: 学習データから除く時間の窓 (巻き戻しで取り消した区間) """ import math @@ -41,10 +42,22 @@ GRASP_HELD = "held" # R を使った補正の巻き戻しで y = 持っ # 推論の中継点・巻き戻し・HOME の判定 (3.0° / 2.5°) とは別 CORRECTION_REACH_MIN_TOL_DEG = 0.5 +# 締め直し (r) は前に送った閉じる値と違う値を送る (同じ目標だとサーボが動かないことがある. 2026-11-11 依頼 152). +# 閉じる値 − TIGHTEN_DELTA と 閉じる値 を交互に送る +TIGHTEN_DELTA = 3 + + +def tighten_value(last_sent, grip_value, delta=TIGHTEN_DELTA): + """締め直し (r) で送る閉じる値. 前に送った閉じる値 last_sent が (grip_value − delta) なら grip_value, + それ以外 (grip_value・まだ送っていない None など) なら (grip_value − delta). 0 より下にはしない.""" + low = max(0, int(grip_value) - int(delta)) + return int(grip_value) if last_sent is not None and int(last_sent) == low else low + + CORRECTION_HELP = ( " [補正] 1〜6=関節を選ぶ (j0〜j5) / + か . = 選んだ関節を +1 刻み / - か , = -1 刻み\n" " [ ] = 刻みを 0.5° / 1° / 2° で切り替え / g = グリッパー開閉\n" - " r = 爪を開かずに締め直す / R = 爪を開いて閉じ直す (つかみ直し). どちらも閉じているときだけ\n" + " r = 爪を開かずに締め直す (閉じる値−3 と 閉じる値 を交互に送る) / R = 爪を開いて閉じ直す (つかみ直し). どちらも閉じているときだけ\n" " Enter = 補正を終えてモデルの推論に戻る / b = 補正を取り消して始めの姿勢へ戻る\n" " ? = この一覧" ) diff --git a/pi05/run_policy_client.py b/pi05/run_policy_client.py index d1eef42..e92eaed 100644 --- a/pi05/run_policy_client.py +++ b/pi05/run_policy_client.py @@ -60,6 +60,8 @@ 1 刻みごとに log.jsonl へ {"type": "correction", ...} r = 爪を開かずに閉じる指令をもう一度 (締め直し) / R = 開いてから閉じ直す (つかみ直し). どちらも爪が閉じているときだけ (開いていれば何もしない. 閉じるのは g). 2026-10-30 依頼 34 + r は前に送った閉じる値と違う値を送る: 1 回目は (閉じる値 − 3), 次は閉じる値, と交互 (同じ目標だと + サーボが動かないことがある. 2026-11-11 依頼 152). log.jsonl の regrip_tighten に grip_value_sent 1 刻みの「着いた」は, 止まるのを待ってから 動かした関節だけ を 刻みの半分 (下限 0.5°) で見る (表示と記録の joint_reached だけ. 送り方・再送・未到達のときの数え直しは今までどおり 3°) 届いていないときは, 同じ関節・同じ向きの次の 1 刻みに 足りなかった分 を足す (上限は刻みの大きさ. @@ -88,6 +90,10 @@ - --dry-keys: ドライランでも 1 回ごとに止まり b / c を試せる (アームにも daemon にも触らない. 仮の姿勢を HOME から動かして流れだけ確かめる. Enter で「仮に実行」= 仮の姿勢が目標へ移る) +爪の閉じる値 (2026-11-11 依頼 152): --grip-value (既定 GRIP_VALUE = 20). 推論の閉じる指令・補正の g / r / R・ + 巻き戻し・ゲームパッドの補正の閉じる指令はすべてこの値. log.jsonl の頭 (type=model) に grip_value を残す. + 推論の終わりに, 閉じて 1 秒後の爪の電流 (recording/currents.csv の gripper_current の中央値) を 1 行で出す (表示だけ) + 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 本線と共有の定数は変えず, ここで上書きする): --send-wait 0.0 (前 1.0) / --grip-wait 0.8 (前 2.0) / --grip-open-wait 0.0 (前 2.0) / --settle-wait 0.8 (前 2.0) 前の値に戻すには --send-wait 1.0 --grip-wait 2.0 --grip-open-wait 2.0 --settle-wait 2.0 @@ -151,6 +157,7 @@ from camera_tilt import ( # noqa: E402 依頼 142・144 TILT_FIX_BORDER, TILT_FIX_FRAMES, fix_angle, fix_text, median_tilt, reference_tilt, rotate_frame, tilt_text, ) from fk_lookup import J1_SAFE_MAX # noqa: E402 88.0. 本線と二重定義しない +from grip_current import after_close_line # noqa: E402 依頼 152 from j0_sweep_scan import SEND_WAIT_SEC, SPEED, _send_and_wait_arrival # noqa: E402 本線と同じ 20 / 1.0 (待ちは下の SEND_WAIT で上書き) from policy_recorder import PolicyRecorder, daemon_read_servo, install_daemon_lock # noqa: E402 from policy_ws import PolicyClient # noqa: E402 @@ -158,7 +165,7 @@ from rewind_correct import ( # noqa: E402 CORRECTION_DEFAULT_STEP_INDEX, CORRECTION_HELP, CORRECTION_STEPS, GRASP_DROPPED, GRASP_PLACED, MODE_CORRECT, GRASP_HELD, MODE_HOME, MODE_MODEL, MODE_REGRIP, MODE_REWIND, MODE_WAIT, correction_carry_used, correction_joint_reached, correction_reach_tol, correction_shortfall, next_step_index, parse_correction_key, - rewind_waypoints, split_leg, + rewind_waypoints, split_leg, tighten_value, ) from visual_servo_pick import ( # noqa: E402 close_daemon_socket, daemon_get_angles, daemon_send_angles, daemon_set_gripper, @@ -168,7 +175,10 @@ from visual_servo_pick import ( # noqa: E402 TASK = "put the red block in the white box" IMG_LONG = 384 # build_demo_episodes.py と同じ (学習画像の長辺) HOME = [0.0, 0.0, 0.0, 0.0, 0.0, -30.0] -GRIP_VALUE = 20 # run_pick_place.py の close 値 +GRIP_VALUE_DEFAULT = 20 # run_pick_place.py の close 値 (--grip-value を付けないとき. 依頼 152) +GRIP_VALUE = GRIP_VALUE_DEFAULT # 閉じる値 (--grip-value で変える. 依頼 152) +GRIP_VALUE_RANGE = (0, 100) # --grip-value に受ける値 (set_gripper_value の範囲) +LAST_CLOSE_VALUE = None # 最後に送った閉じる値 (締め直し r で違う値を選ぶため. 依頼 152) # ---- 待ち時間 (2026-10-30 依頼 35. エイイチの判断で縮めた. 「time.sleep() を短縮しない」より優先) ---- # 本線 (j0_sweep_scan.py / visual_servo_pick.py) と共有の定数は変えず, この中でだけ上書きする. # コマンドの引数 (--send-wait / --grip-wait / --grip-open-wait / --settle-wait) で前の値に戻せる. @@ -489,12 +499,26 @@ def settle_target(target, label, tol, retries): return out, sends -def gripper_cmd(state): +def note_close_value(value): + """最後に送った閉じる値を覚える (締め直し r が次に送る値を決める. 依頼 152).""" + global LAST_CLOSE_VALUE + LAST_CLOSE_VALUE = int(value) + + +def send_grip_close(value=None): + """閉じる指令を 1 回送る (待たない). value を省くと GRIP_VALUE (--grip-value). 戻り値: 送った値.""" + v = GRIP_VALUE if value is None else int(value) + daemon_set_gripper_value(v) + note_close_value(v) + return v + + +def gripper_cmd(state, value=None): """爪の指令と, そのあとの待ち (推論・補正・巻き戻しで共通). - 閉 = daemon_set_gripper_value(GRIP_VALUE) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). + 閉 = daemon_set_gripper_value(GRIP_VALUE. value を渡せばその値) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ (依頼 35 で縮めた. 引数で前の値に戻せる).""" if state == 1: - daemon_set_gripper_value(GRIP_VALUE) + send_grip_close(value) else: daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT) time.sleep(GRIPPER_SLEEP) @@ -546,13 +570,25 @@ class ArmIO: return q, bool(still) return (None if self.sim_q is None else self.sim_q.copy()), True - def gripper(self, state): + def gripper(self, state, value=None): + """value は閉じるときの値 (省くと GRIP_VALUE. 締め直し r だけが渡す. 依頼 152).""" if self.recorder is not None: self.recorder.gripper(state) if not self.execute: + if state == 1: + note_close_value(GRIP_VALUE if value is None else value) print(f" [DRY ] グリッパー→{'閉' if state == 1 else '開'} (送らない)") return - gripper_cmd(state) + gripper_cmd(state, value) + + +def regrip_tighten(arm): + """締め直し (r. 補正の中と外で共通): 開かずに, 前に送った閉じる値と違う値を送る (依頼 152). + 戻り値: log.jsonl の regrip_tighten に足す項目 (送った値・閉じる値・前に送った値).""" + prev = LAST_CLOSE_VALUE + v = tighten_value(prev, GRIP_VALUE) + arm.gripper(1, value=v) + return {"grip_value_sent": v, "grip_value": GRIP_VALUE, "prev_close_value": prev} class KeyReader: @@ -860,7 +896,8 @@ def run_correction(arm, keys, lo, hi, max_move, grip, corr_id, chunk, log_f, rec keys.flush() elif kind == "regrip": # 依頼 34: r = 開かずに閉じる指令をもう一度 / R = 開く → 閉じる (その場でつかみ直す). - # 閉じる・開くは g と同じ arm.gripper (値 GRIP_VALUE・速さ・待ち GRIPPER_SLEEP も同じ) + # 閉じる・開くは g と同じ arm.gripper (速さ・待ち GRIPPER_SLEEP も同じ). R の値は GRIP_VALUE. + # r は前に送った閉じる値と違う値 (閉じる値 − 3 と 閉じる値 を交互. 依頼 152) action = "regrip_tighten" if val == "tighten" else "regrip_reopen" e = {"type": "correction", "action": action, "where": "correction", "correction_id": corr_id, "after_chunk": chunk, "t": time.time(), "key": "r" if val == "tighten" else "R", "before": grip} @@ -869,8 +906,8 @@ def run_correction(arm, keys, lo, hi, max_move, grip, corr_id, chunk, log_f, rec print(f" [補正] 爪が開いているので{what}はしない(閉じるのは g)") e.update(after=grip, sent=False, skipped="gripper_open") elif val == "tighten": - print(" [補正] 爪を締め直します (開かずに閉じる指令をもう一度)") - arm.gripper(1) + print(f" [補正] 爪を締め直します (開かずに閉じる値 {tighten_value(LAST_CLOSE_VALUE, GRIP_VALUE)} を送る)") + e.update(regrip_tighten(arm)) e.update(after=1, sent=True) else: print(" [補正] 爪を開いて, 閉じ直します (つかみ直し)") @@ -985,7 +1022,7 @@ def run_pad_correction(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): if recorder is not None: recorder.gripper(state) if state == 1: - daemon_set_gripper_value(GRIP_VALUE) + send_grip_close() # GRIP_VALUE (--grip-value. 依頼 152) else: daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT) sent = gate.run(_send) @@ -998,7 +1035,9 @@ def run_pad_correction(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): plog = PadLog(log_f, corr_id, chunk) 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" + # 閉じる値はこの (推論の) プロセスが送る. ゲームパッドのプロセス (--pad-io) には題名で渡して画面に出すだけ (依頼 152) + title = (f"PAD CORRECTION {corr_id} [{'EXECUTE' if arm.execute else 'DRY'}] grip close {GRIP_VALUE} " + "square: back to policy") connected = {} def on_connected(info): @@ -1033,6 +1072,8 @@ def run_pad_correction(arm, grip, corr_id, chunk, log_f, recorder, lo, hi): 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 + if grip_rows and new_grip == 1: + note_close_value(GRIP_VALUE) # ゲームパッドで閉じた値 (ドライランでも. 次の r はこれと違う値. 依頼 152) # 送る仕事が戻っていなければ daemon に触らない (その仕事が通信の鍵を持ったままかもしれない. codex R2). # 呼び出し側が pad_worker_done=False を見て推論を止める qa = arm.read() if worker_done else None @@ -1140,6 +1181,22 @@ def apply_wait_args(args): f"{GRIP_OPEN_DAEMON_WAIT:.1f}s) / 止まるのを待つ目安 {SETTLE_WAIT_SEC:.1f}s") +def apply_grip_args(args): + """--grip-value を GRIP_VALUE へ入れる (依頼 152). 指定がなければ GRIP_VALUE_DEFAULT (20). 決まった値は args にも + 書き戻す ({run_stem}.json の args に残る). 最後に送った閉じる値 (締め直し r 用) はここで忘れる (試行ごとに始めから).""" + global GRIP_VALUE, LAST_CLOSE_VALUE + v = getattr(args, "grip_value", None) + if v is not None and not (GRIP_VALUE_RANGE[0] <= v <= GRIP_VALUE_RANGE[1]): + sys.exit(f"[ERROR] --grip-value は {GRIP_VALUE_RANGE[0]}〜{GRIP_VALUE_RANGE[1]} の整数で指定する (指定: {v})") + GRIP_VALUE = GRIP_VALUE_DEFAULT if v is None else int(v) + LAST_CLOSE_VALUE = None + args.grip_value = GRIP_VALUE + low = tighten_value(GRIP_VALUE, GRIP_VALUE) + print(f"[GRIP ] 閉じる値 {GRIP_VALUE}{'' if v is not None else ' (既定)'} " + + (f"締め直し r は {low} と {GRIP_VALUE} を交互に送る" if low != GRIP_VALUE else + f"締め直し r も {GRIP_VALUE} のまま (0 より下にしないので値を変えられない)")) + + def decide_tilt_fix(frames, tilt_dir: Path, enabled: bool) -> dict: """始めの数コマ (生の画像) から, AI に渡す画像を回す角度を決める (依頼 144. 落ちない). @@ -1235,10 +1292,14 @@ def main(): help=f"爪を開くとき daemon が待つ秒 (既定 {GRIP_OPEN_DAEMON_WAIT:.1f}. 前は 2.0)") ap.add_argument("--settle-wait", type=float, default=None, help=f"動きが止まるのを待つ目安の秒 (補正の 1 刻み・settle. 既定 {SETTLE_WAIT_SEC:.1f}. 前は 2.0)") + ap.add_argument("--grip-value", type=int, default=None, + help=f"爪の閉じる値 (既定 {GRIP_VALUE_DEFAULT}. 推論・補正の g / r / R・巻き戻し・ゲームパッドの閉じる指令に使う. " + "依頼 152)") args = ap.parse_args() if args.dry_keys and (args.execute or args.auto): sys.exit("[ERROR] --dry-keys はドライラン用 (--execute / --auto と一緒には使えない)") apply_wait_args(args) + apply_grip_args(args) if not DATA_ROOT.exists(): sys.exit(f"[ERROR] SSD 未マウント: {DATA_ROOT}") @@ -1260,7 +1321,7 @@ def main(): log_f.write(json.dumps({"type": "model", "model": args.model, "raw_dir": str(args.raw_dir), "framing_raw_dir": str(framing_raw_dir(args)), "tilt_raw_dir": str(tilt_raw_dir(args)), "tilt_fix": not args.no_tilt_fix, - "host": args.host, "port": args.port, "t": time.time()}, + "grip_value": GRIP_VALUE, "host": args.host, "port": args.port, "t": time.time()}, ensure_ascii=False) + "\n") log_f.flush() @@ -1487,8 +1548,9 @@ def main(): print(" 爪が開いているので締め直しはしない(閉じるのは g)") e.update(after=grip, sent=False, skipped="gripper_open") else: - print(" 爪を締め直します (開かずに閉じる指令をもう一度)") - arm.gripper(1) + print(f" 爪を締め直します (開かずに閉じる値 " + f"{tighten_value(LAST_CLOSE_VALUE, GRIP_VALUE)} を送る)") + e.update(regrip_tighten(arm)) e.update(after=1, sent=True) log_f.write(json.dumps(e) + "\n"); log_f.flush() continue @@ -1805,6 +1867,11 @@ def main(): for h_ in history], n_rewinds, n_corrections, exit_code) except Exception as e: print(f"[WARN ] {run_stem}.json を書けない: {e}") + try: + # 依頼 152: 閉じて 1 秒後の爪の電流 (currents.csv の中央値). 表示だけ (記録には何も足さない) + print(f"[GRIP ] {after_close_line(log_dir)} (閉じる値 {GRIP_VALUE})") + except Exception as e: + print(f"[GRIP ] 閉じて 1 秒後の爪の電流を出せない: {type(e).__name__}: {e}") if args.record and rec_sock is not None: notify_recorder(rec_sock, {"type": "trial_end", "run_stem": run_stem}) if not log_f.closed: diff --git a/pi05/test_pad_correction.py b/pi05/test_pad_correction.py index ec971d1..bce8b3d 100644 --- a/pi05/test_pad_correction.py +++ b/pi05/test_pad_correction.py @@ -171,6 +171,44 @@ class TestPadExecute(unittest.TestCase): self.assertEqual(grip, 1) self.assertEqual(res["sent_targets"], [[-1.0, 0.0, 0.0, 0.0, 0.0, -30.0]]) + def test_execute_close_uses_grip_value(self): + """依頼 152: ゲームパッドで閉じる指令も --grip-value の値 (ここでは 26). 次の r はこれと違う値 (23).""" + q = {"now": np.array(HOME, dtype=float)} + calls = [] + + def req(c, **k): + 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_CROSS], 60: [gt.BTN_SQUARE]}) + orig = gt.run_embedded + gt.load_daemon_modules() + with mock.patch.object(gt, "run_embedded", functools.partial(orig, on_frame=on_frame, link_factory=gt.InProcessPadLink)), \ + 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(v)), \ + mock.patch.object(rpc, "GRIP_VALUE", 26), mock.patch.object(rpc, "LAST_CLOSE_VALUE", None), \ + 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(rpc.LAST_CLOSE_VALUE, 26) + self.assertEqual(rpc.regrip_tighten(arm)["grip_value_sent"], 23) + self.assertEqual((grip, calls), (1, [26, 23])) + + def test_dry_close_remembers_grip_value(self): + """ドライランのゲームパッドで閉じても, 次の r が閉じる値と違う値になるよう覚える.""" + on_frame = script_frames({3: [gt.BTN_CROSS], 60: [gt.BTN_SQUARE]}) + orig = gt.run_embedded + arm = rpc.ArmIO(False, None, sim_q=HOME) + with mock.patch.object(gt, "run_embedded", functools.partial(orig, on_frame=on_frame, link_factory=gt.InProcessPadLink)), \ + mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch.object(rpc, "GRIP_VALUE", 26), mock.patch.object(rpc, "LAST_CLOSE_VALUE", None): + res, grip = rpc.run_pad_correction(arm, 0, 3, 7, io.StringIO(), None, LO, HI) + self.assertEqual((grip, rpc.LAST_CLOSE_VALUE), (1, 26)) + class TestConfirmP(unittest.TestCase): """確認で p を押すと補正に入り, 終えたあとの b でその補正を巻き戻せる (アームなし --dry-keys).""" diff --git a/pi05/test_rewind_correct.py b/pi05/test_rewind_correct.py index 1d82c52..fbe945f 100644 --- a/pi05/test_rewind_correct.py +++ b/pi05/test_rewind_correct.py @@ -603,7 +603,8 @@ class TestClientFlow(unittest.TestCase): l_new = [x for x in l_new if x.get("type") != "tilt_fix"] self.assertEqual(len(l_old), len(l_new)) new_keys = ("t_exec_start", "t_exec_end", "sent_targets", "framing_raw_dir", # framing_raw_dir は依頼 137 - "tilt_raw_dir", "tilt_fix", "measured_on", "rotate_deg") # 依頼 144 + "tilt_raw_dir", "tilt_fix", "measured_on", "rotate_deg", # 依頼 144 + "grip_value") # 依頼 152 # HEAD が依頼 25 より前なら new_keys は新しい方にだけある. 依頼 25 以降なら両方にある (両方から外す) drop = ("t", "latency_s", "reference") + new_keys so, sn = strip(l_old, drop=drop), strip(l_new, drop=drop) @@ -637,9 +638,9 @@ class EventArm(rpc.ArmIO): self.events.append(("send", tag, [round(float(v), 3) for v in target])) return super().send(target, tag) - def gripper(self, state): + def gripper(self, state, value=None): self.events.append(("grip", int(state))) - return super().gripper(state) + return super().gripper(state, value) class TestGraspRewind(unittest.TestCase): @@ -853,11 +854,11 @@ class TestRegripAndCorrectionReach(unittest.TestCase): self.assertEqual(correction_joint_reached(None, tgt, 0, 0.5), (False, None)) def test_r_and_R_records(self): - """g で閉じる → r → R: r は閉じる指令をもう一度だけ, R は 開く → 閉じる. 記録に前後の状態.""" + """g で閉じる → r → R: r は開かずに閉じる値 − 3 を 1 回 (依頼 152), R は 開く → 閉じる. 記録に前後の状態.""" code, world, run, lines, out = run_client(rpc, self.EXEC, ["c", "", "q", ""], self.td, keys="grR\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) - self.assertEqual(world.grips[:4], [v, v, 0, v]) # g / r / R (開く → 閉じる) + self.assertEqual(world.grips[:4], [v, ("v", rpc.GRIP_VALUE - 3), 0, v]) # g / r / R (開く → 閉じる) corr = [x for x in lines if x.get("type") == "correction"] self.assertEqual([x["action"] for x in corr], ["gripper", "regrip_tighten", "regrip_reopen", "accept"]) r, R = corr[1], corr[2] @@ -1256,13 +1257,13 @@ class TestConfirmKeys(unittest.TestCase): self.assertIn("r=爪の締め直し", out) def test_execute_r_tightens_and_outcome_reasks(self): - """--execute: 閉じているときの r は閉じる指令をもう一度送る (チャンクは送らない). + """--execute: 閉じているときの r は閉じる指令をもう一度送る (値は閉じる値 − 3. 依頼 152. チャンクは送らない). 結果を聞く所も 知らない入力・空白だけ は聞き直す.""" code, world, run, lines, out = run_client( rpc, self.EXEC, ["c", "r", "R", "", "q", "yy", " ", "y"], self.td, keys="g\n") self.assertEqual(code, 0) v = ("v", rpc.GRIP_VALUE) - self.assertEqual(world.grips[:2], [v, v]) # g で閉じる → r でもう一度閉じる (3 つ目は推論の開) + self.assertEqual(world.grips[:2], [v, ("v", rpc.GRIP_VALUE - 3)]) # g で閉じる → r で閉じ直す (3 つ目は推論の開) conf = [x for x in lines if x.get("type") == "confirm"] self.assertEqual([(x["chunk"], x["before"], x["after"], x["sent"]) for x in conf], [(1, 1, 1, True)]) # r では実行せず, あとの空の Enter で推論 1 を実行した (推論 1 の行は r の記録より後に 1 つだけ) @@ -1318,5 +1319,157 @@ class TestConfirmKeys(unittest.TestCase): tf.assert_not_called() + +# ---- 依頼 152: 爪の閉じる値 (--grip-value) と 締め直し r の値を交互に ------------------------------ +class TestGripValue(unittest.TestCase): + EXEC = ["--execute", "--steps-per-chunk", "10", "--via-stride", "3", "--settle-retries", "0", + "--ignore-framing", "--max-chunks", "8"] + + def setUp(self): + self.td = Path(tempfile.mkdtemp()) + time.sleep(1.1) + + def test_tighten_value_alternates(self): + from rewind_correct import tighten_value + self.assertEqual(tighten_value(None, 20), 17) # まだ送っていない → 閉じる値 − 3 + self.assertEqual(tighten_value(20, 20), 17) # 前が閉じる値 → − 3 + self.assertEqual(tighten_value(17, 20), 20) # 前が − 3 → 閉じる値 + self.assertEqual(tighten_value(25, 25), 22) + self.assertEqual(tighten_value(22, 25), 25) + self.assertEqual(tighten_value(20, 25), 22) # 前が別の値 (閉じる値を変えた直後など) → − 3 + self.assertEqual(tighten_value(None, 2), 0) # 0 より下にはしない + self.assertEqual(tighten_value(0, 2), 2) + + def test_default_is_20(self): + """--grip-value を付けないと今までどおり 20. log.jsonl の頭と {run}.json の args に残る.""" + code, world, run, lines, out = run_client( + rpc, ["--dry-keys", "--steps-per-chunk", "10", "--max-chunks", "1"], ["q"], self.td) + self.assertEqual(code, 0) + self.assertEqual(rpc.GRIP_VALUE_DEFAULT, 20) + head = [x for x in lines if x.get("type") == "model"][0] + self.assertEqual(head["grip_value"], 20) + self.assertIn("[GRIP ] 閉じる値 20 (既定)", out) + + def test_grip_value_all_closes_and_r_alternates_in_correction(self): + """--grip-value 25: 補正の g・R・推論の閉じる指令は 25. r は 22 → 25 → 22 と交互 (R のあとは 22 から).""" + code, world, run, lines, out = run_client( + rpc, self.EXEC + ["--grip-value", "25"], ["c", "", "q", ""], self.td, keys="grrrRr\n") + self.assertEqual(code, 0) + # 最後の 0 は補正のあとの推論 (FakePolicy は爪を開く) の指令 + self.assertEqual(world.grips, [("v", 25), ("v", 22), ("v", 25), ("v", 22), 0, ("v", 25), ("v", 22), 0]) + tight = [x for x in lines if x.get("action") == "regrip_tighten"] + self.assertEqual([x["grip_value_sent"] for x in tight], [22, 25, 22, 22]) + self.assertEqual([x["prev_close_value"] for x in tight], [25, 22, 25, 25]) + self.assertTrue(all(x["grip_value"] == 25 and x["where"] == "correction" for x in tight)) + head = [x for x in lines if x.get("type") == "model"][0] + self.assertEqual(head["grip_value"], 25) + j = json.loads((run / f"{run.name}.json").read_text()) + self.assertEqual(j["args"]["grip_value"], 25) + self.assertIn("[GRIP ] 閉じる値 25 締め直し r は 22 と 25 を交互に送る", out) + self.assertNotIn(("v", 20), world.grips) + + def test_grip_value_in_inference_close_and_confirm_r(self): + """推論で閉じる (GraspPolicy) 指令も 25. 補正の外の r も 22 → 25 と交互で, confirm の行に送った値.""" + code, world, run, lines, out = run_client( + rpc, self.EXEC + ["--grip-value", "25"], ["", "", "r", "r", "q", ""], self.td, policy_cls=GraspPolicy) + self.assertEqual(code, 0) + self.assertEqual(world.grips, [0, ("v", 25), ("v", 22), ("v", 25)]) # 開始時の開放・つかむ・r・r + conf = [x for x in lines if x.get("type") == "confirm"] + self.assertEqual([(x["action"], x["grip_value_sent"], x["sent"]) for x in conf], + [("regrip_tighten", 22, True), ("regrip_tighten", 25, True)]) + + def test_rewind_reclose_and_cancel_use_grip_value(self): + """補正を取り消して始めの「閉」へ戻すときの閉じる指令も 25 (arm.gripper → gripper_cmd).""" + code, world, run, lines, out = run_client( + rpc, self.EXEC + ["--grip-value", "25"], ["", "", "c", "q", ""], self.td, policy_cls=GraspPolicy, + keys="g1+b") + self.assertEqual(code, 0) + self.assertEqual(world.grips, [0, ("v", 25), 0, ("v", 25)]) # 開放・つかむ・g (開)・取り消しで閉に戻す + + def test_grip_value_zero_says_r_cannot_change(self): + code, world, run, lines, out = run_client( + rpc, ["--dry-keys", "--max-chunks", "1", "--grip-value", "0"], ["q"], self.td) + self.assertEqual(code, 0) + self.assertIn("締め直し r も 0 のまま (0 より下にしないので値を変えられない)", out) + + def test_grip_value_out_of_range(self): + for bad in ("-1", "101"): + with self.assertRaises(SystemExit) as cm: + run_client(rpc, ["--dry-keys", "--max-chunks", "1", "--grip-value", bad], ["q"], self.td) + self.assertIn("--grip-value", str(cm.exception.code)) + + def test_send_grip_close_and_gripper_cmd(self): + sent = [] + with mock.patch.object(rpc, "daemon_set_gripper_value", lambda v, *a, **k: sent.append(v)), \ + mock.patch.object(rpc, "GRIP_VALUE", 27), mock.patch.object(rpc, "GRIPPER_SLEEP", 0.0), \ + mock.patch.object(rpc, "LAST_CLOSE_VALUE", None): + rpc.gripper_cmd(1) + self.assertEqual(rpc.LAST_CLOSE_VALUE, 27) + rpc.gripper_cmd(1, 24) + self.assertEqual(rpc.LAST_CLOSE_VALUE, 24) + rpc.ArmIO(False).gripper(1) # ドライランも覚える (送らない) + self.assertEqual(rpc.LAST_CLOSE_VALUE, 27) + self.assertEqual(sent, [27, 24]) + + def test_after_close_line(self): + """推論の終わりの 1 行: 閉じた命令ごとに 閉じて 1〜2 秒の gripper_current の中央値.""" + import grip_current as gc + rec = self.td / "run" / "recording" + rec.mkdir(parents=True) + b = 1.8e9 + with (rec / "states.csv").open("w") as f: + f.write(",".join(STATES_COLS) + "\n") + for t, g in ((0.0, 0), (1.0, 1), (1.5, 1), (5.0, 0), (6.0, 1)): # 1.5 は閉じたままの r (数えない) + row = {c: "" for c in STATES_COLS} + row.update(ts=f"{b + t:.3f}", event_type="gripper_cmd", gripper_state=str(g)) + f.write(",".join(row[c] for c in STATES_COLS) + "\n") + with (rec / "currents.csv").open("w") as f: + f.write("ts,gripper_current\n") + for t, v in ((1.5, 900), (2.1, 800), (2.5, 820), (2.9, 810), (3.2, 0), (6.5, 5), (7.2, 760), (7.9, ""), + (8.1, 0)): + f.write(f"{b + t:.3f},{v}\n") + a = gc.after_close(self.td / "run") + self.assertEqual(a["status"], "ok") + self.assertEqual([(c["median"], c["n"]) for c in a["closes"]], [(810, 3), (760, 1)]) + line = gc.after_close_line(self.td / "run") + self.assertEqual(line, "閉じて 1 秒後の爪の電流 (中央値): 1 回目 810 (3 行) / 2 回目 760 (1 行)") + self.assertIn("値なし", gc.after_close_line(self.td / "none")) + # 偶数行は真ん中の 2 つの平均 (codex R1): 2 回目の窓に 0 を足すと (0 + 760) / 2 + with (rec / "currents.csv").open("a") as f: + f.write(f"{b + 7.5:.3f},0\n") + self.assertEqual([(c["median"], c["n"]) for c in gc.after_close(self.td / "run")["closes"]], [(810, 3), (380, 2)]) + self.assertIn("2 回目 380 (2 行)", gc.after_close_line(self.td / "run")) + + def test_after_close_line_printed_at_end(self): + code, world, run, lines, out = run_client(rpc, self.EXEC + ["--grip-value", "25"], ["q", ""], self.td) + self.assertEqual(code, 0) + self.assertIn("[GRIP ] 閉じて 1 秒後の爪の電流 (中央値): ", out) + self.assertIn("(閉じる値 25)", out) + + +class TestExcludesDeepEven10V2(unittest.TestCase): + """依頼 152: deep_even10_v2 で外す位置5 の補正入り 6 本.""" + SIX = ["pi05_20261031_175527", "pi05_20261031_180435", "pi05_20261031_184016", + "pi05_20261101_190347", "pi05_20261106_183926", "pi05_20261106_184650"] + + def test_six_with_reason(self): + d = json.loads((_HERE / "excludes" / "deep_even10_v2.json").read_text()) + self.assertEqual(sorted(d), self.SIX) + for k, v in d.items(): + self.assertIn("位置5 の持ち上げをそろえる", v["reason"]) + self.assertIn("エイイチの決め 2026-11-11", v["reason"]) + self.assertEqual((v["position"], v["kind"]), (5, "補正入り")) + self.assertLessEqual(v["position_check"]["dist_mm"], 30.0) + + def test_names_are_in_deep_even10_v1(self): + raw = Path("/Volumes/KIOXIA/r2r2r-data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes") + if not raw.is_dir(): + self.skipTest("SSD 未マウント") + names = {p.name for p in raw.iterdir() if p.is_dir()} + self.assertEqual(len(names), 50) + self.assertTrue(set(self.SIX) <= names) + self.assertEqual(sorted(n for n in names if n.startswith("pi05_")), self.SIX) # 補正入りはこの 6 本だけ + + if __name__ == "__main__": unittest.main() diff --git a/pi05/test_teach_currents.py b/pi05/test_teach_currents.py index b8c08ee..f639383 100644 --- a/pi05/test_teach_currents.py +++ b/pi05/test_teach_currents.py @@ -21,6 +21,7 @@ policy_recorder.py の頭)・open の set_gripper は事前 2 秒・角度 18 ms - build_demo_episodes で作るデータは currents.csv・currents_meta.json・試行 JSON の currents があっても同じ KIOXIA にもアームにも触らない (一時フォルダと 127.0.0.1 だけ). """ +import argparse import csv import io import json @@ -664,5 +665,47 @@ class TestBuildUnchanged(unittest.TestCase): tgd._same_episode(td / "a" / run.name, td / "b" / run.name) +class TestGripValue(unittest.TestCase): + """依頼 152: record_teach_demo.py run --grip-value. 付けなければ 20. 再生の閉じる指令がその値になる.""" + + def test_default_20(self): + self.assertEqual(rtd.wpsr.GRIP_VALUE, 20) + + def test_replay_sends_grip_value(self): + td = Path(tempfile.mkdtemp()) + pr.install_daemon_lock() + d = ArmDaemon() + try: + with mock.patch.object(rtd, "GRIP_VALUE", 24): + result = replay(d, td / "run_0001_20991111_000000", currents=False) + finally: + d.close() + self.assertEqual(result[0], "completed") + vals = [c.get("value") for cmd, _t0, _t1, c in d.spans if cmd == "set_gripper_value"] + self.assertEqual(vals, [24]) + + def test_cmd_run_sets_and_checks(self): + """範囲の外は止める. 範囲の中なら GRIP_VALUE を置き換える (ここでは --teach-local と --execute の組み合わせで + そのあとすぐ止まる所まで). 付けなければ 20 のまま.""" + base = dict(teach="x.json", execute=True, teach_local="x.json") + with mock.patch.object(rtd, "GRIP_VALUE", 20), mock.patch("sys.stdout", new=io.StringIO()): + with self.assertRaises(SystemExit) as cm: + rtd.cmd_run(argparse.Namespace(grip_value=101, **base)) + self.assertEqual((cm.exception.code, rtd.GRIP_VALUE), (2, 20)) + with self.assertRaises(SystemExit): + rtd.cmd_run(argparse.Namespace(grip_value=None, **base)) + self.assertEqual(rtd.GRIP_VALUE, 20) + with self.assertRaises(SystemExit): + rtd.cmd_run(argparse.Namespace(grip_value=26, **base)) + self.assertEqual(rtd.GRIP_VALUE, 26) + + def test_cli_option(self): + got = {} + with mock.patch.object(rtd, "cmd_run", lambda a: got.update(v=a.grip_value) or 0), \ + mock.patch.object(sys, "argv", ["record_teach_demo.py", "run", "--teach", "x.json", "--grip-value", "25"]): + rtd.main() + self.assertEqual(got["v"], 25) + + if __name__ == "__main__": unittest.main() diff --git a/test_lab_start.sh b/test_lab_start.sh index 24f924b..e15f0bd 100755 --- a/test_lab_start.sh +++ b/test_lab_start.sh @@ -431,6 +431,20 @@ rm -rf "$T" setup; run --model deep1 --no-tilt-fix check "--no-tilt-fix → クライアントに --no-tilt-fix を渡す (回さない. 依頼 144)" '[ $RC = 0 ] && has py.log "--tilt-raw-dir $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes --no-tilt-fix" && has out.txt "--no-tilt-fix なので回さない"' rm -rf "$T" +setup; run --model deep1 --grip-value 25 +check "--grip-value 25 → クライアントに --grip-value 25 を渡し, [GRIP ] に 25 と出す (依頼 152)" '[ $RC = 0 ] && has py.log "--tilt-raw-dir $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes --grip-value 25" && has out.txt "[GRIP ] 爪の閉じる値は 25 (--grip-value)"' +rm -rf "$T" +setup; run --model deep1 +check "--grip-value なし → クライアントに --grip-value を渡さない (既定 20 のまま). [GRIP ] に既定の 20 と出す (依頼 152)" '[ $RC = 0 ] && ! has py.log "--grip-value" && has out.txt "[GRIP ] 爪の閉じる値は 20 (既定"' +rm -rf "$T" +for bad in 101 abc -1 "" 18446744073709551616 0101; do + setup; run --model deep1 --grip-value "$bad" + check "--grip-value '$bad' → API を呼ばずに終了コード 2 (依頼 152)" '[ $RC = 2 ] && has out.txt "--grip-value" && [ ! -s "$T/calls.log" ]' + rm -rf "$T" +done +setup; run --model deep1 --grip-value +check "--grip-value の値がない → 終了コード 2 (依頼 152)" '[ $RC = 2 ] && has out.txt "--grip-value には 0〜100 の整数を付ける"' +rm -rf "$T" setup; rmdir "$T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes"; run --model deep1 check "deep1 で deep_even10_v1 の raw_episodes がない → API を呼ばずに止まる (依頼 135)" '[ $RC = 1 ] && has out.txt "安全範囲を作る学習データがありません: $T/data/datasets/mecharm_redblock_box_deep_even10_v1/raw_episodes" && [ ! -s "$T/calls.log" ]' rm -rf "$T" @@ -585,7 +599,7 @@ check "デーモン・HOME・爪・位置合わせ・推論の段に進まない check "Pod は消さない (DELETE を呼ばない) で, かかった時間と片付け方を出す" '[ $(n_call DELETE) = 0 ] && has out.txt "=== かかった時間" && has out.txt "Pod は残しています (id $ID_NEW). 片付けは ./lab_stop.sh --log LogT"' check "記録 lab_start_*.txt に残る・始めの行に --serve-only と出る" 'grep -qF "サーバーとトンネルだけ (--serve-only" "$T/data/lab/LogT"/lab_start_*.txt' rm -rf "$T" -check "--help に --serve-only の説明が入る (2〜52 行)" 'sed -n "2,52p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,52p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを"' +check "--help に --serve-only・--grip-value の説明が入る (2〜54 行)" 'sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--serve-only Pod を作って" && sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--no-tilt-fix 傾きを" && sed -n "2,54p" "$HERE/lab_start.sh" | grep -qF -- "--grip-value N 爪の閉じる値"' echo echo "OK $OK / NG $NG" diff --git a/tools/gamepad_teleop.py b/tools/gamepad_teleop.py index ebbe738..af0151c 100644 --- a/tools/gamepad_teleop.py +++ b/tools/gamepad_teleop.py @@ -639,7 +639,7 @@ class DaemonArm: return np.array([float(v) for v in a]) if a is not None and len(a) == 6 else None def gripper(self, state, gate): - """run_policy_client.gripper_cmd と同じ中身 (閉 = daemon_set_gripper_value(GRIP_VALUE) / + """run_policy_client.gripper_cmd と同じ中身 (閉 = daemon_set_gripper_value(GRIP_VALUE. --grip-value. 依頼 152) / 開 = daemon_set_gripper(0, wait=GRIP_OPEN_DAEMON_WAIT). そのあと GRIPPER_SLEEP 待つ). 指令だけを gate の中で送り, 待ちは gate の外で行う (停止の受け付けを待ちの分だけ遅らせない). 戻り値: 送ったか.""" @@ -1957,9 +1957,13 @@ def main(argv=None, on_frame=None, start_pose_dry=None): ap.add_argument("--execute", action="store_true", help="daemon に送る (付けなければドライラン)") ap.add_argument("--pad-io", action="store_true", help=argparse.SUPPRESS) # run_policy_client の p から起動される側 ap.add_argument("--title", default="PAD CORRECTION", help=argparse.SUPPRESS) + ap.add_argument("--grip-value", type=int, default=None, + help="爪の閉じる値 (既定は run_policy_client.GRIP_VALUE = 20. 依頼 152)") args = ap.parse_args(argv) if args.pad_io: return pad_io_main(args.title) + if args.grip_value is not None and not (0 <= args.grip_value <= 100): + sys.exit(f"[ABORT] --grip-value は 0〜100 の整数で指定する (指定: {args.grip_value})") stamp = datetime.now().strftime("%Y%m%d_%H%M%S") run_stem = f"teleop_{stamp}" + ("" if args.execute else "_dry") @@ -1969,6 +1973,9 @@ def main(argv=None, on_frame=None, start_pose_dry=None): load_daemon_modules() except Exception as ex: # 読み込みの失敗・値の違い. 記録も daemon もまだ触っていない sys.exit(f"[ABORT] daemon の送り口を読めません: {type(ex).__name__}: {ex}。何も送らずに終わります") + if args.grip_value is not None: + rpc.GRIP_VALUE = int(args.grip_value) # DaemonArm.gripper が読む値 (依頼 152) + print(f"[爪] 閉じる値 {rpc.GRIP_VALUE}") run_dir = LOG_ROOT / run_stem if run_dir.exists(): sys.exit(f"[ABORT] 同じ名前のフォルダがあります: {run_dir}") diff --git a/tools/test_gamepad_teleop.py b/tools/test_gamepad_teleop.py index e3ac3d4..72e6f06 100644 --- a/tools/test_gamepad_teleop.py +++ b/tools/test_gamepad_teleop.py @@ -1510,6 +1510,55 @@ class TestExecuteMainRecords(unittest.TestCase): self.assertIn(key, meta) +class TestGripValue(unittest.TestCase): + """依頼 152: 単独の --execute で --grip-value を付けると, 閉じる指令 (×) と meta の grip_close_value がその値.""" + + def run_main(self, argv): + import pygame + gt.load_daemon_modules() + script = { + 1: lambda: pygame.event.post(pygame.event.Event(pygame.JOYDEVICEADDED, device_index=0)), + 3: lambda: (pygame.event.post(pygame.event.Event(pygame.JOYBUTTONDOWN, instance_id=7, + button=gt.BTN_CROSS, joy=0)), + pygame.event.post(pygame.event.Event(pygame.JOYBUTTONUP, instance_id=7, + button=gt.BTN_CROSS, joy=0))), + 60: lambda: pygame.event.post(pygame.event.Event(pygame.QUIT)), + } + grips = [] + with tempfile.TemporaryDirectory() as td, \ + mock.patch.object(gt, "LOG_ROOT", Path(td)), \ + mock.patch.object(gt, "ensure_ssd_mounted", lambda: None), \ + mock.patch.object(gt.pygame.joystick, "Joystick", FakeJoystick), \ + mock.patch.object(gt, "get_daemon_socket", lambda: None), \ + mock.patch.object(gt, "close_daemon_socket", lambda: None), \ + mock.patch.object(gt.rpc, "install_daemon_lock", lambda: None), \ + mock.patch.object(gt.rpc, "read_angles", lambda: np.array(HOME, dtype=float)), \ + mock.patch.object(gt.rpc, "GRIP_VALUE", 20), mock.patch.object(gt.rpc, "GRIPPER_SLEEP", 0.0), \ + mock.patch.object(gt, "GRIPPER_SLEEP", 0.0), \ + mock.patch.object(gt, "daemon_set_gripper_value", lambda v, *a, **k: grips.append(v)), \ + mock.patch.object(gt, "daemon_request", lambda c, **k: {"ok": True}), \ + mock.patch("builtins.print", lambda *a, **k: None): + gt.main(argv, on_frame=lambda n, tp: script[n]() if n in script else None) + run = list(Path(td).iterdir())[0] + meta = json.loads((run / "meta.json").read_text()) + return grips, meta + + def test_grip_value_option(self): + grips, meta = self.run_main(["--execute", "--grip-value", "24"]) + self.assertEqual(grips, [24]) + self.assertEqual(meta["motion"]["grip_close_value"], 24) + + def test_default_unchanged(self): + grips, meta = self.run_main(["--execute"]) + self.assertEqual(grips, [20]) + self.assertEqual(meta["motion"]["grip_close_value"], 20) + + def test_out_of_range(self): + with self.assertRaises(SystemExit) as cm: + gt.main(["--execute", "--grip-value", "101"]) + self.assertIn("--grip-value", str(cm.exception)) + + class TestNoCv2InDryRun(unittest.TestCase): """依頼 69: ドライランは cv2 (cv2/.dylibs/libSDL2) を読まず, objc の重複の警告が出ない. このプロセスはもう load_daemon_modules() を呼んでいるので, 別のプロセスで main([]) を回す."""