mechArm 270 Pi 動的追従の限界を探る Log#39 | 完走したが、掴めていなかった。空欄だった掴み判定をCSVに残す
Pick&Place は完走した。HOMEに戻り、ブロックの検出から姿勢選択、PICK、LIFT、PLACE、release、VLM判定まで通った。ログ上は最後まで走っている。
だが、掴めていなかった。ブロックは爪に入らず、元の場所に残った。空のグリッパーだけが箱の横へ移動し、そこで開いた。
問題は、その失敗がこれまで台帳に残らなかったことだ。pick_then_place は、完走しても中断しても、本線の台帳 pick_place_ledger_v2.csv に一行を残す約束だった。ところが実際には、JSONと補助インデックスCSVにしか書いておらず、正であるはずの台帳には一行も入っていなかった。
この回では、まず台帳追記を戻す。そのうえで、空欄だった掴み判定、grip前の爪先とブロックの距離、grip後のVLM判定を入れる。完走を一本通すまでに、実機側の壁をいくつも踏む。関節が指令を無視して一切動かない現象、姿勢の到達がわずかに足りずに止まる挙動、ブロックの位置が登録姿勢の谷間に落ちて自動選択が止まる判定。順に潰した先で、完走したが掴めていなかった試行が、33列のCSVとして残る。
台帳に書いていなかった
pick_then_place の出力先は2つある。一試行ごとのJSON、横断インデックスのCSV、そして本線の台帳 pick_place_ledger_v2.csv。台帳は33列の固定スキーマで、過去の試行が一行ずつ積まれている。ルールは「PICK→PLACE を通しで走らせるスクリプトは、完走でも中断でも必ず台帳に一行追記する」。インデックスCSVは補助で、台帳が正。
ところがこのスクリプトは、JSONとインデックスCSVには書いていたが、台帳には一度も書いていなかった。完走した試行が2本、台帳から漏れていた。原因は単純で、台帳追記の処理そのものをスクリプトが持っていなかった。インデックスCSVに書く append_index_row はあるが、台帳に書く関数が存在しない。
最初にやるのは、その台帳追記関数を実装することだった。33列を組み立てて追記する。設計上の制約がいくつかある。台帳は既存ファイルがある前提で、ヘッダは絶対に書き直さない。既存の33列ヘッダを正とし、追記専用で開く。台帳が存在しなければ、新規作成はせず警告だけ出して何も書かない。これはスクリプトの責務ではないからだ。
まず台帳のパスと列定義。
# ---- 本線台帳（追記専用・writeheader 禁止・33列固定）----------------------
LEDGER_CSV = Path("/Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v2.csv")
# 台帳の現行33列（実ファイルのヘッダと厳密一致・順序厳守・絶対に増減しない）
LEDGER_COLS = [
"timestamp", "run_stem", "trial_no",
"pick_verdict", "pick_reason", "pick_confidence",
"place_verdict", "place_reason", "place_confidence",
"block_pick_x", "block_pick_z", "block_place_x", "block_place_z",
"box_x", "box_z", "in_box_geom", "in_box_dist_mm",
"final_j1", "final_j2", "final_j3", "final_j4", "final_j5", "final_j6",
"gripper", "place_capture_path",
"tip_pick_x", "tip_pick_z", "tip_block_dx_mm", "tip_block_dz_mm", "tip_block_dist_mm",
"run_dir", "tip_capture_path", "pick_capture_path",
]次に、台帳の既存行数から次の連番を決める小さな関数。ヘッダ一行を除いた行数の次を返す。
def _trial_no_next():
"""台帳の既存行数から次の trial_no を決める（ヘッダ1行を除く）。
台帳が無い・読めない場合は空文字を返す（台帳新規作成はしない方針のため、
通常運用では台帳は必ず存在する前提）。"""
try:
if not LEDGER_CSV.exists():
return ""
with open(LEDGER_CSV) as f:
n = sum(1 for _ in f)
return str(max(0, n - 1) + 1) # ヘッダ1行を除いた次番号
except Exception:
return ""そして追記関数の本体。完走と中断の判別は、新しい列を足さずに既存台帳の流儀に合わせた。既存の15行を見ると、完走は place_verdict に yes か no が入り、PLACEに到達せず中断した試行は空文字になっている。この空・非空がそのまま判別子になっていた。だから新しい追記も、place_verdict に判定結果をそのまま入れる。完走なら yes/no、PLACE未到達の中断なら空、PLACEまで行ったが評価できなかった場合は unknown。aborted: のような独自のプレフィクスは使わない。pick側も同じ思想で、yes/no/unknown/空の三値＋空に揃えた。
二重追記を防ぐガードと、書き込みの隔離を入れた。完走後にCtrl-Cが入ると例外側でもう一度呼ばれる余地がある。一試行につき一回だけ追記するフラグをローカルのdictに持たせ、二重追記を止める。recordに持たせると試行JSONに焼き込まれて記録が汚れるからだ。また、SSDの容量不足や権限異常で書き込みが例外になったとき、本来のreturn値や安全停止を壊したくない。open/writerowを丸ごとtry/exceptで包んで隔離した。
def append_ledger_row(record, run_stem, run_dir, ledger_appended):
"""本線台帳 pick_place_ledger_v2.csv に1行追記する（33列・追記専用）。
- 完走/中断とも呼ぶ。判別は既存流儀（place_verdict の空/非空）に準拠。
完走=yes/no、PLACE未到達の中断=空文字、PLACE到達したが評価不能=unknown。
- 台帳が存在しない場合は writeheader せず、追記もしない（警告のみ）。
台帳新規作成はこのスクリプトの責務ではない（既存33列を正とする）。
- ledger_appended: cmd_run のローカル dict（{"done": bool}）。
record に持たせると _save() で run JSON に焼き込まれて記録を汚すため外出し。
"""
# C2: 1 run につき台帳追記は1回だけ。既に追記済みなら何もしない
# （完走 fall-through 後に Ctrl-C が入り except 側でも呼ばれる二重追記を防ぐ）
if ledger_appended.get("done"):
return
# 台帳が無いなら作らない・書かない（writeheader 禁止方針）
if not LEDGER_CSV.exists():
print("[LEDGER] [WARN] 台帳が見つからない: {} → 追記をスキップ"
.format(LEDGER_CSV))
return
detect = record.get("detect") or {}
block = detect.get("block_xz_m") or (None, None)
box = detect.get("box_xz_m") or (None, None)
place_judge = ((record.get("judgments") or {}).get("place")
or record.get("place_judgment") or {})
home_post = record.get("home_post") or {}
aborted_at = record.get("aborted_at")
# DAY241移植: block_place（PLACE側ブロック検出）と in_box 幾何計算。
# block_place_xz は record 優先、保険で OBSERVE_STATE から引く。
# box_xz は list 化（_ledger_in_box_geometry は list/tuple 要求・int/float 要素）。
block_place_xz = record.get("block_place_xz") or rpp._OBSERVE_STATE.get("block_place_xz")
box_xz_list = list(box) if all(isinstance(v, (int, float)) for v in box) else None
in_box_geom, in_box_dist_mm = rpp._ledger_in_box_geometry(block_place_xz, box_xz_list)
# DAY243移植: tip vs block 差分（STEP4.5 で測定済み・record["tip_pick"]）。
# tip_x/z は Unity m なので mm 化（*1000）、dx/dz/dist_mm は既に mm。
tip_pick = record.get("tip_pick") or {}
def mm(v):
return "{:.1f}".format(v * 1000) if isinstance(v, (int, float)) else ""
# place_verdict: 既存流儀に準拠して judgments.place.verdict をそのまま引く。
# 完走=yes/no、PLACE未到達の中断=""（judgments.place 無し）、
# swept失敗等でPLACE実行済だが評価不能=unknown。aborted: プレフィクスは使わない。
place_verdict = _v(place_judge.get("verdict"))
# final_j*: HOME復帰後の実測（既存台帳と同運用）。
# 完走は home_post.post_actual。中断は取得できた最後の実測を使う。
final_actual = home_post.get("post_actual")
if not (isinstance(final_actual, (list, tuple)) and len(final_actual) == 6):
# 中断などで home_post 実測が無い場合の最良努力フォールバック
for key in ("lift", "target"):
seg = record.get(key) or {}
pa = seg.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
final_actual = pa
break
else:
final_actual = [None] * 6
# gripper: 完走は release 済 → 0。中断は record["gripped"] の最終状態に従う
if aborted_at:
gripper_val = "1" if record.get("gripped") else "0"
else:
gripper_val = "0" # 完走は STEP10 で release 済
place_cap = _v(place_judge.get("image"))
# PICK判定（STEP5.5・grip直後・LIFT前のVLM判定）。
# 判定済み=yes/no、判定例外/不能=空、grip前に中断=record["pick_judgment"] is None で空。
pick_judge = record.get("pick_judgment") or {}
row = {
"timestamp": record.get("timestamp", ""),
"run_stem": run_stem,
"trial_no": _trial_no_next(),
"pick_verdict": _v(pick_judge.get("verdict")),
"pick_reason": _v(pick_judge.get("reason")),
"pick_confidence": _v(pick_judge.get("confidence"), "{:.2f}"),
"place_verdict": place_verdict,
"place_reason": _v(place_judge.get("reason")),
"place_confidence": _v(place_judge.get("confidence"), "{:.2f}"),
"block_pick_x": mm(block[0] if block else None),
"block_pick_z": mm(block[1] if block else None),
"block_place_x": rpp._ledger_mm_or_blank(block_place_xz, 0),
"block_place_z": rpp._ledger_mm_or_blank(block_place_xz, 1),
"box_x": mm(box[0] if box else None),
"box_z": mm(box[1] if box else None),
"in_box_geom": in_box_geom,
"in_box_dist_mm": in_box_dist_mm,
"gripper": gripper_val,
"place_capture_path": place_cap,
"tip_pick_x": ("{:.1f}".format(tip_pick["tip_x"] * 1000.0)
if tip_pick.get("tip_x") is not None else ""),
"tip_pick_z": ("{:.1f}".format(tip_pick["tip_z"] * 1000.0)
if tip_pick.get("tip_z") is not None else ""),
"tip_block_dx_mm": _v(tip_pick.get("dx_mm"), "{:.1f}"),
"tip_block_dz_mm": _v(tip_pick.get("dz_mm"), "{:.1f}"),
"tip_block_dist_mm": _v(tip_pick.get("dist_mm"), "{:.1f}"),
"run_dir": str(run_dir),
# tip_capture_path = grip前の幾何測定画像（stage4_pick_tip.jpg）。
# pick_capture_path = grip後のVLM PICK判定画像（pick.jpg）。役割分担で両方記録。
"tip_capture_path": _v(record.get("tip_capture_path")),
"pick_capture_path": _v(record.get("pick_capture_path_vlm")),
}
for i in range(6):
v = final_actual[i]
row["final_j{}".format(i + 1)] = (
"{:.2f}".format(v) if isinstance(v, (int, float)) else "")
# 33列厳密一致を実行時に検算（列の取りこぼし/余りを早期検出）
missing = [c for c in LEDGER_COLS if c not in row]
extra = [k for k in row if k not in LEDGER_COLS]
if missing or extra:
print("[LEDGER] [ERROR] 列不一致 missing={} extra={} → 追記中止"
.format(missing, extra))
return
try:
with open(LEDGER_CSV, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=LEDGER_COLS)
# writeheader は絶対に呼ばない（既存ヘッダを正とする・追記専用）
w.writerow(row)
ledger_appended["done"] = True # C2: 成功時のみフラグ（失敗時は再試行余地を残す）
print("[LEDGER] append OK: {} (place_verdict={!r})".format(
LEDGER_CSV, place_verdict))
except Exception as e:
# C4: 記帳失敗（SSD容量不足・権限異常等）が本来の return / 安全停止を壊さないよう隔離。
# フラグは立てない（書けていないため）。動作フローの return 値はそのまま維持される。
print("[LEDGER] [ERROR] 台帳追記に失敗（動作フローは継続）: {}".format(e))この関数を、完走の経路と全ての中断経路の直後で呼ぶ。インデックスCSVの追記がある場所すべてに、台帳追記を並べた。これで完走でも中断でも、必ず台帳に一行残る。中断記録は実機で先に確認できた。自動選択の手前で止まった試行が、place_verdict が空のまま33列で台帳に着地した。
関節が指令を無視する
完走を一本取ろうとして、すぐに止まった。掴みに行く姿勢に届かない。
検出したブロックの位置から、登録済みの教示姿勢のどれを使うかが自動で決まる。今回選ばれたのは seq2 という、アームを深く前に倒した姿勢だ。そこへ送っても、アームが動かない。
[STEP4] 現在 → target 大|Δ|=89.12°
[SEND] 未到達 TIMEOUT max|Δ|=89.12°
実測 [+0.26, +1.58, +1.93, -0.26, -0.26, -29.70]
開始からの移動量 max|Δ|=0.00°移動量 0.00°。指令を送ったのに、一ミリも動いていない。トルクが負けて途中で止まるのとは違う。動き出してすらいない。指令そのものが、どこかで弾かれている。
教示姿勢への移動だけを確認する最小スクリプトを書いて、切り分けた。HOMEへ送る指令は通る。seq2 へ送る指令は無視される。違いは目標角度だけ。seq2 の送信配列の2番目、台帳表記で j1 と呼ぶ関節が90.70度で、ここが怪しかった。j1を88度に下げた以外は同じ姿勢を送ると、今度は動く。
HOME後: [0.26, 1.58, 1.93, -0.26, -0.26, -29.7]
送信: [-45.35, 88.0, -6.94, 0.87, -86.48, -29.53]
実測: [-45.26, 88.85, -5.8, 1.66, -84.55, -29.53]
HOMEからの移動量: 88.85j1=88度なら動く。90.70度は動かない。上限がこの間にある。さらに刻んで測った。
j1= 88.00 -> 動く 移動量=88.85 j1実測=88.85
j1= 89.00 -> 動く 移動量=89.73 j1実測=89.73
j1= 89.50 -> 動く 移動量=90.17 j1実測=90.17
j1= 90.00 -> 動く 移動量=90.61 j1実測=90.61
j1= 90.50 -> 無視 移動量=1.49 j1実測=1.49
j1= 90.70 -> 無視j1=90.0までは到達する。90.5を超えると、指令を含む send_angles がコマンドごと無視される。daemonかサーボのソフトウェア角度上限が90.0度のあたりにあって、それを超える角度が一つでも入ると、その指令全体が実行されない。一部の関節だけ動くのではなく、丸ごと弾かれる。seq2 の j1=90.70度は、この上限を0.7度超えていた。だから一切動かなかった。
教示姿勢は、ドラッグティーチングで手でアームを曲げて記録したものだ。手で曲げれば90.70度まで曲がる。だが send_angles で同じ角度を送ると、上限判定で弾かれる。記録できる角度と、指令で再現できる角度がずれていた。
seq2 の j1 を上限内の90.0に丸めた。それ以外の関節は変えない。90.0で送ると、残差0.79度で到達する。
送信 seq2(j1=90.0): [-45.35, 90.0, -6.94, 0.87, -86.48, -29.53]
実測: [-45.61, 90.61, -6.41, 1.66, -86.04, -29.7]
HOMEからの移動量: 90.61
目標への残差 max|Δ|: 0.79掴めたかどうかを測る
台帳の掴み判定欄 pick_verdict は、ずっと空だった。このスクリプトは教示姿勢で機械的にグリッパーを閉じるだけで、掴めたかどうかを判定していなかったからだ。掴めても掴め損ねても、欄は空のまま。成否そのものを測っていない。
掴めたかを測るには、掴んだ瞬間に爪先を見ればいい。掴めていれば爪先にブロックがあり、掴め損ねたり弾いたりしていれば、ブロックは元の位置に取り残されて爪先から離れている。
二つの記録を足した。グリッパーを閉じる直前、爪先とブロックの位置差を幾何的に測る。爪先には青いシールが貼ってあり、その中点とブロック中心の距離をmmで出す。掴めていれば距離は小さく、取り残せば大きい。そしてグリッパーを閉じた直後、その場で一枚撮ってVLMに「爪にブロックが挟まっているか」を判定させる。持ち上げてからではなく、掴んだ瞬間に撮る。掴み直しはしない。判定して記録し、そのまま次へ進む。
まず幾何測定。掴む直前、関節を動かさずに上カメラで一枚撮り、同じフレームから爪先中点と赤ブロックを検出して差分を取る。検出に失敗しても None を記録して制御は止めない。
# STEP4.5: 爪先(青シール) vs block 差分測定（GRIP前・測定専用・動作非干渉・DAY243移植）
# 関節は動かさない。PICK到達姿勢のまま上カメラ1枚撮り、同フレームで
# 爪先中点と赤blockを検出し差分を mm で取る。失敗は None 記録で素通し。
try:
tip_img = rpp._capture_for_vlm(
"stage4_pick_tip", frames_dir, record, False, run_stem=run_stem)
rpp._OBSERVE_STATE["cap_tip"] = (str(tip_img) if tip_img else None)
tip_diff = rpp._measure_tip_vs_block(tip_img)
record["tip_pick"] = tip_diff
record["tip_capture_path"] = (str(tip_img) if tip_img else None)
rpp._OBSERVE_STATE["tip_pick"] = tip_diff
if tip_diff is not None:
print("[STEP4.5] tip dist={:.1f}mm (dx={:.1f} dz={:.1f})".format(
tip_diff["dist_mm"], tip_diff["dx_mm"], tip_diff["dz_mm"]))
else:
print("[STEP4.5] 測定スキップ（検出不足/前提破綻）")
except Exception as e:
print("[STEP4.5] [WARN] tip測定例外: {}（continue）".format(e))
record["tip_pick"] = None
record["tip_capture_path"] = None次にVLM判定。グリッパーを閉じた直後、その場で一枚撮って「掴めているか」を見る。yes/noと理由が返る。掴め損ねた場合でも、掴み直しはせず、結果を記録してそのまま持ち上げに進む。
# STEP5: gripper 閉
rpp._observe_phase("pkp_stage4_gripper_close")
rpp._OBSERVE_STATE["last_angles"] = get_angles_safe()
rpp._observe_emit_now()
print("\n[STEP5] gripper 閉")
try:
daemon_set_gripper(1); time.sleep(GRIPPER_SLEEP)
record["gripped"] = True
print("[STEP5] gripper close + sleep {:.1f}s".format(GRIPPER_SLEEP))
except Exception as e:
print("[STEP5] [WARN] gripper close 例外: {}".format(e))
record["gripped"] = False
rpp._observe_gripper(1)
# STEP5.5: PICK判定（grip直後・その場・LIFT前・VLM・判定のみ/リトライなし）
# 掴めていれば爪先にブロックがある→yes、掴め損ね/弾き→no。
# 結果が no でも続行（掴み直さない＝regrasp未移植）。撮影・判定の失敗は素通し。
try:
pick_img = rpp._capture_for_vlm(
"pick", frames_dir, record, False, run_stem=run_stem)
rpp._OBSERVE_STATE["cap_pick"] = (str(pick_img) if pick_img else None)
pj = rpp._judge_image(pick_img, "pick", False, None)
record["pick_judgment"] = pj
record["pick_capture_path_vlm"] = (str(pick_img) if pick_img else None)
rpp._observe_verdict(
pj.get("verdict"), pj.get("confidence"), pj.get("reason"))
print("[STEP5.5] PICK verdict={} confidence={} reason={}".format(
pj.get("verdict"), pj.get("confidence"), pj.get("reason")))
except Exception as e:
print("[STEP5.5] [WARN] PICK判定例外: {}（continue）".format(e))
record["pick_judgment"] = None
record["pick_capture_path_vlm"] = Noneこの二つの記録が、台帳の pick_verdict、pick_reason、pick_confidence、tip_pick_x/z、tip_block_dx/dz/dist_mm を埋める。grip前の幾何測定画像とgrip後のVLM判定画像は別物なので、tip_capture_path と pick_capture_path の二列に分けて両方残す。
止まらないようにする
PICKからPLACEへ動く途中で、アームが一度止まっていた。掴んだあと、置く場所の手前まで大きく動く。この大移動が60度を超えると、自動で2段に分割され、中間点で一度到達確認をして止まる。その中間点の到達判定が、目標との残差3.03度で止まった。判定の許容は3.0度。わずか0.03度足りずに未到達と判定され、次の段に進めずに通し全体が中断した。実際にはほぼ動ききっているのに、0.03度で止められた。
中間点で止まる挙動が不要なので、分割をやめて一発で送ることにした。ただ、一発で送って届くかは別問題だ。掴みに行く姿勢で、一発送信が無視される現象を踏んだばかりだった。同じことが起きないか、コードを変える前に単体で測った。掴んだあとのLIFT姿勢から、置く場所の手前の姿勢へ、83度の大移動を一発で送る。
LIFT姿勢へ移動...
実測: [-45.17, 89.03, -37.08, 3.69, -69.08, -29.61]
approach_place へ一発送信（83度の大移動）...
実測: [39.02, 60.11, 17.66, -0.35, -88.41, -29.61]
目標への残差 max|Δ|: 2.72 → 到達残差2.72度で到達した。この姿勢域は一発で届く。掴みに行く姿勢が届かなかったのは、そこのj1が上限を超えていたからで、こちらは上限内だ。中継分割を一発送信に置き換えた。未到達なら掴んだまま止まる安全はそのまま残す。
rpp._observe_phase("pkp_relay_pre_place")
rpp._OBSERVE_STATE["last_angles"] = get_angles_safe()
rpp._observe_emit_now()
print("\n[STEP7] PICK 終端 → approach_place_target へ一発送信")
try:
r_app_place_lookup = rpp.j0_lookup("approach_place", box_xz)
except Exception as e:
print("[ABORT] j0_lookup('approach_place', {}) 失敗: {}".format(box_xz, e))
record["aborted_at"] = "lookup_approach_place"
record["abort_reason"] = "j0_lookup_failed: {}".format(e)
_save(); append_index_row(record); append_ledger_row(record, run_stem, OUT_DIR, ledger_appended)
return 1
ok_guard, reason = rpp._check_guard(r_app_place_lookup, "approach_place_pre")
if not ok_guard:
print("[ABORT] approach_place 送信前ガード NG: {}".format(reason))
record["aborted_at"] = "guard_approach_place"
record["abort_reason"] = "guard_failed: " + reason
_save(); append_index_row(record); append_ledger_row(record, run_stem, OUT_DIR, ledger_appended)
return 1
approach_place_target = list(r_app_place_lookup["target_angles"])
print("[STEP7] approach_place_target = {}".format(fmt_angles(approach_place_target)))
# STEP7: LIFT→approach_place（約83度）を一発送信（中継分割をやめる）。
# 一発で残差2.72°到達を実機単体テストで確認済み。中継点で止まる挙動を除去。
# 未到達時の「掴んだまま停止」安全は最終到達判定で従来通り維持。
at7, pt7, mdt7 = send_and_wait(
approach_place_target, "pkp_approach_place_direct", TOL_RELAY)
record["relay_pre_place"] = {
"arrived": bool(at7), "post_actual": pt7, "max_residual_deg": mdt7,
"steps_done": 1,
}
if not at7:
print()
print("=" * 70)
print("[HOLD] approach_place 一発送信 未到達. 掴んだまま停止。")
print("[HOLD] recover-home --release で目視確認後に開放→HOME")
print("=" * 70)
record["aborted_at"] = "relay_pre_place"
record["abort_reason"] = "approach_place_arrival_failed"
record["note"] = "held_at_relay_pre_place"
_save(); append_index_row(record); append_ledger_row(record, run_stem, OUT_DIR, ledger_appended)
return 1もう一つ、別の止まり方があった。検出したブロックの位置が、登録済みの教示姿勢のどれとも一定以上離れていると、「近い姿勢がない」として自動選択が止まる判定だ。ブロックがマットの手前すぎず奥すぎず、二つの登録姿勢のちょうど谷間に落ちると、最近傍でも35.0mmの上限を超えて弾かれる。
[STEP3.5] 候補: reached_nearest=seq2 gap=35.3mm partial_nearest=seq3 gap=33.7mm
[ABORT] block_z=198.7mm に近い教示姿勢がない (最近傍 seq2, gap=35.3mm > Z_GAP_LIMIT_MM=35.0mm)35.3mmが上限35.0mmを0.3mm超えて止まった。この判定を撤去した。谷間でも、最近傍の姿勢で掴みに行く。掴める保証はないが、掴めたかは新しく入れたPICK判定とtip測定が記録する。掴め損ねても、それは記録として残る。
# Z_GAP_LIMIT 超過時の警告（ABORT はしない・最近傍 seq で続行）。
# 谷間でも掴みに行き、掴めたかは STEP5.5 PICK判定と tip_block_dist で記録する。
if z_gap is not None and z_gap > Z_GAP_LIMIT_MM:
print("[STEP3.5] gap={:.1f}mm > Z_GAP_LIMIT_MM={:.1f}mm だが ABORT せず "
"最近傍 seq{} を採用して続行".format(z_gap, Z_GAP_LIMIT_MM, selected_seq))完走の一行
これらを入れて、完走を一本通した。実機の全ログを残す。
======================================================================
[INIT] grasp_lift_pick_then_place.py run (auto-select) dry_run=False
[INIT] block_z 自動選択モード (--seq 廃止). seq は STEP3.5 で決定
======================================================================
[INIT] daemon 接続中 ...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[STEP1] HOME 移動
[STEP1] 現在 [-0.26, +1.49, +0.96, -0.26, +0.08, -29.70] → HOME 大|Δ|=1.49°
[SEND] pkp_to_home_pre arrived max|Δ|=1.49° (0.0s)
[STEP2] gripper 開
[Pi] GRIPPER: OPEN result= None
[STEP2] gripper open + sleep 2.0s
[STEP3] block + box 同時検出 detect_positions(repeat=3)
[STEP3] block_xz=[0.2127, 0.2326] box_xz=[-0.1967, 0.1882]
[STEP3.5] block_z = 232.6mm → seq 自動選択
[STEP3.5] 採用: seq=2 (z=234.0mm verdict=reached) gap=1.4mm rationale=reached_nearest
[STEP3.5] teaching = [-45.35, +90.00, -6.94, +0.87, -86.48, -29.53]
[STEP4] 教示姿勢 seq=2
[RELAY] pkp_to_teaching_seq2 full max|Δ| > 60° → 中継 2 step に分割（6軸補間）
[SEND] step1 arrival OK
[SEND] step2 arrival OK
[STEP4.5] tip dist=56.3mm (dx=-2.8 dz=-56.2)
[STEP5] gripper 閉
[Pi] GRIPPER: CLOSE result= None
[STEP5] gripper close + sleep 2.0s
[STEP5.5] PICK verdict=no confidence=0.85 reason=爪の先端は赤いブロックの近くにあるが、ブロックは爪の下（床面）に置かれており、爪が閉じてブロックを保持している状態には見えない。ブロックと爪の間に隙間が確認できる。
[STEP6] LIFT
[SEND] pkp_to_lift_seq2 arrived
[STEP7] PICK 終端 → approach_place_target へ一発送信
[STEP7] approach_place_target = [+38.94, +57.39, +17.49, +0.08, -89.20, -29.44]
[SEND] pkp_approach_place_direct arrival OK
[STEP8] approach_place 最終フィニッシュ
[STEP9] place_withblock 降下
[STEP10] gripper open (release)
[STEP11] swept_retreat
[STEP12] place verdict=no confidence=0.97 reason=赤いブロックはマットの左下付近に置かれており、白い箱（右下）とは明らかに離れた別の場所にある
[STEP13] lift_back
[STEP14] HOME 復帰
======================================================================
[DONE] seq=2 pick→place 完走
PICK arrived=True LIFT arrived=True relay_pre=True
PLACE app=True place=True release=True swept=True
VLM verdict=no conf=0.97
lift_back=True home=True
======================================================================
[INDEX] append OK: pick_then_place_index.csv
[LEDGER] append OK: /Volumes/KIOXIA/r2r2r-data/runs/pick_place_ledger_v2.csv (place_verdict='no')完走だが、掴めてはいない。STEP5.5 の判定が no を返している。理由は「ブロックは爪の下に置かれており、爪が閉じて保持している状態に見えない。ブロックと爪の間に隙間が確認できる」。STEP4.5 の幾何測定も、爪先とブロックが56.3mm離れていると出している。掴み損ねて、ブロックを元の位置に取り残したまま、空のグリッパーを箱の横に置いた。その置き場所も箱の外だった。
PLACEの no は、置き動作だけの失敗ではない。PICKで持てていないため、箱に入るブロック自体が無かった。空のグリッパーが、たまたま箱の外で開いた。
台帳に着地した一行の全33列を、列名と並べる。
timestamp = 2026-06-20T19:41:27
run_stem = pick_then_place_auto_20260620_194127
trial_no = 33
pick_verdict = no
pick_reason = 爪の先端は赤いブロックの近くにあるが、ブロックは爪の下（床面）に置かれており、爪が閉じてブロックを保持している状態には見えない。ブロックと爪の間に隙間が確認できる。
pick_confidence = 0.85
place_verdict = no
place_reason = 赤いブロックはマットの左下付近に置かれており、白い箱（右下）とは明らかに離れた別の場所にある
place_confidence = 0.97
block_pick_x = 212.7
block_pick_z = 232.6
block_place_x = 212.7
block_place_z = 232.1
box_x = -196.7
box_z = 188.2
in_box_geom = False
in_box_dist_mm = 411.8
final_j1 = 0.35
final_j2 = 1.58
final_j3 = 2.10
final_j4 = -0.26
final_j5 = -2.10
final_j6 = -29.79
gripper = 0
place_capture_path = /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place/pick_then_place_auto_20260620_194127_place.jpg
tip_pick_x = 208.9
tip_pick_z = 176.4
tip_block_dx_mm = -2.8
tip_block_dz_mm = -56.2
tip_block_dist_mm = 56.3
run_dir = /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place
tip_capture_path = /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place/pick_then_place_auto_20260620_194127_stage4_pick_tip.jpg
pick_capture_path = /Volumes/KIOXIA/r2r2r-data/mecharm_calibration/pick_z_postures_DAY243/pick_then_place/pick_then_place_auto_20260620_194127_pick.jpg空欄がない。以前は空だった列に、今回は値が入っている。
以前の空欄 | 今回の値 | 意味 |
|---|---|---|
pick_verdict | no | 掴めていない |
pick_reason | 爪とブロックの間に隙間 | VLMが見た失敗理由 |
tip_block_dist_mm | 56.3 | grip前から爪先とブロックが離れていた |
block_pick_z / block_place_z | 232.6 / 232.1 | ブロックはほぼ動いていない |
place_verdict | no | 箱にも入っていない |
in_box_dist_mm | 411.8 | 箱から大きく外れている |
この三方向が同じことを指している。pick_verdict=no はVLMの判断。tip_block_dist=56.3mm はgrip前の幾何。block_pick≒block_place は結果の座標。ブロックは掴まれず、PICK時の232.6mmからPLACE後の232.1mmまで、ほとんど動かずに元の場所に残った。爪だけが箱の横へ移動して開いた。一行のCSVが、その事実をそのまま抱え込んでいる。
完走と成功は別だ。スクリプトが最後まで走ったことと、ブロックを掴んで箱に入れたことは、同じではない。これまでは完走したかどうかしか見ていなかった。pick判定とtip距離を入れたことで、完走の中に失敗を見つけられるようになった。
成否を no の一文字で持っていたら、この中身は消えていた。何を持って、どこへ運び、何を置けなかったのか。そこまで残して初めて、次の修正点が見える。空欄は、失敗ではない。失敗を見なかったことにする穴だ。
