mechArm 270 Pi 動的追従の限界を探る Log#52 | pick&place観測画面の見直し。参照と姿勢選択を可視化する
pick&place の一試行が走るあいだ、ロボットが何を見て、どの姿勢を選び、いつ写真を撮ったのか。それが後から追えなかった。observe 画面はあるにはあったが、映していたのは関節角の断片だけで、判断の中身は闇の中だった。
この見直しは、観測画面を直す作業として始まった。stage history の内容修正、raw json の削除、判定理由(reason)の追加、そして「どのスクリプト・どのデータをどのタイミングで参照したか」を絵で捉えられるようにすること。目的ははっきりしていた。試行を可視化して改善の材料にしたい、そして何が動いているのかを把握したい。
結果として、観測画面を直す過程で、ずっと解けずにいた別の問題——箱にブロックを置くと縁に当たる不具合——の原因が、実機をほとんど動かさずに特定できてしまった。観測できる手段を持つことが、推論に先立つ。その順序が、この記録の背骨になっている。
出発点は observe_server.py ではなかった
依頼は「observe_server.py 見直します」だった。stage history の内容修正、raw json の削除、reason の追加(pick_reason と place_reason があるはず)、そして参照タイミングの可視化。
だが observe_server.py を読むと、このファイルには直すところがほとんど無いことがわかった。
observe_server.py がやっているのは、UDP でデータグラムを受け取り、最新の1個だけを保持して WebSocket へ流すこと。それだけだった。
_latest = {"datagram": None, "received_at_ms": None}stage history も、raw json も、reason も、このファイルには存在しない。stage history はブラウザ側(static/index.html)が届いたデータグラムの phase 変化を見て積み上げているだけ。reason は emitter がデータグラムに入れて送っている。raw json は UI がデータグラムをそのまま表示しているだけ。
つまり修正の主戦場は observe_server.py ではなく、run_pick_place.py(emitter)と static/index.html(表示層)だった。観測系の構造を最初に取り違えていたら、直すべきでない場所を延々いじっていたことになる。
stage history が「記録」ではないという構造的欠陥
いちばん重かったのはここだった。保持は最新1個。UDP は fire-and-forget で古いデータグラムを捨てる。WebSocket の push は 100ms 間隔。
したがって stage history は、ブラウザがたまたま受け取れた phase 変化の断片にすぎなかった。データグラムが UDP で落ちればその STAGE は履歴に現れない。STAGE の滞在が 100ms 未満なら push 間隔をすり抜ける。ブラウザをリロードすれば履歴は消える。サーバ再起動でも消える。
証拠は画面にあった。10 STAGE 走ったはずの通しが 1 STAGE しか残っていない TRIAL があった。ここに reason や参照元を足しても、同じ確率で落ちる。台帳の代わりにならない。
だから記録の正は emitter が run_dir に書く JSONL にすることにした。observe_server は「それを読んで配る」に役割を変える。ライブ表示は今のまま UDP でよい(消えても構わない用途)。
verdict が1枠しかない実バグ
raw json を読むと、こうなっていた。
"verdict": {
"success": true,
"reason": "白い箱の内側に赤い四角いブロックが明確に確認できる",
"confidence": 0.97,
"raw_verdict": "yes"
}この reason は PLACE の判定文。だが STAGE5 付近で出たはずの PICK の判定文はどこにも残っていない。単一の verdict スロットを PICK → PLACE で上書きしていた。
台帳は pick_reason(4/5列)、place_reason(7/8列)、side_place(60〜62列)と分けて持っている。テレメトリだけが潰れていた。「reason には pick_reason と place_reason があるはず」という指摘は正しかった。あるべきなのに、テレメトリでは片方が消えていた。
コードで確認すると、_observe_verdict の呼び出しは3箇所あり、いずれも同じ単一スロットに書き込んでいた。
3068: _observe_verdict(adopted, pj["confidence"], pj["reason"]) # PICK
3418: _observe_verdict("unknown", 0.0, "swept_retreat_failed")
3471: _observe_verdict(place_adopted, place_vlm["confidence"], place_vlm["reason"]) # PLACEstage9b で PLACE が書いた瞬間、stage5 の PICK 判定文は消える。スクリーンショットに写っていた「白い箱の内側に…」は PLACE のもので、PICK の reason はどこにも残っていなかった。
refs と decision のデータは、すでに作られていた
ここが転機だった。参照元(refs)と選択理由(decision)を可視化するのに、新しい計測を仕込む必要はなかった。run_pick_place.py の _record_lookup が、lookup 直後にこれを組んで log_record["lookups"] に積んでいた。
rec = {"label","phase","source_file","pose_id","target_xz_m","j0","method",
"x_residual_m","z_residual_m","interp_xz_m",
"joint_limit_ok","joint_limit_violations",
"target_angles","base_angles","nearest_pts",
"max_dj_deg","ee_y_m_selected","joint_tol_deg_used"}さらにガード判定後、guard_pass と guard_reason も同じ rec に入る。method / pose_id / source_file / guards / reason が全部あった。emitter に新しい計測を足す必要はなく、_record_lookup の rec を emit に渡すだけでよかった。本線の制御フローに触らない。物理リスクゼロ。
Phase 1(emitter)の実装と、実データが暴いた欠落
修正を Phase 1(emitter 側)と Phase 2(UI 側)に分けた。Phase 1 は run_pick_place.py と observe/emit.py に触れるが、既にあるデータを配線するだけなので物理リスクは無い。ただし本線ファイルなので dry-run 確認は要る。
emit.py の受け口に lookup と corr の2キーを足し、_OBSERVE_STATE に lookup/corr/captures を持たせ、verdict を pick/place/side_place の3スロットに分割し、STAGE 行を run_dir の telemetry.jsonl に追記する。ガード ABORT した STAGE も emit されるようにした——それまで「なぜ止まったか」が最も知りたい瞬間だけ、画面に何も出ていなかった。
実装後、実機 run の telemetry.jsonl を読むと、想定と違う挙動が見えた。
stage stage2 | None | None None
stage stage5 | fk_nearest | pick False
stage stage3_approach_pick | fk_nearest | True | None
stage stage4_pick | fk_nearest | True | None
stage stage5 | fk_nearest | True | None
stage stage7_approach_place | fk_nearest | True | None
stage stage9 | fk_nearest_ee_y_tol_empty_j4safe_pref | True
stage stage10 | fk_nearest_ee_y_tol_empty_j4safe_pref | True
stage stage9b_judge_place | ...同上
stage stage9b_judge_place | ...同上 # 2行
stage stage10 | ...同上 # 2行2つのバグが見えた。ひとつは lookup と corr が前の STAGE から持ち越される(stale)。_OBSERVE_STATE["lookup"] は _record_lookup が書くだけで、誰もクリアしない。非 lookup stage は前の値をそのまま引きずる。このまま図を描くと、stage9(release)や stage10(HOME復帰)が fk_lookup とFKテーブルを参照したことになってしまう。事実と違う。
もうひとつは同じ stage が2行出る。_observe_emit_now サイトと _do_move サイトの両方で _observe_stage_record を呼んでいた。
修正1は _observe_phase() で phase 遷移のたびに lookup/corr をクリアすること。修正2は run 単位で (stage, kind) の既出セットを持つ重複ガードにすること。修正3は dry-run の早期 return の直前でも STAGE 行を残すこと(観測系の検証を実機に依存させないため)。
dry-run で、本題の原因が先に見えた
修正を入れた後、実機ではなく dry-run で検証した。箱の位置に実機 run の実測値を与えて回すと、ログにこう出た。
[CORR ] key=place_x-180_z200 dx=-10.54mm dz=+40.00mm n=10
target (-175.5,+198.7) -> (-165.0,+158.7) [mm]
[HGT ] PLACE apply: cell=place_x-180_z200 dee_y=36.0mm → target_ee_y=0.1360m
[CORR ] PLACE 補正後 target が不安全 (j4=-104.20 out of [-90.0, 60.0]).
補正を破棄し raw box_xz で降下.
[CORR ] PLACE update skip: reason=detect_nonePLACE 補正のループが、両端で切れていた。適用は safety_rollback で破棄、更新は detect_none で skip。補正記憶は書かれるだけで、使われもせず、更新もされない。
telemetry.jsonl の1行を見ると、探していたものが全部入っていた。
"corr": {
"cell_key": "place_x-180_z200",
"target_before": [-175.5, 198.7],
"target_after": [-175.5, 198.7],
"applied_dx_mm": null,
"applied_dz_mm": null,
"apply_reason": "safety_rollback",
"updated": false
}同じ行の lookup が理由を語っていた。
"j4_selected_deg": -89.2,
"ee_y_m_selected": 0.092317,
"method": "fk_nearest_ee_y_tol_empty_j4safe_pref"補正なしですら j4 が -89.2°、限界(-90)まで 0.8° しかない。ここに高さ補正(dee_y)を足せば j4 は -104.2° に飛ぶ。safety_rollback は必然だった。
観測系を作る作業の途中で、ずっと解けずにいた「PLACE 補正が収束しない」問題の入り口が、実機を回さずに見えた。
「掴めた」が信用できない
台帳を時系列で見ると、ある時間帯から in_box_dist(ブロックが箱中心からどれだけ離れたか)が急に悪化していた。
20:33 pick=yes tipdist= 7.1 place=yes inbox=35.5
20:38 pick=yes tipdist=18.8 place=no inbox=453.3
20:40 pick=yes tipdist=20.4 place=no inbox=473.9place 後のブロック位置が箱から大きく離れている。当初これを「PLACE が置けていない」と読んだ。だが place の画像を見ると、ブロックは箱の中になく、マットの別の場所にあった。
観察が入った。爪がブロックに引っかかっていて、掴める位置になっていない。全部そうだ、と。
pick_verdict は全部 yes だった。VLM が「持ち上がっている」ことを見て yes を出していた。だが爪に引っかかって浮いただけのものも「浮いている」ので yes になる。tip_block_dist_mm は正直な指標で、掴めた試行は 6〜11mm、引っかかった試行は 18〜25mm ときれいに分離していた。判定は yes を出し続け、数字だけが真実を語っていた。
VLM-as-Reward の pick プロンプトは「リフト後、赤ブロックが床から浮いているか」を見ていた。浮きしか問うていないから、爪の端に引っかかって浮いたものと、正しく挟んで浮いたものを区別できない。横から見る place_side には「箱の縁に引っかかっている」を no とする基準があるのに、pick には横カメラ判定が無かった。
side_place の判定を、三度読み違える
観測画面には side_place という判定が出る。横カメラ(iPhone 11)から見た判定で、no が続いていた。
この no を、最初「PLACE 補正が効いていない証拠だ」と説明した。次に画像を見て「箱の外に置いた失敗だ」と言った。三度目に「箱に入っていない=失敗だ」と読んだ。どれも間違いだった。
ひとつ目の訂正が入った。PLACE する前だからブロックが箱に入っていないのは当たり前じゃないか、と。side_place の撮影は release 前、グリッパーがまだブロックを掴んでいる。その画像に「箱の中に収まっているか」と聞けば、掴んだままなのだから no が出て当然だった。プロンプトが撮影タイミングと噛み合っていなかった。
ふたつ目の訂正が入った。side_place が見るのは、白箱と爪が接触していないかだ、と。高さの数値でも「箱に入ったか」でもない。爪が箱にぶつかっていないか、適切な隙間があるか。それだけ。二値。
要は白箱と爪に適切な隙間があるようにするということね、と言葉が定まった。爪の底面と白箱の上縁の間の隙間があるか。隙間がある→離してよい(yes)。爪が縁に乗っている・めり込んでいる→ダメ(no)。
side_place の結果は、どこにも使われていなかった
コードを追うと、side_place の判定結果(side_pj)が使われているのは3箇所だけだった。log_record への記録、台帳への書き込み、ログへの print。制御分岐・補正学習・リトライ・成否判定に使われている箇所は、ひとつも無かった。
毎回の PLACE で横カメラ撮影をし、Anthropic API を叩いて VLM 判定をし、台帳に記録する。そこまでやって、結果は制御にも学習にも一切フィードバックされていない。撮って、聞いて、書いて、捨てていた。
これはこれで問題だ、という指摘が入った。というより、次の試行で同じ失敗にならないようにするのが正しい判断だ、と。試行してループが終わるまで止めない。1試行は最後まで走らせ、side_place の結果は記録して次の試行の補正に反映する。R2R2R の VLM-as-Reward そのもの。報酬は試行を止めるためではなく、次の試行のために貯めるもの。
補正記憶の機構も、そのために存在している。試行を重ねるほど収束する。side_place は、そこに欠けている高さ方向の教師信号を供給しうる。平面(XZ)は上カメラ由来の place_pre 検出で補正記憶に繋がっているが、高さ(dee_y)方向は上からは見えないため信号源が弱い。side_place こそがその信号源になり得た。
プロンプトを直しても、通らなかった
side_place のプロンプトを「箱の中に収まっているか」から「爪と箱の隙間(接触の有無)」に書き換えた。過去の4枚でテストした。
一度目は位置と高さの両方を判定させようとした。misaligned(水平ずれ)を横カメラに求めたが、横カメラは奥行きが見えない。4枚とも「奥行きが判別できない」で全滅した。
いや、そりゃそうだろ、位置判定は上カメラがやってるからね、という言葉が入った。判定軸を私が取り違えていた。横に見えないものを横カメラに聞いていた。
高さ一軸(接触の有無)に絞り直して、また4枚でテストした。今度は全部 yes になった。突っ込みすぎの例に選んだ画像まで yes と出た。confidence が4枚とも 0.72 で同じで、画像を見ずにプロンプトの型通りに答えているように見えた。
4枚では足りない。大量にランダムで
もっと大量の side_place 画像がフォルダにあるはずなので、ランダムに選択させて判定できるかを調べてほしい、という指示が入った。
place_side 画像は 424枚あった。ランダム15枚を判定させると、yes 12・no 1・unknown 2 でばらけた。全部 yes ではなかった。4枚がたまたま全部「隙間のある画像」だっただけだった。confidence も 0.55〜0.82 に散った。VLM は画像を見ていた。
40枚に増やして、台帳の in_box_dist と対照した。だが VLM の判定と in_box_dist は相関しなかった。ここでまた読み違えかけた——side_place は「接触」を見ていて、in_box_dist は「平面位置」を測っている。別の軸だから相関しなくて当然だった。接触センサーを位置センサーで採点していた。
VLM=no を出した画像を目で見ると、3枚とも爪が箱に接触・突っ込みしていた。no の精度は照合した範囲で正しかった。特に一枚は、上カメラでは place=yes・in_box=14.6mm(平面的には成功)なのに、横から見ると爪が箱に深く入っていた。上カメラだけでは分からない「降下が深すぎ」を、side_place だけが捉えていた。
side_place は非対称なセンサーだった。no(接触/深すぎ)は信頼でき、yes(非接触)には見逃しがある。
撮影が5回しか映らない。実際は6枚ある
撮影タイミングも可視化した。cam:top と cam:side の2レーンを REFERENCE LANES に足した。だが画面は撮影5回、実際のフォルダには6枚。1枚足りなかった。
place_pre(release 前に上カメラで撮り、ブロック位置を検出して補正学習に使う画像)が図に出ていなかった。原因を追うと、place_pre は place_side と同じ phase(stage8_5)で撮られるため、重複ガードが2枚目を弾いていた。ガードのキーを (stage, kind) から (stage, kind, label) に変えて、同じ phase の複数撮影を両方残すようにした。
place_pre は補正学習のトリガーそのものだった。この撮影が可視化から漏れていると、「なぜ PLACE 補正が更新されないか」を図で追えない。ちょうど本題の切り分けに直結する撮影が、図から欠けていた。
補正が破棄されるのは、姿勢が辞書に無いからだと考えた
ここから本題の原因究明に入った。PLACE 補正が safety_rollback される。dee_y が上限まで膨らむ。placeが箱に当たる。全部ひとつの原因、place_x-180_z200 セルで j4 が限界だから、と読んだ。
最初の仮説は、FKテーブルに「爪が高くて j4 が安全な姿勢」が無いこと——A_PLACE の被覆不足(FK再スキャンが必要)だった。これを確認するため、PLACE 帯の j4 分布を実測した。
cell x-180 z200: 全8005点 / ee_y>=100mm 3736点 / うちj4>-85(余裕) 351点穴は空いていなかった。問題のセルに、j4 余裕のある高い姿勢が 351点あった。FKテーブルの md5 も bba3df73・1,659,958点で、私が前提にしていた古い版とは違っていた。既にスキャンが進んでいた。FK再スキャンは要らなかった。
姿勢はある。lookup が選べていない
目標(-180, 200)の半径10mm以内に、j4 余裕の姿勢が 255点あった。
ee_y=306.4 j4=-75.0 dist=4.1mm
ee_y=297.1 j4=-80.0 dist=1.7mm
箱の位置の真上に、爪を高く保てて手首も安全な姿勢がいくらでもあった。だが fk_lookup は ee_y ≈ 92mm(j4=-89.2°)を選んでいた。fk_lookup は「目標の ee_y に近い点」を選ぶ。目標 ee_y が低く(箱の高さ付近)設定されているから、低い ee_y の j4限界姿勢が選ばれる。
ee_y 帯別に j4 の余裕を見ると、箱に降ろす高さ(85〜130mm)にも j4=-45〜-55° の安全姿勢が各帯180点ずつあった。同じ高さ帯に、j4 が -45° から -106° まで幅広く同居していて、lookup がその中から限界寄りを選んでいた。被覆の問題ではなく、選択の問題だった。
安全確認の lookup が、本番と別の姿勢を見ていた
fk_lookup を読むと、J4_SAFE 内優先の2段ソートも j4 ペナルティも実装済みだった。place_withblock の本番呼び出し(2894)は j4_safe_min/max を渡していた。だが補正の安全確認をする別の lookup(3479)は、その引数を渡していなかった。
本番と安全確認が別の姿勢を選び、安全確認が悪い姿勢を掴んで「j4=-104 out of range」と判定し、補正を捨てていた——と読んだ。だが実データで確認すると、本番 lookup すら j4=-89.2° を選んでいた。method は全 run で fk_nearest_ee_y_tol_empty_j4safe_pref。empty が入っていた。J4_SAFE 内候補が0と判定され、フォールバックしていた。
候補は空ではなかった
外からフィルタを再現して候補数を数えた。ee_y 帯 × xz近傍 × safe_mask で J4_SAFE 内が223点。連続性をかけても136〜194点残る。どう絞っても候補は空にならなかった。私が外から再現したフィルタと、fk_lookup 内部の実際の絞り込みが食い違っていた。
fk_lookup を読むと、全6軸連続性フィルタ(np.all で全軸が approach ±joint_tol)と place_ee_y_tol(目標 ee_y ±5mm)があった。私は j4 だけで連続性を試していた。実際は6軸 AND。ee_y も帯でなく目標±5mm。
実際の approach_place 姿勢で再現すると、こうなった。
approach_place 姿勢: [40.1, 57.4, 20.0, 0.1, -89.2, -29.4]
全6軸連続性±15 ∩ xz近傍 ∩ safe: 1060点 / SAFE内 650
+ ee_y[92±tol=5mm]: 80点 / SAFE内 50候補は50点残っていた。空ではなかった。50点の SAFE 内候補があるのに、j4=-89.2° を選んでいた。approach_place 自身が j4=-89.2° で降りてきていた。連続性±15°をかけると候補は j4=-104〜-74 に寄り、j4=-50° の余裕姿勢は approach から遠すぎて削られていた。
犯人は approach_place の姿勢ref
approach 帯(ee_y 110-130mm)で、j4 余裕(-74〜-79°)・j1≈44 の姿勢が33点実在した。その族を approach ref にすると、place へ±15°連続性で SAFE 内65点・j4余裕58点が候補に入る。
設定値を確認した。
APPROACH_PLACE_J1_REF = 58.35
APPROACH_PLACE_J4_REF = -89.38approach_place の姿勢 ref が、手首を限界(-89.38°)に張り付かせていた。approach がそこから±5°の族拘束で降り、place はその approach から全6軸±15°しか動けず、j4=-50° の良い姿勢に乗り換えられなかった。上流の姿勢が悪いから、下流も悪いままだった。
approach 帯で j4 余裕姿勢の中央値は j1=44.1 / j4=-76.0。この族(j1=44±5 ∩ j4=-76±5)は33点実在し、そこから place へ±15°で SAFE 内65点繋がる。J1_REF と J4_REF は対で変える(j4 だけ変えると組がテーブルに無く approach が空振りする)。
補正記憶を退避してから、姿勢を直す
approach を直しても、補正記憶に dee_y=40(上限)が焼き付いていると、それが再び j4 を限界に押し込む。approach 修正と補正記憶リセットはセットでないと効果が出ない。この指摘はコードレビュー(codex)からも入った。
place_height_memory と correction_memory の place セルを archive へ退避してからクリアした。コピーのみ、pick セルは温存。
place_height_memory.json 10 -> 0 (place セル削除)
correction_memory.json 27 -> 21 (place セル削除)そのうえで、approach_place の姿勢 ref を修正する2定数変更をコードレビューに回した。物理リスク(joint制御の姿勢変更)があるため、実機の前に codex/gemini レビューを挟む。全箱位置共通の定数なので、既に正常だった place_x-200_z200 が巻き込まれて悪化しないか(回帰)、補正記憶をクリアした状態で approach 是正だけで箱に当たらず置けるか、をレビュー観点に明記した。
何度も外して、実データで着地した
この記録の実質は、原因を何度も取り違え、そのたびに実データや観察に覆された過程そのものにある。
補正記憶が悪いと考えた。違った。姿勢が辞書に無いと考えた(A-3が必要)。違った、姿勢は351点あった。lookup が選べていないと考えた。半分正しかったが、安全確認の引数欠落だと絞ったら、本番すら選べていなかった。候補が空だと考えた。空ではなく50点あった。approach が悪いと考えて、ようやく実データが一致した。
side_place の判定意図も三度読み違えた。「補正が効いていない証拠」「箱の外に置いた失敗」「箱に入っていない」——全部違い、正しくは「爪と箱の接触の有無」だった。そのたびに、PLACE する前だから当たり前だ、位置判定は上カメラがやってる、白箱と爪の隙間を見るんだ、という短い訂正が、推論を現実に引き戻した。
観測画面を直す作業として始めたことが、その画面と記録データを通じて、本命の不具合を実機をほぼ動かさずに approach_place の姿勢 ref 一組まで絞り込む道具になった。測れる手段を持つことが、推論に先立つ。数字と観察が、思い込みより強い。
掲載コード（全量）
この log で扱った観測系の構成ファイルを全量で残す。省略・抜粋はしない。後から本人または AI が見返して使う再現性アーティファクトとして完全であることを優先する。
run_pick_place.py
run_pick_place.py — emitter 本体。verdict 3分割・lookup/corr/captures の emit・telemetry.jsonl 追記・重複ガード・approach_place 姿勢 ref 修正を含む。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""run_pick_place.py — 通し Pick & Place 本体（j0 対応表方式・実装単位2）
赤ブロックを上カメラで検出 → j0 対応表で各 phase の j0 を引く → 単発 send_angles で
基準姿勢へ移動 → グリッパー閉/開 → 箱（DAY232 確定固定値）へ運ぶ。
段ごとに lookup 結果と移動結果を構造化ログに全量記録する。
設計の要点（codex/gemini レビュー反映版）:
- 流用: j0_lookup / j0_sweep_scan._send_and_wait_arrival / visual_servo_pick の検出と daemon I/F
- 姿勢: lookup の target_angles をそのまま送る。J5 を -30 へ強制上書きしない
- 送信: 単発 send_angles のみ（_send_and_wait_arrival 経由）。ストリーミングしない
- ガード: joint_limit_ok / method=interp / x_residual<=THRESH の3条件を送信前に検査
- z_residual: 工程跨ぎ問い合わせでは構造的に大きい（pick弧 vs 箱z）。情報ログのみで ABORT しない
- グリッパー: daemon_set_gripper の戻り値は判定に使わない。各操作後に sleep>=2s（DAY175 対策）
- 工程跨ぎ位置データ禁止: block_xz は pick 系のみ、BOX_XZ は place 系のみ
絶対ルール:
- send_coords() / release_all_servos() を呼ばない
- Stiffness/Damping を触らない
- fk_scan_motion_based_v2.csv を書き換えない
CLI:
python3 run_pick_place.py # 一本通し実機実行
python3 run_pick_place.py --dry-run # 送信せず lookup+ガード判定だけ出力
python3 run_pick_place.py --dry-run --dummy-block-x 0.05 --dummy-block-z 0.30
"""
from __future__ import print_function
import argparse
import csv
import json
import math
import os
import sys
import time
import types
from datetime import datetime
from pathlib import Path
# ---- パス解決 ----------------------------------------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
_DRAG_TEACHING_DIR = _SCRIPTS_ROOT / "dynamic_tracking_limits" / "drag_teaching"
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
sys.path.insert(0, str(_DRAG_TEACHING_DIR))
# ---- pymycobot スタブ（waypoint_replay 経由の import を Mac 上で通すため） --
if "pymycobot" not in sys.modules:
_pkg = types.ModuleType("pymycobot")
_mod = types.ModuleType("pymycobot.mycobot")
class _MyCobotStub(object):
def __init__(self, *args, **kwargs):
raise RuntimeError(
"run_pick_place.py: MyCobot stub is not callable. "
"アーム制御は daemon TCP 経由のみ。")
_mod.MyCobot = _MyCobotStub
sys.modules["pymycobot"] = _pkg
sys.modules["pymycobot.mycobot"] = _mod
# ---- 流用 import ------------------------------------------------------------
# 段3/4/7/8 の j0 決定（B 案 = x のみマッチ）
from j0_lookup import j0_lookup # noqa: E402 (全 stage で利用 / DAY257 復旧)
from fk_lookup import fk_lookup # noqa: E402 (将来の実験用に温存。現状未呼出)
# DAY257 後段: descend (stage4_pick) のみ fk_lookup に切替。
# DAY262: place_withblock も fk_lookup 化 (円弧 lookup では x-z が円弧上のみで Z を狙えない.
# in_box=False 2 本 (Z 残差 +36.4/+83.6mm・両方奥) の解決策として X-Z 独立化)。
# approach_place は j0_lookup 維持 (中間姿勢を壊さない)。
# DAY277: approach_pick を fk_lookup 化. j0_lookup(円弧・x のみ) と descend の
# fk_lookup(ee 最近傍) が別基準の j0 を出す二重基準を解消し, approach で xz を
# 姿勢族拘束付きで置き, descend で j0 lock して「真下降下」を実装する.
# fk_lookup は j1<=88 safety 復活版で、descend で z を動かせるようにする目的。
# OFF にする場合は () に戻すか、"pick" / "place_withblock" を抜く。
FK_LOOKUP_PHASES = ("pick", "place_withblock", "approach_pick",
"approach_place")
# DAY262: place_withblock 用の ee_y 帯 (PICK 用 10-40mm 床近く帯とは別).
# 箱高さ 43mm を中心に ±10mm 強. 仮値 (measure-first: 初回実機で out_of_range 頻度と
# 落下点を見て調整). 帯外 → fk_lookup が method="out_of_range" を返して _check_guard が
# ABORT する (送角しない). PICK 側 (EE_Y_MIN/MAX_DEFAULT) は fk_lookup.py 側で保持.
PLACE_EE_Y_MIN = 0.085 # m (85mm・DAY263 70mm 確定 → DAY268 白箱接触回避で +15mm)
PLACE_EE_Y_MAX = 0.115 # m (115mm・DAY263 100mm 確定 → DAY268 白箱接触回避で +15mm)
# DAY265: PLACE の FK 点選択に姿勢連続性 (全軸帯フィルタ) を入れる.
# approach_place 6 軸から ±PLACE_JOINT_TOL_DEG 以内の点のみ候補にし、
# その中で ee_y 帯 ∩ x-z 最近傍を選ぶ (j1/j4 に歪みが逃げるのを防ぐ).
# 値は dry-run (fk_lookup CLI で候補数と残差を tol=10/15/20/25 振り) で確定.
# 実装時仮値 15.0. dry-run 結果でエイイチ確定後に固定.
PLACE_JOINT_TOL_DEG = 15.0
# DAY265: 白箱激突二重防御 (ee_y 下限). 帯フィルタが正しければ発火しないはずの保険.
PLACE_EE_Y_FLOOR = 0.085 # m (85mm・PLACE_EE_Y_MIN と同値・DAY263 70mm 確定 → DAY268 白箱接触回避で +15mm)
# DAY274 案B': PLACE 降下の前腕姿勢下限 (箱側面激突防御).
# fk_nearest が (ee_x, ee_z) 距離のみで選ぶと、前腕を寝かせた低 j1 姿勢
# (例: 2026-07-05 実測 j1=46.65・ee_y=105.3mm) が勝ち、爪先は高くても前腕が箱を擦る.
# 弁別軸は ee_y ではなく j1. DAY235 place_withblock base j1=59.15° (実測: 当たらない
# 安全姿勢) から余裕 5° を引いた 55° を下限とする. 実装前実測 (2026-07-06):
# - 台帳 362 trial 全域で ABORT 増分 +1 (現状 80 → B' 81)
# - 今日の当たるセル (box=(-197,+179)) で j1 46.65 → 57.39 に切替わることを確認
# - resid p50 1.71→2.15mm (差 0.44mm、GUARD 20mm を大幅下回る)
# 今回対象は stage8 place_withblock のみ. stage7 approach_place は j0_lookup 経由で
# adopted JSON base_angles の j1=57.39° 固定 (既に安全) のため手当て不要.
PLACE_J1_MIN_DEG = 55.0
# 段1/3/4/6/7/8/10 の送信・到達確認（単発 send_angles + 沈黙リトライ最大1回）
from j0_sweep_scan import ( # noqa: E402
_send_and_wait_arrival,
_daemon_get_angles_filtered,
)
# 段2 の block+box 同時検出（detect_positions が内部で camera_resolver →
# cv2.VideoCapture → warm-up → setup_homography_and_mat_range → detect_red_block と
# 06f detect_white_box_once を一括で叩く。中央値集約 repeat 対応）
from detect_positions_once import detect_positions # noqa: E402
from visual_servo_pick import ( # noqa: E402
get_daemon_socket,
close_daemon_socket,
daemon_set_gripper,
daemon_set_gripper_value, # DAY261: close の push 保持用 (open は state(0) のまま)
daemon_set_fresh_mode,
daemon_send_angles, # DAY273 (fix): PLACE 再照準 (_visual_servo_loop_place)
# で使用. run_0001_20260705_182346 が NameError で 1 ループ目
# に即死していた (未 import). PICK 側 visual_servo_loop でも
# 実運用中の関数を同じ名前で流用する.
daemon_get_angles, # DAY273 (fix): 同上. 再照準ループ内の j0 実測用.
# 既存 _daemon_get_angles_filtered (j0_sweep_scan 由来) は
# base 軸フィルタ用途で意味論が異なるため raw を使う
# (PICK 側 visual_servo_loop L792 と同じ).
detect_red_block,
pixel_to_unity,
)
# VLM 判定用のフレーム撮影と Anthropic API 呼び出し
import cv2 # noqa: E402
import numpy as np # noqa: E402
from camera_resolver import resolve_cameras # noqa: E402
from vlm_judge import judge as vlm_judge_call # noqa: E402
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# 観測モジュールが理由で制御が止まらないよう、import を guard する
try:
from observe.emit import emit as _observe_emit # noqa: E402
except Exception:
def _observe_emit(*_args, **_kwargs): # 観測無効でも制御は動く
pass
# ---- 定数（仕様準拠） -------------------------------------------------------
HOME = [0, 0, 0, 0, 0, -30]
SPEED = 20
BOX_XZ = (-0.010, 0.108) # DAY232 実測値。dry-run のデフォルト + 緊急時固定値として残置
# 実機 run では段2 で実検出した box_xz を使う（DAY236〜）
X_RESID_THRESH = 0.03 # m。これ超で ABORT
# DAY277 (エイイチ判断): 0.02→0.03 に緩和.
# 実機 stage4_pick が x_resid=0.02286m で
# 止まった件への対処 (ガードを緩めて完走優先).
BOX_SCORE_THRESH = 0.5 # 06f のテンプレ score。これ未満は箱位置不確実として段2 ABORT
# 根拠: DAY230 実績 score 0.618、06f 既定 TEMPLATE_SCORE_THRESHOLD=0.5
GRIPPER_SLEEP = 2.0 # s。DAY175 対策（>=2s）
GRIP_VALUE = 20 # close の push 保持目標値（DAY261 tour 実測の暫定値）。
# 閉じ切り state(1) は「位置で止まり握り直さず落ちる」(DAY259)。
# set_gripper_value(20) で閉じ切らず押し込み力を残す。
# gv は 開=97 / 把持厚み相当≈36 / これより小さいほど押し込み深い。
# 効かなければこの値のみ振る（descend 把持は tour と条件が違う点に留意）。
STAGE9_RELEASE_PRE_SLEEP = 2.0 # s。stage9 release は直前 PLACE 後に一呼吸置く
STAGE9_RELEASE_OPEN_ATTEMPTS = 2 # open→sleep→open で stage9 の離しを確実化
TOL_APPROACH = 2.0 # approach 系
TOL_APPROACH_PLACE = 3.0 # stage7 approach_place 専用（大移動で最後の数度が埋まらない対策）
TOL_SWEPT_RETREAT = 3.0 # stage9b swept_retreat 専用（残差2.55°で未到達になる対策・他stage非共有）
TOL_LIFT_BACK = 3.0 # stage10 lift_back 専用。実測残差 2.46〜2.55°（j2主因）を吸収。
# stage9b TOL_SWEPT_RETREAT=3.0 と同形。他stageと非共有
TOL_PICK_PLACE = 3.0 # pick / place 系
TOL_HOME = 2.5 # HOME 復帰
SEND_WAIT_SEC = 1.0 # daemon 側送信直後の get_angles ログ遅延（DAY268 0.3→1.0 に戻し・j4 未到達/HOME復帰不能多発のため。短縮は j4 物理ストール切り分け後に再検討）
DETECT_REPEAT = 3 # 段2 検出の中央値集約サンプル数
SWEPT_J0_FOR_PLACE_JUDGE = 85.0 # 段9b の退避 j0（approach_place 採用版スイープ端＝採用済み値）
MAX_PICK_ATTEMPTS = 3 # pick=no 掴み直しの最大試行回数（初回含む）
SWEPT_J0_FOR_PICK_RETREAT = 85.0 # 掴み直し前のカメラ退避 j0（approach_pick base で振る・place の退避と同形）
VLM_CAPTURE_WARMUP_FRAMES = 10 # VLM 撮影前の warm-up（DAY268 30→10 短縮）
VLM_CAPTURE_DROP_FRAMES = 3 # 撮影直前のバッファクリア（DAY268 5→3 短縮）
# JSON・画像の出力先（KIOXIA・1 run = 1 フォルダで集約）。
# LEDGER_DIR と同じドライブ。未マウント時の誤書き込みは run() 冒頭ガードで防ぐ。
LOG_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")
# ---- 試行台帳 CSV（KIOXIA SSD 専用・ローカルへフォールバックしない）-------
LEDGER_DIR = Path("/Volumes/KIOXIA/r2r2r-data/runs")
LEDGER_CSV_PATH = LEDGER_DIR / "pick_place_ledger_v3.csv"
# in_box 判定の閾値（mm）: 白箱半幅 40mm + ブロック半幅 12mm ≈ 52mm 目安.
# 実機を見て調整可能な 1 定数.
IN_BOX_TOL_MM = 52.0
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
# tip 測定で得た Δ(dx,dz) をブロック (x,z) のセル別に蓄え、次回 lookup 直前に
# block_xz から Δ を引いて狙いを補正する。EMA で 1 セル 1 値に集約。
# 記憶 JSON は KIOXIA SSD の diag/ 配下。実機ローカルには書き込まない。
CELL_MM = 20.0 # セル幅 (mm)
EMA_ALPHA = 0.5 # EMA 重み (新 : 旧)
MAX_CORRECTION_MM = 40.0 # 補正量の絶対値クランプ (mm)
# PLACE 補正 (DAY268 案1): 学習採用の距離閾値.
# in_box_dist_mm がこの値以下なら place_verdict に関わらず Δ を学習に採用する.
# 箱 80x80mm・半対角 ≈56.6mm を外に余裕を取った値. 惜しい no (48〜58mm) を拾い,
# 大外し no (137/151/241mm) を捨てる. 台帳 place_x-160_z220 の 7 試行 (56mm 前後で
# 足踏み) が学習に回るようになり, no が続くセルの逆進性を解消する.
PLACE_LEARN_DIST_MAX_MM = 90.0 # 【旧定数・未使用】DAY274 で更新条件を pick=yes ゲート +
# 破綻ガード 3 種に切り替えた際にゲートとしては使わなくなった.
# 現行の外れ値ゲートは PLACE_CORR_MAX_DIST_MM=120.0.
# data_manifest 方針で対比記録として残置 (削除しない).
# DAY274 案 PLACE 補正記憶起動: 破綻ガードと補正後姿勢の安全チェック用定数.
# 起動と同時に必須 (過去に PLACE 補正を汚染で全削除した前科あり).
# 目的: 「pick=yes 全試行を対称に学習しつつ、検出破綻・過去残像を絶対に混ぜない」.
PLACE_CORR_SIGN_FLIP_GAP_MM = 150.0 # sign flip AND gap>150mm を PICK残像/別物拾いと判定
PLACE_CORR_MAX_DIST_MM = 120.0 # dist(block_place - box) > 120mm を外れ値と判定
# DAY277 (E2 修正): J4_SAFE を PICK/PLACE 共通のモジュール定数に一本化.
# 数値は不変 (-90.0 / +60.0). 参照点は 2 箇所:
# (a) _check_guard 内の送信前 backstop 検査 (全経路一律・DAY277 追加).
# (b) 補正が適用された場合の fk_lookup 事前検査 (L2721 以降・DAY274 既存).
# fk_lookup 側の safe_mask には J4_SAFE は含まれず (fk_lookup.py L255 の設計:
# 「呼び出し側の事前検査で追加確認する」), (a) が全経路の backstop となる.
# 補正後 target で fk_lookup した姿勢の j4 がこの範囲外なら補正を破棄し raw box_xz で降下
# (servo が深い j4 領域=swept_retreat スタール -89.2 付近へ補正で押し込まないため).
J4_SAFE_MIN = -90.0
J4_SAFE_MAX = +60.0
# 旧名 (L2739-L2744 で参照) は本モジュール内でのみエイリアスとして残す.
# 外部モジュールから import されていないため削除しても実害は無いが, PLACE 補正
# 事前検査の意図 (「補正が押し込むな」) を局所コメント併記で残す目的で存置.
PLACE_CORR_J4_SAFE_MIN = J4_SAFE_MIN
PLACE_CORR_J4_SAFE_MAX = J4_SAFE_MAX
# PLACE/PICK 補正 (DAY268): 1 更新あたりの変化量クランプ.
# EMA 出力が前回値からこの幅を超えて動かないよう制限する. 60→241→24 のような
# 1 試行での過補正オーバーシュートを抑える. 絶対値天井 (MAX_CORRECTION_MM=40) とは
# 別で, こちらは「1 回でどれだけ動けるか」の制限.
MAX_CORRECTION_STEP_MM = 15.0
# ---- DAY272 案T: PLACE 再照準 (stage8.5 visual servo) ----------------------
# stage8 到達後・stage9 リリース前に、上カメラで保持ブロック X と箱中心 X の
# ズレを j0 だけで締める. Z は触らない (DAY200 地雷回避).
# PICK 視覚サーボ (visual_servo_pick.visual_servo_loop) の複製移植.
# PICK 本線関数は 1 バイトも改変しない (DAY249 長期安定運用の保全).
#
# 係数 MM_PER_DEG_J0_X_PLACE:
# PICK 側 MM_PER_DEG_J0_X = -6.63 は PICK 姿勢 (j1=87.74 / j4=-70.11) で
# 実測校正された値. PLACE 姿勢は stage8 の j1≈55 / j4≈-89 と PICK と大きく
# 異なる (j1 が 33° 浅く j4 がさらに折れている) ため係数は姿勢依存で正確に
# 一致しない. 初期値として PICK 値を採用し, 実機ログ (下記 history) から
# 後段で校正する前提. 符号 (負値) は幾何的に姿勢を跨いで保存されるため反転不要.
# 実測校正のため, サーボループの各反復で
# delta_j0_actual (実測 j0 変化) と delta_block_x_actual (実測 block_x 変化)
# を log_record["servo_place"]["history"] に残す. 台帳新列は追加しない
# (67 列ヘッダ不変・[ABORT-APPEND] 回避).
MM_PER_DEG_J0_X_PLACE = -6.63
PLACE_CONVERGE_THRESHOLD_MM = 8.0 # 箱内幅 80mm に対し PICK(5.0) より緩め
PLACE_MAX_DELTA_DEG = 5.0 # 1 ループ最大 delta (自動運転暴走保護).
# PICK は DAY249 で撤去したが PLACE は自動運転
# かつ係数未校正のため再導入.
PLACE_SERVO_MAX_ITER = 10 # ループ上限 (時間ガードと二重で強制収束)
PLACE_SERVO_MAX_SEC = 15.0 # 時間上限
PLACE_SERVO_LOOP_WAIT_SEC = 0.5 # ループ間 sleep (PICK LOOP_WAIT_SEC と同値)
PLACE_SERVO_DETECT_FAIL_MAX = 3 # 連続検出失敗許容回数 (超えたら再照準中止)
# ---- DAY277: PICK 視覚サーボ (PLACE の複製移植・逆方向) --------------------
# PLACE 側 `_visual_servo_loop_place` / `_run_place_visual_servo` を PICK に対称
# 移植する. j0 だけ更新し j1..j5 は stage4_pick 到達姿勢に固定. 目標は「TIP_X を
# 検出 block_x に寄せる」(PLACE が 「保持 block_x を box_x に寄せる」の対称).
#
# 係数 MM_PER_DEG_J0_X_PICK:
# visual_servo_pick.py L69 の MM_PER_DEG_J0_X = -6.63 (PICK 姿勢 j1=87.74 /
# j4=-70.11 で実測校正済). PLACE 係数 (MM_PER_DEG_J0_X_PLACE) は流用しない
# (係数は姿勢依存・仕様書 §3-2 の明示).
#
# ガード定数の由来:
# PICK_CONVERGE_THRESHOLD_MM = 10.0 ← arm_kinematics.md PICK_VISUAL_SERVO 記述
# (visual_servo_pick.py L71 の 5.0 より緩め・
# arm_kinematics に合わせる. 仕様書 §3-2).
# PICK_MAX_DELTA_DEG = 5.0 ← arm_kinematics.md (DAY249 で
# visual_servo_pick からは撤去されたが本経路
# (自動運転) では PLACE と同じく再導入・
# 仕様書 §3-2).
# PICK_SERVO_MAX_ITER = 10 ← PLACE と同値 (仕様書 §3-2).
# PICK_SERVO_MAX_SEC = 15.0 ← PLACE と同値.
# PICK_SERVO_LOOP_WAIT_SEC = 0.5 ← visual_servo_pick.py L72 と一致.
# PICK_SERVO_DETECT_FAIL_MAX = 3 ← PLACE と同値.
#
# PICK は PLACE の PLACE_SERVO_OBSERVE_ONCE のような観測モードを持たない.
# 通常収束ループのみ (PLACE は係数校正観測目的の暫定フラグを持つ).
MM_PER_DEG_J0_X_PICK = -6.63
PICK_CONVERGE_THRESHOLD_MM = 10.0
PICK_MAX_DELTA_DEG = 5.0
PICK_SERVO_MAX_ITER = 10
PICK_SERVO_MAX_SEC = 15.0
PICK_SERVO_LOOP_WAIT_SEC = 0.5
PICK_SERVO_DETECT_FAIL_MAX = 3
# ---- DAY277: A_PICK fk_lookup 化 + descend j0 lock ----------------------
# 依頼書 §3 準拠. dry-run で確定した値を呼び出し側で fk_lookup に渡す
# (コードに焼かない方針は「定数 = 呼び出し側の名前空間」で満たす).
#
# APPROACH_PICK ee_y 帯 (上空・approach 姿勢族の median 112mm ± 12.5mm).
# dry-run 実測: FK approach 姿勢族 (j1..j5 base±5°) の ee_y ピーク [110,120)mm.
APPROACH_PICK_EE_Y_MIN = 0.100
APPROACH_PICK_EE_Y_MAX = 0.125
# APPROACH 姿勢族拘束 (adopted JSON base_angles L112 準拠).
# 良い族 j1=87.74/j4=-70.11 は base 差 <1.3°. 悪い族 j1=64/j4=-35 は差 >20°.
# tol=5° で分離できる (dry-run 確認済).
APPROACH_PICK_J1_REF = 87.74
APPROACH_PICK_J4_REF = -70.11
PICK_J1_TOL_DEG = 5.0
PICK_J4_TOL_DEG = 5.0
# DESCEND j0 lock (真下降下).
# FK grid の j0 step 5° より狭く, approach±3° 内に descend を強制.
PICK_J0_LOCK_TOL_DEG = 3.0
# ---- DAY278: A_PLACE fk_lookup 化 + descend_place j0 lock (A_PICK 同型) --
# 依頼書 §背景 準拠. stage7 approach_place を j0_lookup (円弧・x のみ) から
# fk_lookup に切り替え, 姿勢族拘束 + descend j0_lock で「真上→真下」を成立させる.
#
# APPROACH_PLACE ee_y 帯 (上空・approach_place 姿勢族の Stage 0 実測):
# adopted sweep ee_y: min=112mm / median=120mm / max=120mm (n=20・stdev=2.4)
# FK 姿勢族 (j1..j5 base±5°) の ee_y ピーク [110,120)mm (1,557 行).
# 実測 median±10mm で 20mm 窓 = [110, 130)mm.
# place_withblock descend 帯 [85,115]mm と方針整合 (approach は同帯かやや上).
APPROACH_PLACE_EE_Y_MIN = 0.110
APPROACH_PLACE_EE_Y_MAX = 0.130
# APPROACH 姿勢族拘束 (2026-07-11 是正・実データ確定).
# 軸命名は fk_lookup 引数と同じ FK 列 index (物理軸: 送信 J2=FK j1, 送信 J5=FK j4).
# FK j1 (肩) = 44.1° → APPROACH_PLACE_J1_REF
# FK j4 (手首) = -76.0° → APPROACH_PLACE_J4_REF
#
# 是正の根拠: 旧 j1=58.35 / j4=-89.38 は approach を J4_SAFE_MIN=-90° の限界に
# 張り付ける姿勢で, ここから ±15° 連続性で place を引くと SAFE 内候補が empty
# → fk_lookup が limit 姿勢 (j4=-89.2) を選び, 補正記憶が j4=-104° を要求して
# safety_rollback される事故が place_x-180_z200 で発生 (実機で observed).
# FK テーブル実測 (target place_x-180_z200):
# - approach 帯 ee_y[110,130)mm に j4 余裕 (-74〜-79) ∩ j1=44 の姿勢が 33 点実在
# - この族 (j1=44±5 ∩ j4=-76±5) から place 帯 [85,115]mm へ ±15° 連続性で繋ぐと
# SAFE 内 65 点・うち j4 余裕 58 点 (現状は SAFE empty)
# tol は A_PICK 同型で PLACE_J1_TOL_DEG / PLACE_J4_TOL_DEG=1.5° 維持.
APPROACH_PLACE_J1_REF = 44.1
APPROACH_PLACE_J4_REF = -76.0
# DAY278 追記 (依頼書: approach_place 族 tol 締め): tol=5.0° は Stage 0 実測
# (adopted approach_place sweep で j1/j4 とも stdev=0・単一姿勢に張り付き) に対して
# 広すぎ, 族 [J4_REF±5°]=[-94.38, -84.38] のうち J4_SAFE_MIN=-90.0 を割った下側
# [-94.38, -90) に fk_lookup が最近傍を当てて j4=-91.00 を採用 → E2 backstop で ABORT.
# dry-run で tol=1.5° なら X− 側 target 7/7 が E2 通過することを確認済み.
# J4_SAFE_MIN/MAX・E2 backstop・APPROACH_PLACE_J1_REF/J4_REF は無改変.
PLACE_J1_TOL_DEG = 1.5
PLACE_J4_TOL_DEG = 1.5
# DESCEND_PLACE j0 lock. A_PICK の PICK_J0_LOCK_TOL_DEG=3.0 に対応する place 版.
# approach_place で確定した j0 に ±3° で descend_place を拘束
# (approach と descend の j0 食い違いを排除・「真上→真下」の place 版).
PLACE_J0_LOCK_TOL_DEG = 3.0
# DAY272 案T (拡張): 観測モード. True で 1 ループのみ実行し即座に抜ける
# (1 回の j0 送角 → 実測 → 送角後 block_x 追加測定 → verdict 出力で停止, 収束判定を待たない).
# 初回実機で「j0 符号が正しい向きに締まるか」「保持ブロックが箱縁に接触しないか」を
# 目視+ログで確認する用. exit_reason="observe_once" で抜ける.
# フラグ分岐追加のみで既存収束ループ本体は無変更.
PLACE_SERVO_OBSERVE_ONCE = True
# C1 (DAY268 案R): 案A (絶対差分ガード・閾値 10°) は実機テスト run_0001_20260704_191203
# で正常な LIFT 移動量 (stage6=18° / stage8=12.5° / stage10=44°) を誤 ABORT した.
# 閾値方式では「正常な大移動」と「未到達直後の急動作」を切り分けられないため撤去し,
# 案R (未到達フラグ方式) に移行. 発火条件は「対応する DESCEND move が arrived=False か」
# のみで, 呼び出し側 (stage6_lift / stage10_lift_back) が log_record["moves"] から
# 直前 DESCEND の arrived を引いて _do_move に abort_if_prev_not_reached を渡す.
# 詳細は _do_move の docstring と stage6/stage10 の呼び出し箇所のコメント参照.
CORR_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/correction_memory.json")
# DAY274 層2: PLACE 高さ補正記憶 (XZ 補正と別記憶・独立管理).
# 白箱に当たったセルで目標 ee_y を上げる (ラチェット学習・上げ速く下げ遅く).
# 初期値は保守的. 実機のラチェットで自己調整される前提 (焼き付けない).
# box_shift bimodal(10mm 非当たり / 216mm 当たり) の谷を HEIGHT_HIT_THRESH に置く.
# HEIGHT_MAX_MM=40 は箱縁+ブロック厚(24mm)+余裕. これ以上は上げない.
PLACE_HEIGHT_MEM_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/place_height_memory.json")
HEIGHT_HIT_THRESH_MM = 50.0 # box_shift_dist > これ で「当たった」= 上げる
HEIGHT_CLEAR_THRESH_MM = 20.0 # box_shift_dist < これ で「明確に非当たり」= 下げる
HEIGHT_STEP_MM = 5.0 # 一段の上げ (速い). 当たり続ければ +5mm 積む
HEIGHT_DECAY_MM = 1.0 # 一段の下げ (遅い. STEP の 1/5)
HEIGHT_MAX_MM = 40.0 # 上限. 箱縁+ブロック厚+余裕. これ以上は上げない
USE_CORRECTION = True # False で補正完全無効 (回帰確認用)
# 手0 (案S・DAY268): PLACE 補正の計測モード. True のときのみ:
# - _apply_correction(phase="place", ...) を素通り (target_before == target_after)
# - PLACE 側 _corr_update を skip (correction_memory.json を汚さない)
# PICK 側の apply/update は一切変更しない (掴めないと PLACE まで届かないため通常運転).
# ee_y +15mm 後の PLACE Z バイアス素値を実測する目的.
PLACE_CORR_MEASURE_MODE = False # DAY274: True→False. PLACE 補正記憶を起動.
# PICK 側 pick_x*_z* と対称の place_x*_z* セル・EMA α=0.5
# で学習開始. 破綻ガード 3 種 + 補正後姿勢の安全チェック
# (下記 update 側 / apply 直後の fk_lookup 事前検査) 併用.
# DAY257 後段: pick 判定の二重チェック閾値。VLM yes でも tip 計測の dist がこれを
# 超えていれば「掴めていない」として no に倒す (over-pickの誤学習を防ぐ)。
# 単位 mm。実機データを見て振る前提の可変定数。
PICK_DIST_MAX_MM = 30.0
# DAY273 案A: tip_dist 測定破綻ガード. この値を超えたら override を発火させず
# VLM 判定を信じる. 爪先と保持ブロックが 150mm 超離れて「掴めている」ことは
# 幾何的にありえず、それはブロック検出位置の破綻 (mechArm 作業半径 270mm・
# ブロックは爪の間にある前提).
# run_0001_20260705_183800 で vlm=yes・confidence=0.82・reason 正確だったのに
# tip_dist=362.8mm で override 発火→ regrasp_open→ 落下した反省.
# 分岐:
# dist <= PICK_DIST_MAX_MM (30) → override せず VLM を信じる (従来)
# 30 < dist <= PICK_DIST_BREAKDOWN_MM (150) → override 発火 no (掴めていない・従来)
# dist > PICK_DIST_BREAKDOWN_MM (150) → override せず VLM を信じる (新規・測定破綻)
PICK_DIST_BREAKDOWN_MM = 150.0
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",
# v3 ヘッダ 34-36（align 系：本実装では出所なし・空欄固定）
"align_j0_final", "align_red_x_mm", "align_converged",
# v3 ヘッダ 37-43（pick で参照した姿勢と到達時の実測角）
"pick_pose_id",
"pick_used_j0", "pick_used_j1", "pick_used_j2",
"pick_used_j3", "pick_used_j4", "pick_used_j5",
# v3 ヘッダ 44-50（place_withblock で参照した姿勢と到達時の実測角）
"place_pose_id",
"place_used_j0", "place_used_j1", "place_used_j2",
"place_used_j3", "place_used_j4", "place_used_j5",
# DAY257 試行間補正の記録 (5 列追加・v3 ヘッダ末尾 51-55)
"corr_cell_key", # 例: "x180_z260"
"corr_applied_dx", # 適用した補正 mm (x)
"corr_applied_dz", # 適用した補正 mm (z)
"target_before", # 補正前 (block) [mm]: "(+196.1,+252.5)" 等
"target_after", # 補正後 (lookup へ渡した値) [mm]
# DAY257 後段: pick 判定の VLM / dist override 内訳 (2 列追加)
"vlm_pick_verdict", # VLM 単独判定 (override 前) "yes/no/unknown/..."
"pick_dist_override", # tip_dist 二重チェックで no に倒したか "True"/"False"/""
# DAY265: 横カメラ (Iriun index=1) の記録列 (5 列追加, 58-62).
# side_ap_place_capture_path は stage7 到達後撮影 (判定なし・画像パスのみ).
# side_place_* は stage8 到達後撮影 + VLM judge (kind="place_side").
"side_ap_place_capture_path", # 58: stage7 横カメラ画像パス (判定なし)
"side_place_capture_path", # 59: stage8 横カメラ画像パス
"side_place_verdict", # 60: 横カメラ PLACE 判定 "yes/no/unknown/"
"side_place_confidence", # 61: 横カメラ PLACE 判定 confidence 0.0-1.0
"side_place_reason", # 62: 横カメラ PLACE 判定 reason 短文
# DAY265: PLACE 側の補正記憶 (5 列追加, 63-67). PICK 側の corr_cell_key /
# corr_applied_dx/dz / target_before/target_after (51-55) と同構造・列名だけ
# phase 区別のため place_ プレフィックス. apply/update は DAY262 で既に実装済.
# ここは _append_ledger が log_record.correction.place_last を読んで書き込むだけ.
"place_corr_cell_key", # 63: 例 "place_x-40_z100" (CELL_MM=20 セル中心)
"place_corr_applied_dx", # 64: 適用した X 補正量 mm (EMA 平均後)
"place_corr_applied_dz", # 65: 適用した Z 補正量 mm
"place_target_before", # 66: 補正前狙い box_xz [mm] "(+X,+Z)"
"place_target_after", # 67: 補正後狙い (lookup へ渡した値) [mm]
# DAY274: 白箱の接触前後ズレ (5 列追加, 68-72). stage2 検出の box_xz と
# stage9b 退避後の place 画像から再検出した箱位置の差分. 箱が動いた =
# 前腕/爪が箱に当たった、を毎試行データで残す. 生値 3 列は今回から埋める.
# 判定 2 列は空欄で用意 (閾値確定後に既存空列を埋める運用). 退避画像は
# 既存の VLM 判定と同じ jpg. 撮影は追加しない (観測のみ).
"box_shift_dx", # 68: (box_xz_last[0] - box_xz[0]) * 1000 [mm]
"box_shift_dz", # 69: (box_xz_last[1] - box_xz[1]) * 1000 [mm]
"box_shift_dist", # 70: sqrt(dx^2 + dz^2) [mm]
"box_hit_flag", # 71: 当面空欄. 閾値判定は後日.
"box_hit_thresh_mm", # 72: 当面空欄. 適用した閾値の記録用.
# DAY274 層2/3: PLACE 高さ補正の適用値 (3 列追加, 73-75). place_x*_z* セル,
# dee_y_mm (ラチェット学習で蓄積), 選ばれた姿勢の j4 (層3 j4 優先が効いたか
# 確認用). 過去行は空欄 (8-A・過去行保持で末尾に空セル付与).
"place_height_cell", # 73: 例 "place_x-200_z180" (PLACE XZ 補正と同格子)
"place_height_dee_y_mm", # 74: 適用した Δee_y (mm)
"place_height_j4_selected", # 75: stage8 で fk が選んだ姿勢の j4 (度)
# DAY274 記録層修正: 失敗理由と画像パスを常時記録 (3 列追加, 76-78).
# エイイチ原則「失敗しても必ず記録を残す・画像が残っていれば後から記憶できる」.
# 空欄を作らず, 全試行でいずれかの語彙で必ず埋まる. 判定ロジック無改変.
# 過去行は 8-A で末尾空セル付与 (0 行作り直しはしない).
"place_corr_reason", # 76: applied/measure_mode/no_memory/pick_not_yes/
# detect_none/box_xz_none/sign_flip/dist_over/
# safety_rollback/not_reached/disabled/
# ssd_unmounted/entry_corrupted/unknown
"box_shift_reason", # 77: ok/place_img_none/white_box_detect_fail/box_xz_none
"place_img_path", # 78: 退避後 place 画像のフルパス (検出失敗の後追い再検出用)
]
# 爪先(青シール)検出パラメータ（detect_blue_markers.py DAY168 のHSV閾値を流用）
# 座標変換は本線の pixel_to_unity に統一（このモジュール独自の homography は使わない）
TIP_BLUE_LOWER = (100, 100, 50)
TIP_BLUE_UPPER = (130, 255, 255)
TIP_MIN_AREA = 80
TIP_MAX_AREA = 8000
# ---- 観測 emit 用の状態（run() が更新、_do_move が読む）-----------------
# 状態は run() スコープ所有（_observe_phase / _observe_gripper ヘルパ経由で更新）。
# 内側関数（stage_lookup_and_move）には phase 更新を持たせない。
# emit は _do_move（移動完了時）に加え、非移動 stage（stage2/5/9/9b・regrasp 再検出）でも
# _observe_emit_now() で通知する。全 stage を履歴表示するため 1 サイト集約は意図的に解除済み。
_OBSERVE_STATE = {
"phase": None,
"phase_t0_ms": None,
"gripper": None,
"verdict": {"pick": None, "place": None, "side_place": None},
"block_xz": None,
"box_xz": None,
"block_place_xz": None,
"tip_pick": None, # PICK到達時の爪先vs block差分（_append_ledger が読む）
"run_dir": None, # この run の出力フォルダ（KIOXIA）。画面のファイル情報表示用
"cap_tip": None, # stage4_pick_tip 画像パス（画面/CSV用）
"cap_pick": None, # pick 判定画像パス
"cap_place": None, # place 判定画像パス
"box_xz_last": None, # DAY274: stage9b 退避後の place 画像から再検出した白箱 [x,z]
# (None = 検出失敗 or 画像なし). 生値 3 列は _append_ledger が読む.
# 非移動 stage の通知専用 emit が再利用する angles キャッシュ.
# 直近 _do_move の post_actual で更新する（新規 daemon I/O ゼロ）.
"last_angles": None,
"run_stem": None,
"lookup": None, # 直近 stage の lookup 参照元 + 判断（_record_lookup が書く）
"corr": None, # 直近 stage の補正記憶の適用/更新
"captures": None, # その stage で撮影した画像リスト（vlm_captures の直近分）
# 要素は {label, camera_role, image}. phase 遷移でクリア.
"task": "pick_and_place_with_j0_table",
}
def _observe_phase(phase_str):
"""run() からフェーズ遷移を記録する。phase_t0 も自動更新.
lookup / corr / captures は「その stage で実際に参照/撮影したもの」だけを表すため、
phase が変わった時点で必ずクリアする（前 stage の値を持ち越さない）。
持ち越すと、参照していない stage が fk_lookup を引いたように見えたり、
撮影していない stage で画像が載ったりする.
PICK/PLACE stage は phase 遷移直後に snap を再アサインして復元する.
"""
_OBSERVE_STATE["phase"] = phase_str
_OBSERVE_STATE["phase_t0_ms"] = int(time.time() * 1000)
_OBSERVE_STATE["lookup"] = None
_OBSERVE_STATE["corr"] = None
_OBSERVE_STATE["captures"] = None
def _observe_gripper(state):
"""run() から gripper 状態を記録する（0=open / 1=close）."""
_OBSERVE_STATE["gripper"] = state
def _observe_capture_push(log_record):
"""直近の vlm_captures エントリを観測状態へ積み、その場で UDP emit +
JSONL 追記（kind="capture"）を発火する.
撮影は「その stage の move emit の後」に来るため、単に state を更新して
次の emit を待つと、次 phase の _observe_phase で captures がクリアされて
JSONL にも UDP にも載らない（旧設計の落とし穴）.
そこで capture の瞬間に自分で 1 行書く. kind="capture" にすることで
_STAGE_RECORD_LAST の (stem, stage, "stage") とキーが分かれ dedup を回避.
複数枚を 1 stage で撮る場合も撮影ごとに 1 行残る.
dry_run では画像パスは None のまま (label と camera_role で試行だけは残る).
全例外を握る・raise しない・制御を絶対止めない."""
try:
caps = log_record.get("vlm_captures") or []
if not caps:
return
last = caps[-1]
rec = {
"label": last.get("label"),
"camera_role": last.get("camera_role") or "top",
"image": (str(last.get("image")) if last.get("image") else None),
}
cur = _OBSERVE_STATE.get("captures")
if not isinstance(cur, list):
cur = []
cur.append(rec)
_OBSERVE_STATE["captures"] = cur
# 撮影ごとに UDP emit + JSONL 追記. 制御を一切止めない fire-and-forget.
_observe_emit_now()
_observe_stage_record(kind="capture")
except Exception:
pass
def _detect_block_in_place_img(image_path):
"""place_img（保存済み jpg）から赤ブロックの Unity 座標 [x, z] を検出する.
stage2 と同じ detect_red_block + pixel_to_unity を使う（同 homography）.
全例外を握り、失敗時 None を返す（制御を絶対止めない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
pixel = detect_red_block(frame)
if pixel is None:
return None
cx, cy = pixel
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except Exception:
return None
def _detect_block_from_frame(frame):
"""DAY272 案T: cv2 frame から赤ブロックの Unity 座標 [x, z] を検出する.
_detect_block_in_place_img の frame 直渡し版. jpg 保存を挟まないので
サーボループから毎反復呼べる. 検出/変換 pipeline は同一 (detect_red_block
+ pixel_to_unity・同 homography). 全例外を握って None (制御を絶対止めない).
"""
if frame is None:
return None
try:
pixel = detect_red_block(frame)
if pixel is None:
return None
cx, cy = pixel
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except Exception:
return None
# DAY274: 白箱テンプレのモジュール-lazy キャッシュ.
# 初回だけ 06f._load_white_box_templates() を呼び、以降は保持したものを使う.
# 失敗 (テンプレ欠落等) は None を返し, 呼び出し側で「検出失敗=空欄」に落ちる.
_WHITE_BOX_M06F = None
_WHITE_BOX_TEMPLATES = None
_WHITE_BOX_LOAD_TRIED = False
def _get_white_box_detector():
"""06f モジュールと templates を lazy に取得する.
stage2 の detect_positions_once と同一 API (06f._load_white_box_templates).
返り値: (m06f, templates) 失敗時は (None, None).
"""
global _WHITE_BOX_M06F, _WHITE_BOX_TEMPLATES, _WHITE_BOX_LOAD_TRIED
if _WHITE_BOX_LOAD_TRIED:
return _WHITE_BOX_M06F, _WHITE_BOX_TEMPLATES
_WHITE_BOX_LOAD_TRIED = True
try:
import importlib
m06f = importlib.import_module("06f_top_white_box_detect_v7")
templates = m06f._load_white_box_templates()
_WHITE_BOX_M06F = m06f
_WHITE_BOX_TEMPLATES = templates
return m06f, templates
except (Exception, SystemExit):
# DAY274 codex ❌ B4: 06f (test_white_box_score.py) はテンプレロード失敗時
# に sys.exit(1) する経路がある. SystemExit は BaseException 直系で
# except Exception では捕まらず, PLACE 本線が停止する. 観測処理の失敗は
# 3 列空欄で完走・HOME 復帰・台帳追記まで進める設計意図に合わせ,
# SystemExit も握って None に倒す. KeyboardInterrupt は BaseException 直系で
# (Exception, SystemExit) に含まれないため Ctrl+C は生きる (意図).
# except BaseException にはしない (Ctrl+C を握らないため).
_WHITE_BOX_M06F = None
_WHITE_BOX_TEMPLATES = None
return None, None
def _detect_white_box_in_place_img(image_path):
"""DAY274: place 画像 (stage9b 退避後の保存済み jpg) から白箱の Unity 座標
[x, z] を検出する.
- stage2 と同一検出器 (06f evaluate_frame_gray) + 同一 homography
(visual_servo_pick.pixel_to_unity). 新規検出ロジックは書かない.
- detect_white_box_once は VideoCapture 引数を取るため, 静止画向けに内部
関数 evaluate_frame_gray を直接呼ぶ (これが本体検出器). しきい値は
m06f.TEMPLATE_SCORE_THRESHOLD (stage2 と同値).
- _mat_px_range は stage2 の setup_homography_and_mat_range で既に初期化済
(未初期化なら None を返す・制御は止めない).
- 全例外を握り、失敗時 None (box_shift 3 列は空欄で埋まる).
"""
if image_path is None:
return None
try:
import visual_servo_pick as vsp
if vsp._mat_px_range is None:
return None
m06f, templates = _get_white_box_detector()
if m06f is None or templates is None:
return None
frame = cv2.imread(str(image_path))
if frame is None:
return None
results = m06f.evaluate_frame_gray(frame, vsp._mat_px_range, templates)
best = results.get("overall_best") if isinstance(results, dict) else None
if best is None:
return None
score = best.get("best_score")
if score is None or score < m06f.TEMPLATE_SCORE_THRESHOLD:
return None
cx = best.get("cx"); cy = best.get("cy")
if cx is None or cy is None:
return None
ux, uz = pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except (Exception, SystemExit):
# DAY274 codex ❌ B4: 06f 内部 (evaluate_frame_gray など) が sys.exit を
# 呼ぶ経路があっても本線を止めない. SystemExit を握って None に倒す.
# KeyboardInterrupt (BaseException 直系) は含まれず、Ctrl+C は生きる (意図).
return None
def _detect_blue_tips_px(frame):
"""フレームから青シールを検出し、面積上位3点のpixel重心 [(cx,cy),...] を返す.
変換はしない（pixel座標のみ）。3点未満なら None。全例外を握って None.
"""
try:
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower = np.array(TIP_BLUE_LOWER)
upper = np.array(TIP_BLUE_UPPER)
mask = cv2.inRange(hsv, lower, upper)
k = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
cnts, _ = cv2.findContours(
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
pts = []
for c in cnts:
a = cv2.contourArea(c)
if a < TIP_MIN_AREA or a > TIP_MAX_AREA:
continue
m = cv2.moments(c)
if m["m00"] == 0:
continue
cx = m["m10"] / m["m00"]
cy = m["m01"] / m["m00"]
pts.append((cx, cy, a))
if len(pts) < 3:
return None
pts.sort(key=lambda t: t[2], reverse=True) # 面積降順
top3 = [(p[0], p[1]) for p in pts[:3]] # 上位3点（ノイズ除去）
return top3
except Exception:
return None
def _measure_tip_vs_block(image_path):
"""pick到達フレームから 爪先中点 と 赤block を同一frame・同一 pixel_to_unity
で検出し差分を返す. 失敗時 None（制御を絶対止めない）.
戻り値 dict: tip_x, tip_z, block_x, block_z（Unity m）, dx_mm, dz_mm, dist_mm
姿勢非依存の爪先判定: 上位3点のうち「互いに最も近い2点＝爪先」、
残り1点＝手首. これは横/上カメラやアーム向きに依存しない（DAY168 の
「Y下方2点」は姿勢依存なので採用しない）.
ガード: 爪先間距離 < 手首までの距離 が崩れたら（開きすぎ等で前提破綻）
差分を None 記録してそのまま grip へ進む（測定が動作に干渉しない）.
"""
if image_path is None:
return None
try:
frame = cv2.imread(str(image_path))
if frame is None:
return None
# --- 爪先（青シール上位3点 → 最近2点の中点）---
top3 = _detect_blue_tips_px(frame)
if top3 is None:
return None
import itertools
pairs = list(itertools.combinations(range(3), 2))
def _d(p, q):
return math.hypot(p[0] - q[0], p[1] - q[1])
# 最近ペア＝爪先
i, j = min(pairs, key=lambda ij: _d(top3[ij[0]], top3[ij[1]]))
tip1, tip2 = top3[i], top3[j]
wrist = top3[3 - i - j] # 0+1+2=3 → 残り1点
mid_px = ((tip1[0] + tip2[0]) / 2.0, (tip1[1] + tip2[1]) / 2.0)
tip_pair_d = _d(tip1, tip2)
wrist_d = _d(mid_px, wrist)
if not (tip_pair_d < wrist_d): # 前提破綻ガード
return None
# --- block（同フレーム・同 pixel_to_unity で再検出）---
red_px = detect_red_block(frame)
if red_px is None:
return None
tip_ux, tip_uz = pixel_to_unity(mid_px[0], mid_px[1])
blk_ux, blk_uz = pixel_to_unity(red_px[0], red_px[1])
dx_mm = (tip_ux - blk_ux) * 1000.0
dz_mm = (tip_uz - blk_uz) * 1000.0
dist_mm = math.hypot(dx_mm, dz_mm)
return {
"tip_x": float(tip_ux), "tip_z": float(tip_uz),
"block_x": float(blk_ux), "block_z": float(blk_uz),
"dx_mm": float(dx_mm), "dz_mm": float(dz_mm),
"dist_mm": float(dist_mm),
}
except Exception:
return None
def _observe_verdict(kind, raw_verdict, confidence, reason):
"""run() から VLM 判定結果を kind 別スロットに記録する。
kind ∈ {"pick", "place", "side_place"}。台帳の
pick_* / place_* / side_place_* 列に 1:1 対応させ、上書き衝突を防ぐ。
raw_verdict ∈ {'yes','no','unknown',...} を success ∈ {True,False,None} にマップ.
reward は判定器側で未算出のため None 固定（VLM-as-Reward 実装時に充填）.
"""
if raw_verdict == "yes":
success = True
elif raw_verdict == "no":
success = False
else:
success = None
if not isinstance(_OBSERVE_STATE.get("verdict"), dict):
_OBSERVE_STATE["verdict"] = {"pick": None, "place": None, "side_place": None}
if kind not in ("pick", "place", "side_place"):
return # 未知 kind は黙って捨てる（制御を止めない）
_OBSERVE_STATE["verdict"][kind] = {
"success": success,
"reward": None,
"reason": reason,
"confidence": confidence,
"raw_verdict": raw_verdict,
}
def _observe_emit_now():
"""非移動 stage 用の通知専用 emit. シリアル/daemon 読み取りを伴わない.
angles は _do_move でキャッシュ済みの last_angles をそのまま再利用（新規 I/O ゼロ）.
fire-and-forget. send_angles / グリッパー / sleep / 制御フローには一切触れない.
"""
# in_box は台帳と同じ _ledger_in_box_geometry を再利用（判定の二重実装を避ける）.
# 戻り値は ("True"/"False"/"", dist_str). bool に正規化し、欠落時は None.
# 同じ IN_BOX_TOL_MM・同じ関数を画面/台帳が共有するため値の食い違いが起きない.
_ibg, _ = _ledger_in_box_geometry(
_OBSERVE_STATE["block_place_xz"], _OBSERVE_STATE["box_xz"])
_in_box = (True if _ibg == "True" else False if _ibg == "False" else None)
_observe_emit(
angles=_OBSERVE_STATE["last_angles"],
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
in_box=_in_box,
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
lookup=_OBSERVE_STATE["lookup"],
corr=_OBSERVE_STATE["corr"],
captures=_OBSERVE_STATE["captures"],
)
_TELEMETRY_JSONL_NAME = "telemetry.jsonl"
# 「1 STAGE = 1 行」の DUP=0 原則を run 単位のセットで守る.
# 旧実装 (_STAGE_RECORD_LAST) は直前 1 件しか比較しないため、
# stage → capture → stage の順で 3 件目の stage が「直前と違う」と判定されて
# DUP を通してしまった (Phase 3-A で発覚). run が変わったら keys を空に戻す.
_STAGE_RECORD_SEEN = {"run_stem": None, "keys": set()}
def _observe_stage_record(kind="stage"):
"""STAGE 1 行を run_dir/telemetry.jsonl に追記する（記録の正）。
UDP は最新主義で取りこぼすため、履歴はこのファイルを正とする。
全例外を握る・raise しない・制御を絶対止めない（emit と同じ性質）。
追記のみ。既存行の上書き・削除をしない。
同一 run の同一 (stage, kind) は 1 度だけ (Phase 3-A で DUP 復活したので
直前 1 件比較ではなく run 単位のセットで抑える).
ただし kind == "capture" は同一 phase で複数撮影があり得るため、
key に直近 capture の label を含めてラベル毎に 1 行残す
(stage8_5 で place_side と place_pre が両方撮られるケース).
"""
try:
run_dir = _OBSERVE_STATE.get("run_dir")
if not run_dir:
return
stem = _OBSERVE_STATE.get("run_stem")
stage = _OBSERVE_STATE.get("phase")
# run が変わったらセットをリセット
if _STAGE_RECORD_SEEN["run_stem"] != stem:
_STAGE_RECORD_SEEN["run_stem"] = stem
_STAGE_RECORD_SEEN["keys"] = set()
if kind == "capture":
caps = _OBSERVE_STATE.get("captures") or []
lbl = caps[-1].get("label") if caps else None
key = (stage, kind, lbl)
else:
key = (stage, kind)
if key in _STAGE_RECORD_SEEN["keys"]:
return # 同一 run で同一 key は 1 度だけ
_STAGE_RECORD_SEEN["keys"].add(key)
rec = {
"t": int(time.time() * 1000),
"kind": kind, # "stage" | "abort"
"stage": _OBSERVE_STATE.get("phase"),
"phase_t0": _OBSERVE_STATE.get("phase_t0_ms"),
"trial_id": _OBSERVE_STATE.get("run_stem"),
"angles": _OBSERVE_STATE.get("last_angles"),
"gripper": _OBSERVE_STATE.get("gripper"),
"lookup": _OBSERVE_STATE.get("lookup"),
"corr": _OBSERVE_STATE.get("corr"),
"captures": _OBSERVE_STATE.get("captures"),
"block_xz": _OBSERVE_STATE.get("block_xz"),
"box_xz": _OBSERVE_STATE.get("box_xz"),
"block_place_xz": _OBSERVE_STATE.get("block_place_xz"),
"tip_pick": _OBSERVE_STATE.get("tip_pick"),
"verdict": _OBSERVE_STATE.get("verdict"),
}
p = Path(run_dir) / _TELEMETRY_JSONL_NAME
with open(str(p), "a", encoding="utf-8") as f:
f.write(json.dumps(rec, ensure_ascii=False) + "\n")
except Exception:
pass
# ---- ヘルパ ------------------------------------------------------------------
def _next_log_path():
LOG_ROOT.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
for n in range(1, 10000):
stem = "run_{:04d}_{}".format(n, ts)
run_dir = LOG_ROOT / stem
if not run_dir.exists():
run_dir.mkdir(parents=True, exist_ok=True)
return run_dir / (stem + ".json")
raise RuntimeError("log seq 9999 超過")
def _atomic_write_json(path, data):
tmp = Path(str(path) + ".tmp")
with open(tmp, "w") as f:
json.dump(data, f, ensure_ascii=False, indent=2, default=str)
tmp.replace(path)
def _ensure_ssd_mounted():
"""KIOXIA SSD の LEDGER_DIR が存在・書込可なら True、それ以外 False.
fire-and-forget: 例外を握り False を返す."""
try:
if not LEDGER_DIR.exists():
return False
if not LEDGER_DIR.is_dir():
return False
# 書込テスト（write_test ファイルを作って即削除）
test = LEDGER_DIR / ".write_test"
try:
test.write_text("")
test.unlink()
return True
except Exception:
return False
except Exception:
return False
# ---- 試行間補正記憶 (DAY257) ---------------------------------------------
def _corr_load():
"""補正記憶 JSON をロード。失敗時は {} を返す（制御を絶対止めない）。"""
if not CORR_MEM_PATH.exists():
return {}
try:
with open(CORR_MEM_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[CORR ] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _corr_save(mem):
"""補正記憶を atomic に保存。KIOXIA 未マウント時は False を返してスキップ。"""
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正記憶の保存をスキップ")
return False
try:
CORR_MEM_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = CORR_MEM_PATH.with_suffix(CORR_MEM_PATH.suffix + ".tmp")
with open(tmp, "w", encoding="utf-8") as f:
json.dump(mem, f, ensure_ascii=False, indent=2)
os.replace(str(tmp), str(CORR_MEM_PATH))
return True
except Exception as e:
print("[CORR ] [WARN] 保存失敗: {}".format(e))
return False
def _corr_key(phase, x_m, z_m):
"""(phase, x, z)[m] → セルキー文字列 (例: 'pick_x180_z260' / 'place_x-40_z100')。
DAY262: phase プレフィックスを付けて pick/place の名前空間を分離
(同一 (x,z) セルでも掴み残差と置き残差は別物なので学習を汚染しない).
CELL_MM=20 で round(中心) して mm 化."""
cx = int(round(float(x_m) * 1000.0 / CELL_MM) * CELL_MM)
cz = int(round(float(z_m) * 1000.0 / CELL_MM) * CELL_MM)
return "{}_x{}_z{}".format(phase, cx, cz)
def _corr_get(mem, key):
"""セルキー → {'dx_mm','dz_mm','n'} or None"""
return mem.get(key)
def _corr_update(mem, key, dx_mm_new, dz_mm_new):
"""1 セル 1 値の補正 Δ を残差から更新する.
DAY277 修正 (A / codex 再レビュー): 更新則を "残差 EMA" から "積分則" へ.
旧 (残差 EMA): Δ_new = α·r + (1-α)·Δ_old
→ 不動点は Δ=b/2 / 残差=b/2 (α=0.5 で Δ_old 項が相殺し即座に半分値).
理想ゲイン 1 でも残差は 0 にならず bias の半分に張り付く
(codex DAY275 A1 独立検算・台帳 pick_x200_z260 で dz~-6.66mm 床).
新 (積分則): Δ_new = Δ_old + α·r
→ 不動点は残差=0 のみ (aim=block-Δ の下, r=b-Δ から Δ=b で r=0).
α は "1 試行で残差の何割を積むか (ループゲイン)" に意味が変わる.
値は暫定で EMA_ALPHA=0.5 を流用 (最終確定は B 伝達比計測後).
初手 (新規セル): 現行は Δ=r (残差そのもの・実質 α=1 の初手オーバーシュート).
積分則の一貫性のため Δ=α·r に変える (0 から α 分だけ積む).
クランプ (振動安全弁): 1 更新あたり MAX_CORRECTION_STEP_MM=15 は積分則でも
維持. 絶対値 MAX_CORRECTION_MM=40 は _apply_correction 側で別途効く.
真バイアスが 40mm 以内なら収束を阻害しない (codex A3).
破損ガード (dict 以外を捨てて新規化) は無改変.
"""
e = mem.get(key)
# 破損ガード: 既存エントリが dict 以外 (str/list/null 等) なら捨てて新規扱いに
if e is not None and not isinstance(e, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
a = EMA_ALPHA
if e is None:
# DAY277: 初手も積分則の一貫性で Δ = α·r (旧 Δ=r 全部積む挙動を廃す).
# 1 更新あたりの変化量クランプ (±s) を 0 起点で適用.
s = MAX_CORRECTION_STEP_MM
new_dx = max(-s, min(+s, a * float(dx_mm_new)))
new_dz = max(-s, min(+s, a * float(dz_mm_new)))
mem[key] = {"dx_mm": new_dx, "dz_mm": new_dz, "n": 1}
else:
old_dx = float(e.get("dx_mm", 0.0))
old_dz = float(e.get("dz_mm", 0.0))
# DAY277: 積分則. Δ_new = Δ_old + α·r.
new_dx = old_dx + a * float(dx_mm_new)
new_dz = old_dz + a * float(dz_mm_new)
# DAY268: 1 更新あたりの変化量クランプ (跳ね幅制限).
# 積分則でも維持 (振動安全弁). 60→241→24 のような 1 試行での過補正
# オーバーシュートを抑える. 絶対値天井 MAX_CORRECTION_MM=40 は
# _apply_correction 側で別途効く (真バイアスが 40mm 以内なら収束を
# 阻害しない・codex A3).
s = MAX_CORRECTION_STEP_MM
new_dx = max(old_dx - s, min(old_dx + s, new_dx))
new_dz = max(old_dz - s, min(old_dz + s, new_dz))
mem[key] = {
"dx_mm": new_dx,
"dz_mm": new_dz,
"n": int(e.get("n", 0)) + 1,
}
return mem[key]
# ---- DAY274 層2: PLACE 高さ補正記憶 (ラチェット学習・XZ 補正と独立) --------
def _height_load():
"""place_height_memory.json をロード. 失敗時は {} (制御を絶対止めない)."""
if not PLACE_HEIGHT_MEM_PATH.exists():
return {}
try:
with open(PLACE_HEIGHT_MEM_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[HGT ] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _height_save(mem):
"""高さ補正記憶を atomic に保存. KIOXIA 未マウント時は False でスキップ."""
if not _ensure_ssd_mounted():
print("[HGT ] KIOXIA 未マウント → 高さ補正記憶の保存をスキップ")
return False
try:
PLACE_HEIGHT_MEM_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = PLACE_HEIGHT_MEM_PATH.with_suffix(
PLACE_HEIGHT_MEM_PATH.suffix + ".tmp")
with open(tmp, "w", encoding="utf-8") as f:
json.dump(mem, f, ensure_ascii=False, indent=2)
os.replace(str(tmp), str(PLACE_HEIGHT_MEM_PATH))
return True
except Exception as e:
print("[HGT ] [WARN] 保存失敗: {}".format(e))
return False
def _height_key(x_m, z_m):
"""(x, z)[m] → セルキー 'place_x{X}_z{Z}' (PLACE の XZ 補正と同一格子・20mm)."""
return _corr_key("place", x_m, z_m)
def _height_get_dee_y_mm(mem, key):
"""セルキー → dee_y_mm (0.0 なら None・未登録). 破損時は 0.0."""
e = mem.get(key)
if e is None or not isinstance(e, dict):
return 0.0
try:
return float(e.get("dee_y_mm", 0.0))
except (TypeError, ValueError):
return 0.0
def _height_update_ratchet(mem, key, box_shift_dist_mm):
"""ラチェット則で 1 セルの dee_y_mm を更新して mem に書き戻す.
- box_shift_dist_mm > HEIGHT_HIT_THRESH_MM → dee_y += HEIGHT_STEP_MM (速い)
- box_shift_dist_mm < HEIGHT_CLEAR_THRESH_MM → dee_y -= HEIGHT_DECAY_MM (遅い)
- 中間帯 (CLEAR ≤ dist ≤ HIT) は現状維持 (ヒステリシス)
- dee_y は [0.0, HEIGHT_MAX_MM] にクランプ.
box_shift_dist_mm が None (検出失敗) の場合は上位で呼ばず, 呼ばれても現状維持.
戻り値: 更新後エントリ dict {'dee_y_mm', 'n'}.
"""
if box_shift_dist_mm is None:
# 上位が守るはず (呼ばない) だが二重に安全側.
e = mem.get(key)
if isinstance(e, dict):
return e
mem[key] = {"dee_y_mm": 0.0, "n": 0}
return mem[key]
e = mem.get(key)
if e is not None and not isinstance(e, dict):
print("[HGT ] [WARN] mem[{}] が dict 以外 ({}). 破棄して再初期化"
.format(key, type(e).__name__))
e = None
if e is None:
old_dee_y = 0.0
n = 0
else:
try:
old_dee_y = float(e.get("dee_y_mm", 0.0))
except (TypeError, ValueError):
old_dee_y = 0.0
try:
n = int(e.get("n", 0))
except (TypeError, ValueError):
n = 0
d = float(box_shift_dist_mm)
if d > HEIGHT_HIT_THRESH_MM:
new_dee_y = old_dee_y + HEIGHT_STEP_MM
elif d < HEIGHT_CLEAR_THRESH_MM:
new_dee_y = old_dee_y - HEIGHT_DECAY_MM
else:
new_dee_y = old_dee_y
new_dee_y = max(0.0, min(HEIGHT_MAX_MM, new_dee_y))
mem[key] = {"dee_y_mm": new_dee_y, "n": n + 1,
"last_box_shift_dist_mm": d}
return mem[key]
def _apply_correction(phase, block_xz):
"""(phase, block_xz) から補正を適用し (target_xz, meta) を返す。
DAY262: phase 引数を追加 (pick/place の名前空間分離). PICK は phase="pick",
PLACE (stage8 place_withblock) は phase="place" で呼ぶ.
USE_CORRECTION=False / KIOXIA 未マウント / 記憶なし → 素通り (meta は空欄).
MAX_CORRECTION_MM で絶対値クランプ. 狙い = block - Δ.
Δ の符号: PICK は (爪先 - ブロック), PLACE は (落下点 - 箱). どちらも「実測 - 目標」.
DAY274 記録層修正: 判定ロジックは無改変. cell_key を全経路で必ず入れる (原則1).
reason を meta に埋め込む (原則2): applied / measure_mode / disabled /
ssd_unmounted / no_memory / entry_corrupted."""
bx_m = float(block_xz[0]); bz_m = float(block_xz[1])
# DAY274 原則1: cell_key は補正適用の成否と無関係に常に算出.
# 早期 return する経路 (USE_CORRECTION=False / measure_mode / SSD 未マウント) でも
# 「どのセルの試行か」を残す. 判定ロジックは無改変・記録だけ変える.
key = _corr_key(phase, bx_m, bz_m)
meta = {
"phase": phase,
"target_before_mm": (bx_m * 1000.0, bz_m * 1000.0),
"target_after_mm": (bx_m * 1000.0, bz_m * 1000.0),
"cell_key": key,
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
"reason": "", # DAY274: 経路ごとに埋める.
}
if not USE_CORRECTION:
meta["reason"] = "disabled"
return (bx_m, bz_m), meta
# 手0 (案S・DAY268): PLACE 計測モードのとき phase="place" の apply を skip.
# PICK は無影響. target_before == target_after のまま返る.
if phase == "place" and PLACE_CORR_MEASURE_MODE:
print("[CORR ] PLACE measure mode: apply/update skipped")
meta["reason"] = "measure_mode"
return (bx_m, bz_m), meta
if not _ensure_ssd_mounted():
print("[CORR ] KIOXIA 未マウント → 補正スキップ (素通り)")
meta["reason"] = "ssd_unmounted"
return (bx_m, bz_m), meta
mem = _corr_load()
entry = _corr_get(mem, key)
if entry is None:
# 記憶未生成 (このセル初回). cell_key は残す (原則1).
meta["reason"] = "no_memory"
return (bx_m, bz_m), meta
# 破損ガード: エントリが dict 以外 (str/list 等) は補正せず素通り。
# 次回の _corr_update が新規扱いで上書きするので、自動で正常状態に戻る。
if not isinstance(entry, dict):
print("[CORR ] [WARN] mem[{}] が dict 以外 ({}). 補正せず素通り"
.format(key, type(entry).__name__))
meta["reason"] = "entry_corrupted"
return (bx_m, bz_m), meta
dx_mm = float(entry.get("dx_mm", 0.0))
dz_mm = float(entry.get("dz_mm", 0.0))
n = int(entry.get("n", 0))
# MAX_CORRECTION_MM クランプ (記憶破損ガード)
dx_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dx_mm))
dz_mm = max(-MAX_CORRECTION_MM, min(MAX_CORRECTION_MM, dz_mm))
# 狙い = block - Δ (Δ は 爪先 - ブロック の符号)
corrected_x_m = bx_m - dx_mm / 1000.0
corrected_z_m = bz_m - dz_mm / 1000.0
meta["target_after_mm"] = (corrected_x_m * 1000.0, corrected_z_m * 1000.0)
meta["dx_mm"] = dx_mm
meta["dz_mm"] = dz_mm
meta["n"] = n
meta["applied"] = True
meta["reason"] = "applied"
print("[CORR ] key={} dx={:+.2f}mm dz={:+.2f}mm n={} "
"target ({:+.1f},{:+.1f}) -> ({:+.1f},{:+.1f}) [mm]".format(
key, dx_mm, dz_mm, n,
bx_m * 1000.0, bz_m * 1000.0,
corrected_x_m * 1000.0, corrected_z_m * 1000.0))
return (corrected_x_m, corrected_z_m), meta
def _ledger_extract_trial_no(run_stem):
"""run_stem ("run_0001_YYYYMMDD_HHMMSS" 想定) の先頭数字を int で返す。
取得失敗時は空文字列."""
try:
for part in str(run_stem).split("_"):
if part.isdigit():
return int(part)
except Exception:
pass
return ""
def _ledger_mm_or_blank(arr, idx):
"""[x, z] m リストから idx 要素を mm 化して文字列に。取れなければ空."""
if not isinstance(arr, (list, tuple)) or len(arr) <= idx:
return ""
v = arr[idx]
if not isinstance(v, (int, float)):
return ""
return "{:.1f}".format(v * 1000.0)
def _ledger_in_box_geometry(block_place_xz, box_xz):
"""block_place と box 中心の距離 mm + IN_BOX_TOL_MM 比較.
戻り値: (in_box_geom_str, dist_mm_str). どちらか欠落で ('','') を返す."""
if not (isinstance(block_place_xz, (list, tuple)) and len(block_place_xz) == 2
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2):
return "", ""
vals = list(block_place_xz) + list(box_xz)
if not all(isinstance(v, (int, float)) for v in vals):
return "", ""
dx_mm = (block_place_xz[0] - box_xz[0]) * 1000.0
dz_mm = (block_place_xz[1] - box_xz[1]) * 1000.0
dist = math.hypot(dx_mm, dz_mm)
return str(dist < IN_BOX_TOL_MM), "{:.1f}".format(dist)
def _append_ledger(log_record, run_stem):
"""試行 1 行を KIOXIA SSD の CSV 台帳へ追記.
fire-and-forget: 例外で制御を絶対止めない. ローカルへのフォールバック書き込みはしない.
既存ヘッダ無ければ新規作成、あれば append.
"""
try:
if not _ensure_ssd_mounted():
print("[LEDGER] KIOXIA SSD ({}) 未マウント or 書込不可 → CSV スキップ"
.format(LEDGER_DIR))
return
judgments = log_record.get("judgments") or {}
pick = judgments.get("pick") or {}
place = judgments.get("place") or {}
# block/box は _OBSERVE_STATE の最終値 (regrasp 反映済み・stage9b 検出反映済み)
block_xz = _OBSERVE_STATE.get("block_xz")
box_xz = _OBSERVE_STATE.get("box_xz")
block_place_xz = _OBSERVE_STATE.get("block_place_xz")
gripper = _OBSERVE_STATE.get("gripper")
tip_pick = _OBSERVE_STATE.get("tip_pick")
# 最後の move の post_actual = 試行終了時の関節角
final_angles = ["", "", "", "", "", ""]
for m in reversed(log_record.get("moves", []) or []):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
for i in range(6):
v = pa[i]
final_angles[i] = ("{:.2f}".format(v)
if isinstance(v, (int, float)) else "")
break
# place 撮影画像のフルパス
place_capture_path = ""
for c in reversed(log_record.get("vlm_captures", []) or []):
if c.get("label") == "place" and c.get("image"):
place_capture_path = str(c["image"])
break
in_box_geom, in_box_dist_mm = _ledger_in_box_geometry(
block_place_xz, box_xz)
row = {
"timestamp": log_record.get("ended_at") or datetime.now().isoformat(),
"run_stem": run_stem,
"trial_no": _ledger_extract_trial_no(run_stem),
"pick_verdict": (pick.get("verdict") or ""),
"pick_reason": (pick.get("reason") or ""),
"pick_confidence": (pick.get("confidence")
if isinstance(pick.get("confidence"), (int, float))
else ""),
"place_verdict": (place.get("verdict") or ""),
"place_reason": (place.get("reason") or ""),
"place_confidence": (place.get("confidence")
if isinstance(place.get("confidence"), (int, float))
else ""),
"block_pick_x": _ledger_mm_or_blank(block_xz, 0),
"block_pick_z": _ledger_mm_or_blank(block_xz, 1),
"block_place_x": _ledger_mm_or_blank(block_place_xz, 0),
"block_place_z": _ledger_mm_or_blank(block_place_xz, 1),
"box_x": _ledger_mm_or_blank(box_xz, 0),
"box_z": _ledger_mm_or_blank(box_xz, 1),
"in_box_geom": in_box_geom,
"in_box_dist_mm": in_box_dist_mm,
"final_j1": final_angles[0],
"final_j2": final_angles[1],
"final_j3": final_angles[2],
"final_j4": final_angles[3],
"final_j5": final_angles[4],
"final_j6": final_angles[5],
"gripper": ("" if gripper is None else str(gripper)),
"place_capture_path": place_capture_path,
"tip_pick_x": ("{:.1f}".format(tip_pick["tip_x"] * 1000.0)
if tip_pick else ""),
"tip_pick_z": ("{:.1f}".format(tip_pick["tip_z"] * 1000.0)
if tip_pick else ""),
"tip_block_dx_mm": ("{:.1f}".format(tip_pick["dx_mm"])
if tip_pick else ""),
"tip_block_dz_mm": ("{:.1f}".format(tip_pick["dz_mm"])
if tip_pick else ""),
"tip_block_dist_mm": ("{:.1f}".format(tip_pick["dist_mm"])
if tip_pick else ""),
"run_dir": (_OBSERVE_STATE.get("run_dir") or ""),
"tip_capture_path": (_OBSERVE_STATE.get("cap_tip") or ""),
"pick_capture_path": (_OBSERVE_STATE.get("cap_pick") or ""),
}
# v3 ヘッダ 34-50: align 系 3 列と pick/place の pose_id・
# 到達時実測角 14 列.
# DAY277 (再修正): PICK 視覚サーボを本線から切り離し (block 未検出時に
# 収束偽装で j0 を悪化させる問題あり). align 3 列は従来通り空欄固定に戻す.
# v3 ヘッダの列名・順は無改変 (追記ガードに触らない).
pose_used = log_record.get("pose_used") or {}
pick_used = pose_used.get("pick_used") or [None] * 6
place_used = pose_used.get("place_used") or [None] * 6
def _fmt_j(v):
return ("{:.2f}".format(v) if isinstance(v, (int, float)) else "")
row.update({
"align_j0_final": "",
"align_red_x_mm": "",
"align_converged": "",
"pick_pose_id": (pose_used.get("pick_pose_id") or ""),
"pick_used_j0": _fmt_j(pick_used[0]),
"pick_used_j1": _fmt_j(pick_used[1]),
"pick_used_j2": _fmt_j(pick_used[2]),
"pick_used_j3": _fmt_j(pick_used[3]),
"pick_used_j4": _fmt_j(pick_used[4]),
"pick_used_j5": _fmt_j(pick_used[5]),
"place_pose_id": (pose_used.get("place_pose_id") or ""),
"place_used_j0": _fmt_j(place_used[0]),
"place_used_j1": _fmt_j(place_used[1]),
"place_used_j2": _fmt_j(place_used[2]),
"place_used_j3": _fmt_j(place_used[3]),
"place_used_j4": _fmt_j(place_used[4]),
"place_used_j5": _fmt_j(place_used[5]),
})
# DAY257 補正の記録 (5 列・PICK 側).
corr = (log_record.get("correction") or {}).get("last") or {}
def _fmt_xz(t):
if not (isinstance(t, (list, tuple)) and len(t) == 2):
return ""
return "({:+.1f},{:+.1f})".format(float(t[0]), float(t[1]))
row.update({
"corr_cell_key": corr.get("cell_key", ""),
"corr_applied_dx": ("{:.2f}".format(corr["dx_mm"])
if isinstance(corr.get("dx_mm"), (int, float))
else ""),
"corr_applied_dz": ("{:.2f}".format(corr["dz_mm"])
if isinstance(corr.get("dz_mm"), (int, float))
else ""),
"target_before": _fmt_xz(corr.get("target_before_mm")),
"target_after": _fmt_xz(corr.get("target_after_mm")),
})
# DAY265: PLACE 側の補正記録 (5 列・PICK と同構造). apply/update は
# DAY262 で既に実装済 (log_record.correction.place_last に格納される).
# ここでは stage8 で適用した meta を CSV に反映するだけ.
place_corr = (log_record.get("correction") or {}).get("place_last") or {}
row.update({
"place_corr_cell_key": place_corr.get("cell_key", ""),
"place_corr_applied_dx": ("{:.2f}".format(place_corr["dx_mm"])
if isinstance(place_corr.get("dx_mm"), (int, float))
else ""),
"place_corr_applied_dz": ("{:.2f}".format(place_corr["dz_mm"])
if isinstance(place_corr.get("dz_mm"), (int, float))
else ""),
"place_target_before": _fmt_xz(place_corr.get("target_before_mm")),
"place_target_after": _fmt_xz(place_corr.get("target_after_mm")),
})
# DAY274: 白箱の接触前後ズレ (5 列・生値 3 + 判定 2 空欄).
# 最初 (接触前) = stage2 検出 box_xz (既存. 誰も触っていない).
# 最後 (接触後) = stage9b 退避後の place 画像から再検出 (box_xz_last).
# どちらかが None なら 3 列とも空欄 (箱位置比較不能・クラッシュしない).
# 判定 (box_hit_flag / box_hit_thresh_mm) は当面空欄で保持し, 閾値確定後に埋める.
box_xz_last = _OBSERVE_STATE.get("box_xz_last")
_box_shift_dx_str = ""
_box_shift_dz_str = ""
_box_shift_dist_str = ""
if (isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and isinstance(box_xz[0], (int, float))
and isinstance(box_xz[1], (int, float))
and isinstance(box_xz_last, (list, tuple)) and len(box_xz_last) == 2
and isinstance(box_xz_last[0], (int, float))
and isinstance(box_xz_last[1], (int, float))):
_dx_mm = (float(box_xz_last[0]) - float(box_xz[0])) * 1000.0
_dz_mm = (float(box_xz_last[1]) - float(box_xz[1])) * 1000.0
_dist_mm = math.sqrt(_dx_mm * _dx_mm + _dz_mm * _dz_mm)
_box_shift_dx_str = "{:.2f}".format(_dx_mm)
_box_shift_dz_str = "{:.2f}".format(_dz_mm)
_box_shift_dist_str = "{:.2f}".format(_dist_mm)
row.update({
"box_shift_dx": _box_shift_dx_str,
"box_shift_dz": _box_shift_dz_str,
"box_shift_dist": _box_shift_dist_str,
"box_hit_flag": "", # 閾値判定は後日 (空で保持)
"box_hit_thresh_mm": "", # 適用した閾値の記録用 (空で保持)
})
# DAY274 層2/3: PLACE 高さ補正の適用値 (3 列). 未適用の試行は空欄.
# log_record.correction.place_height_last に stage8 で格納された meta を読む.
place_hgt = (log_record.get("correction") or {}).get("place_height_last") or {}
_hgt_cell = place_hgt.get("cell_key", "") or ""
_hgt_dee_y = place_hgt.get("dee_y_mm")
_hgt_j4 = place_hgt.get("j4_selected_deg")
row.update({
"place_height_cell": _hgt_cell,
"place_height_dee_y_mm": ("{:.2f}".format(_hgt_dee_y)
if isinstance(_hgt_dee_y, (int, float))
else ""),
"place_height_j4_selected": ("{:.2f}".format(_hgt_j4)
if isinstance(_hgt_j4, (int, float))
else ""),
})
# DAY274 原則2: 記録層の欠損理由 3 列 (place_corr_reason / box_shift_reason /
# place_img_path). 判定ロジックは無改変. どの試行でも必ずどれかの語彙で埋まる.
# ---- place_corr_reason (PLACE 補正の apply/update 理由) ----
# 優先度: (1) place_last meta 未設定 = PLACE 段8まで到達せず (not_reached)
# (2) apply 側 reason (measure_mode/disabled/ssd_unmounted/entry_corrupted/
# safety_rollback) は「補正できない」で hard-stop
# (3) update 側 reason があればそれ (pick_not_yes/detect_none/box_xz_none/
# sign_flip/dist_over/updated).
# (4) update 側 reason 未設定 (未到達で update スキップ) は apply 側 reason
# (applied/no_memory) を採用.
# updated → "applied" にマップ (成功の意味で User 語彙の "applied" に統合).
place_last = (log_record.get("correction") or {}).get("place_last")
if not isinstance(place_last, dict):
_place_corr_reason = "not_reached"
else:
_apply_r = place_last.get("reason", "") or ""
_upd_r = place_last.get("update_reason", "") or ""
if _apply_r in ("measure_mode", "disabled", "ssd_unmounted",
"entry_corrupted", "safety_rollback"):
_place_corr_reason = _apply_r
elif _upd_r in ("pick_not_yes", "detect_none", "box_xz_none",
"sign_flip", "dist_over"):
_place_corr_reason = _upd_r
elif _upd_r == "updated":
_place_corr_reason = "applied"
elif _apply_r in ("applied", "no_memory"):
_place_corr_reason = _apply_r
else:
_place_corr_reason = "unknown"
# ---- box_shift_reason (box_shift 3 列が埋まらない理由) ----
# 優先度: box_xz なし → box_xz_none / place 画像なし (swept_retreat 失敗等)
# → place_img_none / 画像あるが白箱検出失敗 → white_box_detect_fail / 揃った → ok.
_cap_place = _OBSERVE_STATE.get("cap_place")
_bx_valid = (isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and isinstance(box_xz[0], (int, float))
and isinstance(box_xz[1], (int, float)))
_bxl_valid = (isinstance(box_xz_last, (list, tuple))
and len(box_xz_last) == 2
and isinstance(box_xz_last[0], (int, float))
and isinstance(box_xz_last[1], (int, float)))
if not _bx_valid:
_box_shift_reason = "box_xz_none"
elif not _cap_place:
_box_shift_reason = "place_img_none"
elif not _bxl_valid:
_box_shift_reason = "white_box_detect_fail"
else:
_box_shift_reason = "ok"
# ---- place_img_path (退避後 place 画像パス. 検出失敗の後追い再検出用) ----
_place_img_path = _cap_place or ""
row.update({
"place_corr_reason": _place_corr_reason,
"box_shift_reason": _box_shift_reason,
"place_img_path": _place_img_path,
})
# DAY257 後段: VLM/dist override の内訳 2 列。最終 pick_verdict は既存
# "pick_verdict" 列 (override 後の結論)。最新 (=最終) pick_attempt から拾う。
attempts = log_record.get("pick_attempts") or []
last_pick = attempts[-1] if attempts else {}
vlm_pv = last_pick.get("vlm_verdict") or ""
if "dist_override" in last_pick:
dist_ov = "True" if last_pick["dist_override"] else "False"
else:
dist_ov = ""
row.update({
"vlm_pick_verdict": vlm_pv,
"pick_dist_override": dist_ov,
})
# DAY265: 横カメラ関連の 5 列を埋める.
# 画像パスは vlm_captures から camera_role=="side" + label で拾う (関節/上カメラ既存に非干渉).
side_ap_path = ""
side_place_path = ""
for c in log_record.get("vlm_captures", []) or []:
if c.get("camera_role") != "side":
continue
lbl = c.get("label", "")
img = c.get("image") or ""
if lbl == "ap_place_side" and img and not side_ap_path:
side_ap_path = str(img)
elif lbl == "place_side" and img and not side_place_path:
side_place_path = str(img)
side_pj = (log_record.get("judgments") or {}).get("place_side") or {}
side_verdict = side_pj.get("verdict") or ""
side_conf = (side_pj.get("confidence")
if isinstance(side_pj.get("confidence"), (int, float))
else "")
side_reason = side_pj.get("reason") or ""
row.update({
"side_ap_place_capture_path": side_ap_path,
"side_place_capture_path": side_place_path,
"side_place_verdict": side_verdict,
"side_place_confidence": side_conf,
"side_place_reason": side_reason,
})
# DAY265: 失敗完走時、stage_failures を pick_reason / place_reason に注記.
# v3 57 列に新列を足せないため既存の自由文列に |fails:... を append する.
# 検索: CSV で "|fails:" を grep すれば失敗完走行だけ抜ける.
# 振り分けは stage 番号で pick 側 (1-6, regrasp, pick_overall) と
# place 側 (7-10) を分離. 通常 (失敗なし) では両列とも VLM reason のまま.
stage_failures = log_record.get("stage_failures") or []
if stage_failures:
def _encode(fails):
if not fails:
return ""
parts = []
for f in fails:
st = f.get("stage")
kd = f.get("kind", "")
parts.append("{}:{}".format(st, kd))
return "|fails:" + ",".join(parts)
pick_side = []
place_side = []
for f in stage_failures:
st = f.get("stage")
# 数値 stage 番号 1-6 か "regrasp_*" か "pick_overall" が pick 側.
if isinstance(st, int):
(pick_side if st <= 6 else place_side).append(f)
else:
s = str(st)
if s.startswith("regrasp") or s.startswith("pick"):
pick_side.append(f)
else:
place_side.append(f)
if pick_side:
row["pick_reason"] = (row["pick_reason"] or "") + _encode(pick_side)
if place_side:
row["place_reason"] = (row["place_reason"] or "") + _encode(place_side)
is_new = not LEDGER_CSV_PATH.exists()
# codex 指摘ガード: 既存ヘッダと LEDGER_COLS が不一致なら追記しない。
# 不一致のまま append すると列ズレでデータが汚染されるため、ここで中断する
# (制御は止めず台帳記録だけスキップ)。
if not is_new:
try:
with open(LEDGER_CSV_PATH, "r", newline="") as fchk:
existing_header = next(csv.reader(fchk), None)
except Exception as e:
print("[LEDGER] [WARN] ヘッダ読込失敗 ({}). append スキップ"
.format(e))
return
if existing_header != LEDGER_COLS:
print("[LEDGER] [ABORT-APPEND] 既存ヘッダ ({} 列) と LEDGER_COLS "
"({} 列) が不一致 → append スキップ".format(
len(existing_header) if existing_header is not None else 0,
len(LEDGER_COLS)))
if existing_header is not None:
diff_extra = set(existing_header) - set(LEDGER_COLS)
diff_missing = set(LEDGER_COLS) - set(existing_header)
if diff_extra:
print("[LEDGER] 既存にのみ存在: {}".format(sorted(diff_extra)))
if diff_missing:
print("[LEDGER] LEDGER_COLS にのみ存在: {}".format(sorted(diff_missing)))
return
with open(LEDGER_CSV_PATH, "a", newline="") as f:
w = csv.DictWriter(f, fieldnames=LEDGER_COLS)
if is_new:
w.writeheader()
w.writerow(row)
print("[LEDGER] append OK: {} (in_box_geom={} dist={})".format(
LEDGER_CSV_PATH, in_box_geom, in_box_dist_mm))
except Exception as e:
print("[LEDGER] [WARN] append 例外（制御は継続）: {}".format(e))
def _max_delta(target, actual):
if target is None or actual is None:
return None
return max(abs(float(target[i]) - float(actual[i])) for i in range(6))
def _check_guard(r, label):
"""送信前ガード（レビュー反映版）。
1. joint_limit_ok == False → ABORT
2. method が送信許可リスト外 → ABORT
3. target_angles 不正 / j4 が J4_SAFE 外 → ABORT (DAY277 E2)
4. x_residual_m > X_RESID_THRESH → ABORT
5. z_residual_m → ログのみ（ここでは判定しない）
戻り値: (ok: bool, reason: str | None)
DAY274 修正: method 白リストに層3 の "fk_j4_priority" (j4 優先スコアで
選ばれた PLACE 姿勢) を追加. fk_lookup 内部で JOINT_LIMITS + j1<=88 の
safe_mask + PLACE_J1_MIN_DEG 帯を通過した安全姿勢だが, 白リストが
更新されていなかったため joint_ok=True・x_resid≈1mm でも撥ねられていた
(2026-07-06 の place_x-160_z200 で dee_y=5 の 5 連続 place_img_none の原因).
DAY277 修正 (E1 / codex 再レビュー): "fk_nearest_ee_y_tol_empty" を白リストから
除外. これは ee_y 帯内候補ゼロ時に target_ee_y を無視して xz 最近傍を返す
fk_lookup のフォールバック (fk_lookup.py L455-L463) で, 高さ目標を落とした姿勢を
実機に送るのはカバレッジ穴を送信で誤魔化すのと同義で危険 (置き高さ不定・ram/
高すぎ落下). ここでの ABORT は「その ee_y に FK 点が無い」ことの正しい顕在化で,
対処は当該 ee_y 帯の FK 再スキャン (カバレッジ拡充) であって送信許可ではない.
DAY277 修正 (E2 / codex 再レビュー): J4_SAFE=[J4_SAFE_MIN, J4_SAFE_MAX] を
送信前 backstop として全経路一律で検査. fk_lookup の safe_mask には J4_SAFE は
含まれず (fk_lookup.py L255 の設計「呼び出し側の事前検査で追加確認する」),
既存の補正事前検査 (L2721 付近) は「補正が適用された経路」限定. 高さ補正のみ /
no_memory / measure_mode の経路では J4_SAFE を誰も見ず, j4∉[-90,60] が
hard JOINT_LIMITS[4]=(-100,100) 内なら実機に出るリスクがあった (2026-07-06 実測
place_x-160_z200 × PLACE_HI で候補の 43% が j4∈[-100,-95) 帯). 本ガードは
target_angles を直接見るので, 経路 (補正の有無・phase・method) に依らず backstop.
ガードロジック本体 (joint_ok / x_resid / X_RESID_THRESH) と数値 (X_RESID_THRESH,
J4_SAFE_MIN/MAX) は変更していない.
"""
if not r.get("joint_limit_ok"):
return False, "joint_limit違反: {}".format(r.get("joint_limit_violations"))
# 許可 method:
# "interp" : j0_lookup の補間成功
# "fk_nearest" : fk_lookup の帯内最近傍成功 (従来の PLACE 経路)
# "fk_j4_priority" : DAY274 層3 の j4 優先スコアで選択成功
# "fk_nearest_ee_y_tol_empty" : ee_y 帯内候補ゼロ時 target_ee_y を無視して
# xz 最近傍を返す fallback. DAY277 (エイイチ判断)
# で白リストに再追加. 実機 stage8_place が
# この method で ABORT した件への対処
# (ガードを緩めて完走優先・カバレッジ穴の
# 再スキャンは別途判断).
# 不許可:
# "clamp" : j0_lookup 弧端張り付き
# "out_of_range" : fk_lookup 帯外 or 距離超過
_ALLOWED = ("interp", "fk_nearest", "fk_j4_priority",
"fk_nearest_ee_y_tol_empty")
_METHOD_SUFFIXES = ("_j4safe_pref", "_j4safe_absent")
_method = r.get("method")
_base_method = _method
if isinstance(_base_method, str):
for _suffix in _METHOD_SUFFIXES:
if _base_method.endswith(_suffix):
_base_method = _base_method[:-len(_suffix)]
break
if _base_method not in _ALLOWED:
return False, "method={} は送信許可リスト外".format(_method)
# DAY277 E2: 送角予定姿勢の j4 が J4_SAFE 帯内かを送信前 backstop で確認.
# target_angles は _do_move が実際に投げる配列 (r["target_angles"], index 4 = j4).
# 不正 (None / len!=6 / 数値でない) は安全側 ABORT (送らない).
_tgt = r.get("target_angles")
if not (isinstance(_tgt, (list, tuple)) and len(_tgt) == 6):
return False, ("target_angles 不正 (期待 len=6): {!r}".format(_tgt))
try:
_j4 = float(_tgt[4])
except (TypeError, ValueError):
return False, ("target_angles[4] (j4) が数値でない: {!r}".format(_tgt[4]))
if not (J4_SAFE_MIN <= _j4 <= J4_SAFE_MAX):
return False, ("j4={:+.2f} が J4_SAFE[{},{}] 外".format(
_j4, J4_SAFE_MIN, J4_SAFE_MAX))
xr = r.get("x_residual_m")
if xr is None or xr > X_RESID_THRESH:
return False, "x_resid={}m > X_RESID_THRESH ({})".format(xr, X_RESID_THRESH)
return True, None
def _do_move(target_angles, label, tol, log_record, *, dry_run,
abort_if_prev_not_reached=False):
"""target_angles へ移動し、結果を log_record["moves"] に追加。
dry_run=True の場合は送信せず計画のみ記録。
戻り値: (success: bool, post_actual: list | None)
C1 (DAY268 案R): abort_if_prev_not_reached=True が渡されたときだけ,
送信前に ABORT する. 呼び出し側 (stage6_lift / stage10_lift_back) が
log_record["moves"] から直前 DESCEND (stage4_pick / stage8_place) の arrived を
引いて渡す. 全 stage 到達時は False が渡り発火しない (案A の絶対差分方式で
正常な大移動を誤 ABORT した経路を解消).
"""
pre_actual = None if dry_run else _daemon_get_angles_filtered()
pre_max = _max_delta(target_angles, pre_actual)
move_rec = {
"label": label,
"target_angles": list(target_angles),
"reach_tol_deg": tol,
"pre_actual": list(pre_actual) if pre_actual is not None else None,
"pre_max_delta_deg": pre_max,
"dry_run": dry_run,
}
# C1 (DAY268 案R): 直前 DESCEND が未到達なら, この LIFT を送らず ABORT.
# 未到達で姿勢がズレた状態から理論姿勢へ急送角するのを防ぐ. 呼び出し側が
# 直前 DESCEND の arrived を見て abort_if_prev_not_reached を決める.
# 全 stage 到達時は False が渡るので発火しない (絶対差分方式の誤 ABORT を解消).
if not dry_run and abort_if_prev_not_reached:
move_rec["arrived"] = False
move_rec["retried"] = False
move_rec["post_actual"] = list(pre_actual) if pre_actual is not None else None
move_rec["post_max_delta_deg"] = pre_max
move_rec["aborted_reason"] = "prev_descend_not_reached"
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} ABORT (直前 DESCEND 未到達・送角せず)".format(label))
return False, (list(pre_actual) if pre_actual is not None else None)
if dry_run:
move_rec["arrived"] = None
move_rec["retried"] = None
move_rec["post_actual"] = None
move_rec["post_max_delta_deg"] = None
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} dry_run（送信なし）".format(label))
# dry-run でも STAGE 行を残す（観測系の検証を実機に依存させない）。
# 送信していない事実は moves[].post_actual = None が持つ。
_observe_emit_now()
_observe_stage_record(kind="stage")
return True, None
arrived, retried = _send_and_wait_arrival(
target_angles, label, tol, send_wait_sec=SEND_WAIT_SEC)
post_actual = _daemon_get_angles_filtered()
# 観測 emit（脇 UDP・fire-and-forget・例外は emit 側で握る）
# post_actual を再利用するので daemon I/O は増えない。print 出力にも影響しない
# in_box は _observe_emit_now と同じ _ledger_in_box_geometry を再利用（判定の二重実装を避ける）.
_ibg, _ = _ledger_in_box_geometry(
_OBSERVE_STATE["block_place_xz"], _OBSERVE_STATE["box_xz"])
_in_box = (True if _ibg == "True" else False if _ibg == "False" else None)
_observe_emit(
angles=post_actual,
phase=_OBSERVE_STATE["phase"],
phase_t0_ms=_OBSERVE_STATE["phase_t0_ms"],
gripper=_OBSERVE_STATE["gripper"],
trial=({"id": _OBSERVE_STATE["run_stem"],
"task": _OBSERVE_STATE["task"]}
if _OBSERVE_STATE["run_stem"] else None),
verdict=_OBSERVE_STATE["verdict"],
block_xz=_OBSERVE_STATE["block_xz"],
box_xz=_OBSERVE_STATE["box_xz"],
block_place_xz=_OBSERVE_STATE["block_place_xz"],
tip_pick=_OBSERVE_STATE["tip_pick"],
run_dir=_OBSERVE_STATE["run_dir"],
cap_tip=_OBSERVE_STATE["cap_tip"],
cap_pick=_OBSERVE_STATE["cap_pick"],
cap_place=_OBSERVE_STATE["cap_place"],
in_box=_in_box,
lookup=_OBSERVE_STATE["lookup"],
corr=_OBSERVE_STATE["corr"],
captures=_OBSERVE_STATE["captures"],
)
# 非移動 stage の通知 emit 用に最後の post_actual をキャッシュ（新規 I/O ゼロ）
_OBSERVE_STATE["last_angles"] = list(post_actual) if post_actual is not None else None
# 移動 stage 完了時の JSONL 追記（記録の正・UDP と独立）.
_observe_stage_record(kind="stage")
post_max = _max_delta(target_angles, post_actual)
move_rec["arrived"] = bool(arrived)
move_rec["retried"] = bool(retried)
move_rec["post_actual"] = list(post_actual) if post_actual is not None else None
move_rec["post_max_delta_deg"] = post_max
log_record.setdefault("moves", []).append(move_rec)
print("[MOVE ] {} arrived={} retried={} post_max|Δ|={}".format(
label, arrived, retried,
"{:.3f}°".format(post_max) if post_max is not None else "None"))
return arrived, post_actual
def _finite(v):
"""inf/nan を None に落とす（JSON.parse 互換のため）."""
try:
f = float(v)
return f if f == f and abs(f) != float("inf") else None
except Exception:
return None
def _record_lookup(r, label, target_xz, log_record):
"""j0_lookup 結果を log_record["lookups"] に追加 + コンソールに出力."""
rec = {
"label": label,
"phase": r["phase"],
"source_file": r["source_file"],
"pose_id": r.get("pose_id"),
"target_xz_m": list(target_xz),
"j0": r["j0"],
"method": r["method"],
"x_residual_m": r["x_residual_m"],
"z_residual_m": r["z_residual_m"],
"interp_xz_m": list(r["interp_xz_m"]),
"joint_limit_ok": r["joint_limit_ok"],
"joint_limit_violations": r["joint_limit_violations"],
"target_angles": list(r["target_angles"]),
"base_angles": list(r["base_angles"]),
"nearest_pts": r["nearest_pts"],
}
if "interp_t" in r:
rec["interp_t"] = r["interp_t"]
rec["interp_seg_idx"] = r["interp_seg_idx"]
# DAY265: fk_lookup の全軸帯 (ref_joints) 適用時は max_dj_deg と ee_y も残す.
if "max_dj_deg" in r and r.get("max_dj_deg") is not None:
rec["max_dj_deg"] = r["max_dj_deg"]
if "ee_y_m_selected" in r and r.get("ee_y_m_selected") is not None:
rec["ee_y_m_selected"] = r["ee_y_m_selected"]
if "joint_tol_deg_used" in r and r.get("joint_tol_deg_used") is not None:
rec["joint_tol_deg_used"] = r["joint_tol_deg_used"]
log_record.setdefault("lookups", []).append(rec)
# 観測用スリム版。UDP datagram に載せるため nearest_pts / base_angles は落とす
# （rec 本体は log_record["lookups"] に全量残る）。
# inf/nan は _finite で None に落とす（JS の JSON.parse が Infinity で落ちるため）。
_OBSERVE_STATE["lookup"] = {
"label": rec["label"],
"phase": rec["phase"],
"lookup_name": ("fk_lookup" if r["phase"] in FK_LOOKUP_PHASES else "j0_lookup"),
"source_file": rec["source_file"],
"pose_id": rec.get("pose_id"),
"method": rec["method"],
"target_xz_m": rec["target_xz_m"],
"x_residual_m": _finite(rec["x_residual_m"]),
"z_residual_m": _finite(rec["z_residual_m"]),
"joint_limit_ok": rec["joint_limit_ok"],
"joint_limit_violations": rec["joint_limit_violations"],
"ee_y_m_selected": _finite(rec.get("ee_y_m_selected")),
"ee_y_band_m": r.get("ee_y_band_m"),
"selected_idx": r.get("selected_idx"),
"dist_m": _finite(r.get("dist_m")),
"max_dj_deg": _finite(rec.get("max_dj_deg")),
"joint_tol_deg_used": rec.get("joint_tol_deg_used"),
"j1_selected_deg": r.get("j1_selected_deg"),
"j4_selected_deg": r.get("j4_selected_deg"),
"abort_reason": r.get("abort_reason"),
"guard_pass": None, # _check_guard 後に stage_lookup_and_move が上書き
"guard_reason": None,
}
# DAY265: place_withblock 経路 (ref_joints 使用) のときは max|Δjoint| と ee_y を表示.
extra = ""
if r.get("max_dj_deg") is not None:
extra = " max|Δj|={:.2f}°".format(r["max_dj_deg"])
if r.get("ee_y_m_selected") is not None:
extra += " ee_y={:.4f}m".format(r["ee_y_m_selected"])
# DAY265: out_of_range で sel=-1 のとき j0/x_resid/z_resid が None or inf のことがある.
# 従来 format {:+.3f} だと None で TypeError を出したのでヘルパで防御.
def _f3(v):
return "{:+.3f}".format(v) if isinstance(v, (int, float)) and v == v else "None"
def _f5(v):
return "{:.5f}".format(v) if isinstance(v, (int, float)) and v == v else "inf"
print("[LOOKUP] {} phase={} target_xz=({:+.4f},{:+.4f}) → j0={}° "
"method={} x_resid={}m z_resid={}m joint_ok={}{}".format(
label, r["phase"], target_xz[0], target_xz[1], _f3(r.get("j0")),
r["method"], _f5(r.get("x_residual_m")), _f5(r.get("z_residual_m")),
r["joint_limit_ok"], extra))
if r.get("method") == "out_of_range" and r.get("abort_reason"):
print("[LOOKUP] abort_reason: {}".format(r["abort_reason"]))
return rec
def _do_gripper(action, stage_label, log_record, dry_run):
"""グリッパー操作。戻り値（result=-1 含む）は DAY175 仕様で判定に使わない。
一方、daemon TCP 例外は本物の失敗として bool False を返す（close 未遂のまま
LIFT などの事故を防ぐ）。
DAY261: close は set_gripper_value(GRIP_VALUE) で push 保持に切替（位置で
止まらず押し込み力を残す）。open は従来どおり set_gripper_state(0)。
戻り値: True = 正常 or dry_run / False = daemon 例外（呼び出し側で ABORT 連動）
"""
state = 1 if action == "close" else 0
is_close = (action == "close")
print("[GRIP ] {} action={} method={} sleep={:.1f}s dry_run={}".format(
stage_label, action,
"value({})".format(GRIP_VALUE) if is_close else "state(0)",
GRIPPER_SLEEP, dry_run))
rec = {
"stage": stage_label, "action": action, "state": state,
"sleep_sec": GRIPPER_SLEEP, "dry_run": dry_run,
# DAY261: 把持方式の判別用。dry-run でも残るよう rec 構築時に書く。
"grip_method": "value" if is_close else "state",
}
if is_close:
rec["grip_value"] = GRIP_VALUE
ok = True
if not dry_run:
try:
if is_close:
rec["daemon_return"] = daemon_set_gripper_value(GRIP_VALUE)
else:
rec["daemon_return"] = daemon_set_gripper(state)
# 戻り値（result=-1 等）は判定に使わない（DAY175・成否無関係）
except Exception as e:
rec["error"] = str(e)
ok = False
print("[GRIP ] [ERROR] {}: daemon 例外: {}".format(stage_label, e))
time.sleep(GRIPPER_SLEEP)
log_record.setdefault("gripper", []).append(rec)
return ok
def _do_stage9_release_gripper(log_record, dry_run):
"""stage9 専用のグリッパー開放。
既知問題（戻り値が成否と無関係、連続コマンド無視、sleep 必須）に合わせ、
stage9 だけ open を二重送信する。把持 close 側や他 stage の open には触れない。
"""
print("[GRIP ] stage9_release action=open pre_sleep={:.1f}s "
"attempts={} sleep_each={:.1f}s dry_run={}".format(
STAGE9_RELEASE_PRE_SLEEP, STAGE9_RELEASE_OPEN_ATTEMPTS,
GRIPPER_SLEEP, dry_run))
rec = {
"stage": "stage9_release",
"action": "open",
"state": 0,
"pre_sleep_sec": STAGE9_RELEASE_PRE_SLEEP,
"sleep_sec": GRIPPER_SLEEP,
"attempt_count": STAGE9_RELEASE_OPEN_ATTEMPTS,
"dry_run": dry_run,
"attempts": [],
}
ok = True
if not dry_run:
time.sleep(STAGE9_RELEASE_PRE_SLEEP)
ok = False
for attempt in range(1, STAGE9_RELEASE_OPEN_ATTEMPTS + 1):
attempt_rec = {"attempt": attempt}
try:
attempt_rec["daemon_return"] = daemon_set_gripper(0)
ok = True
except Exception as e:
attempt_rec["error"] = str(e)
print("[GRIP ] [ERROR] stage9_release attempt {}: daemon 例外: {}"
.format(attempt, e))
rec["attempts"].append(attempt_rec)
time.sleep(GRIPPER_SLEEP)
log_record.setdefault("gripper", []).append(rec)
return ok
def _abort(log_record, stage, reason):
log_record["abort"] = {"stage": stage, "reason": reason}
print("[ABORT ] stage={} reason={}".format(stage, reason))
def _capture_for_vlm(label, frames_dir, log_record, dry_run, run_stem=None,
camera_role="top"):
"""VLM 判定用に上カメラ or 横カメラから 1 フレーム取得して frames_dir/{run_stem}_{label}.jpg に保存.
camera_role="top" (既定): iPhone Continuity index (既存挙動・完全互換).
camera_role="side": Iriun index=1 (DAY265 追加. approach_place / place の横視点判定用).
run_stem が None なら旧仕様 {label}.jpg にフォールバック（後方互換）.
dry_run の場合は撮影せず None を返す.
戻り値: 保存パス or None（取得失敗時）"""
cap_rec = {"label": label, "image": None, "dry_run": bool(dry_run),
"camera_role": camera_role}
if dry_run:
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
try:
frames_dir.mkdir(parents=True, exist_ok=True)
except Exception as e:
print("[CAP-VLM] {} frames_dir 作成失敗: {}".format(label, e))
cap_rec["error"] = "mkdir:{}".format(type(e).__name__)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
# camera_role に応じて resolve_cameras の要求を切り替え. side でも top を要求
# しないよう対称に. 未解決なら error 記録して None (通しは継続).
require_side_flag = (camera_role == "side")
require_top_flag = (camera_role == "top")
try:
cams = resolve_cameras(require_top=require_top_flag,
require_side=require_side_flag)
except Exception as e:
print("[CAP-VLM] {} camera_resolver 例外: {}".format(label, e))
cap_rec["error"] = "resolver:{}".format(type(e).__name__)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
if camera_role == "side":
cam_idx = cams.get("side") if cams else None
role_name = "横カメラ"
else:
cam_idx = cams.get("top") if cams else None
role_name = "上カメラ"
if cam_idx is None:
print("[CAP-VLM] {} {}未解決".format(label, role_name))
cap_rec["error"] = "no_{}_camera".format(camera_role)
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
cap = cv2.VideoCapture(cam_idx)
if not cap.isOpened():
print("[CAP-VLM] {} cap open 失敗 (index={} role={})".format(
label, cam_idx, camera_role))
cap_rec["error"] = "cap_open"
log_record.setdefault("vlm_captures", []).append(cap_rec)
return None
out_path = None
try:
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
for _ in range(VLM_CAPTURE_DROP_FRAMES):
cap.read()
ret, frame = cap.read()
if not ret or frame is None:
print("[CAP-VLM] {} フレーム取得失敗".format(label))
cap_rec["error"] = "read_fail"
return None
fn = ("{}_{}.jpg".format(run_stem, label)
if run_stem else "{}.jpg".format(label))
out_path = frames_dir / fn
if not cv2.imwrite(str(out_path), frame):
print("[CAP-VLM] {} 保存失敗: {}".format(label, out_path))
cap_rec["error"] = "imwrite_fail"
out_path = None
return None
cap_rec["image"] = str(out_path)
# 後方互換のため top_camera_index キーは残しつつ、side は別キーで残す.
if camera_role == "side":
cap_rec["side_camera_index"] = int(cam_idx)
else:
cap_rec["top_camera_index"] = int(cam_idx)
print("[CAP-VLM] {} 保存 ({}): {}".format(label, role_name, out_path))
finally:
try:
cap.release()
except Exception:
pass
log_record.setdefault("vlm_captures", []).append(cap_rec)
return out_path
# ---- DAY272 案T: PLACE 再照準 (stage8.5) -----------------------------------
def _visual_servo_loop_place(cap, j0_current, target_x_mm,
servo_base_j1_j5, servo_rec):
"""PLACE 再照準の中核ループ. PICK の visual_servo_pick.visual_servo_loop を
PLACE 用に複製移植 (PICK 本線関数は 1 バイトも触らない・DAY272 案T).
j0 以外の 5 軸 (servo_base_j1_j5) は stage8 到達姿勢に固定. j0 だけ更新.
符号論理は PICK と同一:
diff_x_mm = target_x_mm - block_x_mm (目標 - 実測)
delta_deg = diff_x_mm / MM_PER_DEG_J0_X_PLACE (負値の係数)
j0_new = j0_current + delta_deg
PLACE 姿勢が j0=+44° 帯で PICK の j0=-30° 帯と逆符号側でも, 更新式は
幾何的に同じで通る (係数はモジュール定数側で姿勢依存差を吸収).
ガード:
- PLACE_CONVERGE_THRESHOLD_MM (=8.0mm) で収束
- PLACE_MAX_DELTA_DEG (=5.0°) で 1 ループ delta をクランプ
- PLACE_SERVO_MAX_ITER / PLACE_SERVO_MAX_SEC で強制収束
- PLACE_SERVO_DETECT_FAIL_MAX 連続検出失敗で再照準中止
校正観測:
各反復で delta_j0_actual (実測 j0 変化) と delta_block_x_actual
(実測 block_x 変化) を servo_rec["history"] に残す. 後段で
MM_PER_DEG_J0_X_PLACE を実測校正する材料.
戻り値: (final_j0, converged, loop_count, exit_reason)
"""
t_start = time.time()
prev_block_x_mm = None
consecutive_fail = 0
loop_count = 0
while True:
elapsed = time.time() - t_start
servo_rec["elapsed_sec"] = elapsed
# 時間ガード + ループガード (強制収束)
if elapsed > PLACE_SERVO_MAX_SEC:
return j0_current, False, loop_count, "servo_forced_converge_time"
if loop_count >= PLACE_SERVO_MAX_ITER:
return j0_current, False, loop_count, "servo_forced_converge_iter"
loop_count += 1
# 1. フレーム取得 + 保持ブロック検出.
ret, frame = cap.read()
block_xz = _detect_block_from_frame(frame) if ret else None
if block_xz is None:
consecutive_fail += 1
servo_rec["history"].append({
"iter": loop_count,
"note": "detect_fail (consecutive={})".format(consecutive_fail),
})
if consecutive_fail >= PLACE_SERVO_DETECT_FAIL_MAX:
return j0_current, False, loop_count, "detection_failed"
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
continue
consecutive_fail = 0
# 2. diff 計算.
block_x_mm = float(block_xz[0]) * 1000.0
diff_x_mm = target_x_mm - block_x_mm
# 3. 収束判定 (唯一の自動成功停止条件).
if abs(diff_x_mm) <= PLACE_CONVERGE_THRESHOLD_MM:
servo_rec["history"].append({
"iter": loop_count,
"block_x_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_current, 3),
"converged": True,
})
print("[SERVO] [CONVERGED] iter={} block_x={:+.1f} target={:+.1f} "
"diff={:+.1f}mm <= {}mm".format(
loop_count, block_x_mm, target_x_mm,
diff_x_mm, PLACE_CONVERGE_THRESHOLD_MM))
return j0_current, True, loop_count, "converged"
# 4. delta 計算 + クランプ (暴走保護・PICK は DAY249 で撤去だが PLACE は再導入).
delta_deg_raw = diff_x_mm / MM_PER_DEG_J0_X_PLACE
delta_deg = max(-PLACE_MAX_DELTA_DEG,
min(PLACE_MAX_DELTA_DEG, delta_deg_raw))
j0_new = j0_current + delta_deg
# 5. 送角 (j0 のみ変更・j1..j5 は stage8 到達姿勢固定).
new_angles = [j0_new] + list(servo_base_j1_j5)
j0_before_send = j0_current
try:
daemon_send_angles(new_angles, wait=2)
except Exception as e:
servo_rec["note"] = "daemon_send_angles 例外: {}".format(e)
return j0_current, False, loop_count, "send_angle_error"
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
# 6. 実測 j0 (校正観測用).
measured = daemon_get_angles()
j0_measured = (measured[0]
if measured and len(measured) == 6 else None)
delta_j0_actual = (j0_measured - j0_before_send
if j0_measured is not None else None)
delta_block_x_actual = (block_x_mm - prev_block_x_mm
if prev_block_x_mm is not None else None)
# 7. 履歴に記録 (MM_PER_DEG_J0_X_PLACE 実測校正材料).
servo_rec["history"].append({
"iter": loop_count,
"block_x_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_before_send, 3),
"delta_deg_raw": round(delta_deg_raw, 3),
"delta_deg_clamped": round(delta_deg, 3),
"j0_sent": round(j0_new, 3),
"j0_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"delta_j0_actual": (round(delta_j0_actual, 3)
if delta_j0_actual is not None else None),
"delta_block_x_actual": (round(delta_block_x_actual, 2)
if delta_block_x_actual is not None else None),
})
print("[SERVO] iter={} block_x={:+.1f} target={:+.1f} diff={:+.1f} "
"j0 {:+.2f}→{:+.2f} (raw={:+.2f} clamp={:+.2f})".format(
loop_count, block_x_mm, target_x_mm, diff_x_mm,
j0_before_send, j0_new, delta_deg_raw, delta_deg))
# DAY272 案T (拡張): 観測モード = 1 ループのみで即座に抜ける.
# 送角後の block_x を追加測定し「箱中心へ近づいた／遠ざかった」を verdict
# 出力. 目視で「箱接触なし」を並行確認する用. 既存収束ループ本体は無変更.
# 検出失敗・カメラ例外時は verdict 出せずに抜けるが exit は observe_once.
if PLACE_SERVO_OBSERVE_ONCE:
observe = {
"delta_j0_sent": round(delta_deg, 3),
"j0_before": round(j0_before_send, 3),
"j0_after_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"block_x_before_mm": round(block_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_before_mm": round(diff_x_mm, 2),
}
# 送角後の block_x を追加測定 (動作完了を待つため 1 ループ分 sleep).
time.sleep(PLACE_SERVO_LOOP_WAIT_SEC)
ret2, frame2 = cap.read()
block_xz_after = (_detect_block_from_frame(frame2)
if ret2 else None)
if block_xz_after is not None:
block_x_after_mm = float(block_xz_after[0]) * 1000.0
diff_after = target_x_mm - block_x_after_mm
abs_before = abs(diff_x_mm)
abs_after = abs(diff_after)
if abs_after < abs_before:
verdict = "箱中心へ近づいた (縮んだ・符号方向 OK)"
elif abs_after > abs_before:
verdict = ("箱中心から遠ざかった (広がった・符号反転の疑い"
"・MM_PER_DEG_J0_X_PLACE の符号確認要)")
else:
verdict = "変わらない (誤差内・要判断)"
observe.update({
"block_x_after_mm": round(block_x_after_mm, 2),
"diff_after_mm": round(diff_after, 2),
"abs_diff_change_mm": round(abs_after - abs_before, 2),
"verdict": verdict,
})
print("[SERVO] ============ 観測モード (OBSERVE_ONCE) ============")
print("[SERVO] 送角 delta_j0={:+.2f}° j0 {:+.2f}→{}"
.format(delta_deg, j0_before_send,
("{:+.2f}".format(j0_measured)
if j0_measured is not None else "?")))
print("[SERVO] block_x before={:+.1f}mm after={:+.1f}mm"
.format(block_x_mm, block_x_after_mm))
print("[SERVO] diff_x before={:+.1f}mm after={:+.1f}mm"
.format(diff_x_mm, diff_after))
print("[SERVO] |diff| before={:.1f}mm after={:.1f}mm "
"Δ={:+.1f}mm".format(
abs_before, abs_after, abs_after - abs_before))
print("[SERVO] verdict: {}".format(verdict))
print("[SERVO] ====================================================")
else:
observe["verdict"] = "送角後 block 検出失敗 (verdict 出せず)"
print("[SERVO] 観測モード: 送角後 block 検出失敗. "
"verdict 出せず.")
servo_rec["observe_once"] = observe
# 現在姿勢を final_j0 に反映して抜ける.
j0_current = (j0_measured if j0_measured is not None else j0_new)
return j0_current, False, loop_count, "observe_once"
# 8. 次ループへ状態更新.
j0_current = (j0_measured if j0_measured is not None else j0_new)
prev_block_x_mm = block_x_mm
def _run_place_visual_servo(box_xz, log_record, dry_run):
"""stage8 到達後の PLACE 再照準 (stage8.5) を実行する.
- stage8 到達姿勢の j0 と j1..j5 を moves から取り出し, サーボループへ渡す
- 上カメラを 1 回だけ open してループ内で毎反復 read
- servo_rec = log_record["servo_place"] に history / exit_reason 等を残す
- 制御は絶対止めない (失敗時は skip して stage9 へ)
- dry_run は送角しない
- 補正記憶とは独立 (計測モード PLACE_CORR_MEASURE_MODE=True でも動く実時間制御)
"""
servo_rec = {
"history": [],
"exit_reason": "",
"converged": False,
"iter_count": 0,
"elapsed_sec": 0.0,
"j0_start": None,
"j0_final": None,
"target_x_mm": None,
"note": "",
}
log_record["servo_place"] = servo_rec
if dry_run:
servo_rec["exit_reason"] = "dry_run"
servo_rec["note"] = "dry_run のため送角せず"
print("[SERVO] PLACE 再照準 dry_run (送角なし)")
return
# stage8 到達姿勢を moves から取得 (握り潰しなし・実測値のみ使用).
angles_after_stage8 = None
for m in reversed(log_record.get("moves") or []):
if str(m.get("label", "")).startswith("stage8_place"):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
angles_after_stage8 = list(pa)
break
if angles_after_stage8 is None:
servo_rec["exit_reason"] = "no_stage8_post_actual"
servo_rec["note"] = "stage8 の post_actual が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
j0_current = float(angles_after_stage8[0])
servo_base_j1_j5 = [float(v) for v in angles_after_stage8[1:]]
servo_rec["j0_start"] = round(j0_current, 3)
servo_rec["servo_base_j1_j5"] = [round(v, 3) for v in servo_base_j1_j5]
print("[SERVO] stage8 到達姿勢: j0={:+.2f} base_j1..j5={}"
.format(j0_current, [round(v, 2) for v in servo_base_j1_j5]))
# box_xz の有効性.
if not (isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and box_xz[0] is not None):
servo_rec["exit_reason"] = "no_box_xz"
servo_rec["note"] = "box_xz が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
target_x_mm = float(box_xz[0]) * 1000.0
servo_rec["target_x_mm"] = round(target_x_mm, 2)
print("[SERVO] target_x_mm={:+.2f} (box_xz[0]={:+.4f}m)"
.format(target_x_mm, float(box_xz[0])))
# 上カメラ open (1 回・ループ内で reuse).
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
servo_rec["exit_reason"] = "resolver_error"
servo_rec["note"] = "camera_resolver 例外: {}".format(e)
print("[SERVO] {}".format(servo_rec["note"]))
return
top_idx = cams.get("top") if cams else None
if top_idx is None:
servo_rec["exit_reason"] = "no_top_camera"
servo_rec["note"] = "上カメラ未解決"
print("[SERVO] {}".format(servo_rec["note"]))
return
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
servo_rec["exit_reason"] = "cap_open_fail"
servo_rec["note"] = "cap.open() 失敗 index={}".format(top_idx)
print("[SERVO] {}".format(servo_rec["note"]))
return
try:
# warm-up.
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
# サーボ実行.
(final_j0, converged, loop_count,
exit_reason) = _visual_servo_loop_place(
cap, j0_current, target_x_mm, servo_base_j1_j5, servo_rec)
servo_rec["exit_reason"] = exit_reason
servo_rec["converged"] = bool(converged)
servo_rec["iter_count"] = int(loop_count)
servo_rec["j0_final"] = round(final_j0, 3)
except Exception as e:
servo_rec["exit_reason"] = "loop_exception"
servo_rec["note"] = "servo_loop 例外: {}".format(e)
print("[SERVO] loop 例外: {}".format(e), file=sys.stderr)
finally:
try:
cap.release()
except Exception:
pass
print("[SERVO] PLACE 再照準 完了 converged={} iter={} exit={} "
"j0 {}→{} elapsed={:.2f}s".format(
servo_rec.get("converged"),
servo_rec.get("iter_count"),
servo_rec.get("exit_reason"),
servo_rec.get("j0_start"),
servo_rec.get("j0_final"),
servo_rec.get("elapsed_sec", 0.0)))
# ---- DAY277: PICK 視覚サーボ (PLACE の対称移植) ----------------------------
# PLACE 側 `_visual_servo_loop_place` / `_run_place_visual_servo` を PICK に対称
# 移植. 新ロジックなし・PLACE 本線関数は 1 バイトも触らない.
#
# 目的: stage4_pick 到達後・stage4.5 爪先測定前に j0 だけで爪先(TIP)を検出
# block_x に寄せる閉ループ. 開ループ (lookup 一発) の残差を送信前に詰める.
#
# PLACE との対称:
# PLACE: 保持 block_x を box_x に寄せる (arm 保持ブロックを箱中心へ).
# PICK : TIP_x を検出 block_x に寄せる (arm 爪先を赤ブロックへ).
# 検出器の違い:
# PLACE: `_detect_block_from_frame(frame)` (赤ブロックの Unity xz).
# PICK : `_detect_tip_from_frame(frame)` (青シール 3 点 → 最近ペア中点の
# Unity xz. `_measure_tip_vs_block` L614-628 と同じ姿勢非依存判定).
def _detect_tip_from_frame(frame):
"""DAY277: cv2 frame から 爪先 (青シール上位3点のうち最近ペア中点) の
Unity 座標 [x, z] を検出する. `_measure_tip_vs_block` (L596) から画像
ロード後の tip 検出部分だけを抜き出した frame 直渡し版 (jpg 保存を挟まない
ためサーボループから毎反復呼べる). 検出/変換 pipeline は同一
(_detect_blue_tips_px + pixel_to_unity). 全例外を握って None (制御を
絶対止めない).
姿勢非依存の爪先判定 (`_measure_tip_vs_block` docstring 準拠):
上位 3 点のうち「互いに最も近い 2 点＝爪先」, 残り 1 点＝手首.
爪先間距離 < 手首までの距離 が崩れたら前提破綻で None.
"""
if frame is None:
return None
try:
top3 = _detect_blue_tips_px(frame)
if top3 is None:
return None
import itertools
pairs = list(itertools.combinations(range(3), 2))
def _d(p, q):
return math.hypot(p[0] - q[0], p[1] - q[1])
i, j = min(pairs, key=lambda ij: _d(top3[ij[0]], top3[ij[1]]))
tip1, tip2 = top3[i], top3[j]
wrist = top3[3 - i - j]
mid_px = ((tip1[0] + tip2[0]) / 2.0, (tip1[1] + tip2[1]) / 2.0)
if not (_d(tip1, tip2) < _d(mid_px, wrist)):
return None
tip_ux, tip_uz = pixel_to_unity(mid_px[0], mid_px[1])
return [float(tip_ux), float(tip_uz)]
except Exception:
return None
def _visual_servo_loop_pick(cap, j0_current, target_x_mm,
servo_base_j1_j5, servo_rec):
"""PICK 再照準の中核ループ. `_visual_servo_loop_place` (L1807) を PICK に
対称移植 (PLACE 本線関数は 1 バイトも触らない).
j0 以外の 5 軸 (servo_base_j1_j5) は stage4_pick 到達姿勢に固定. j0 だけ更新.
符号論理 (PLACE と同一):
diff_x_mm = target_x_mm - tip_x_mm (目標 block_x - 実測 tip_x)
delta_deg = diff_x_mm / MM_PER_DEG_J0_X_PICK (負値の係数)
j0_new = j0_current + delta_deg
ガード:
- PICK_CONVERGE_THRESHOLD_MM (=10.0mm) で収束
- PICK_MAX_DELTA_DEG (=5.0°) で 1 ループ delta をクランプ
- PICK_SERVO_MAX_ITER / PICK_SERVO_MAX_SEC で強制収束
- PICK_SERVO_DETECT_FAIL_MAX 連続検出失敗で再照準中止
戻り値: (final_j0, converged, loop_count, exit_reason)
"""
t_start = time.time()
consecutive_fail = 0
loop_count = 0
while True:
elapsed = time.time() - t_start
servo_rec["elapsed_sec"] = elapsed
# 時間ガード + ループガード (強制収束).
if elapsed > PICK_SERVO_MAX_SEC:
return j0_current, False, loop_count, "servo_forced_converge_time"
if loop_count >= PICK_SERVO_MAX_ITER:
return j0_current, False, loop_count, "servo_forced_converge_iter"
loop_count += 1
# 1. フレーム取得 + TIP 検出.
ret, frame = cap.read()
tip_xz = _detect_tip_from_frame(frame) if ret else None
if tip_xz is None:
consecutive_fail += 1
servo_rec["history"].append({
"iter": loop_count,
"note": "detect_fail (consecutive={})".format(consecutive_fail),
})
if consecutive_fail >= PICK_SERVO_DETECT_FAIL_MAX:
return j0_current, False, loop_count, "detection_failed"
time.sleep(PICK_SERVO_LOOP_WAIT_SEC)
continue
consecutive_fail = 0
# 2. diff 計算 (目標 block_x - 実測 tip_x).
tip_x_mm = float(tip_xz[0]) * 1000.0
diff_x_mm = target_x_mm - tip_x_mm
# 3. 収束判定 (唯一の自動成功停止条件).
if abs(diff_x_mm) <= PICK_CONVERGE_THRESHOLD_MM:
servo_rec["history"].append({
"iter": loop_count,
"tip_x_mm": round(tip_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_current, 3),
"converged": True,
})
print("[SERVO] [CONVERGED] iter={} tip_x={:+.1f} target={:+.1f} "
"diff={:+.1f}mm <= {}mm".format(
loop_count, tip_x_mm, target_x_mm,
diff_x_mm, PICK_CONVERGE_THRESHOLD_MM))
return j0_current, True, loop_count, "converged"
# 4. delta 計算 + クランプ.
delta_deg_raw = diff_x_mm / MM_PER_DEG_J0_X_PICK
delta_deg = max(-PICK_MAX_DELTA_DEG,
min(PICK_MAX_DELTA_DEG, delta_deg_raw))
j0_new = j0_current + delta_deg
# 5. 送角 (j0 のみ変更・j1..j5 は stage4_pick 到達姿勢固定).
new_angles = [j0_new] + list(servo_base_j1_j5)
j0_before_send = j0_current
try:
daemon_send_angles(new_angles, wait=2)
except Exception as e:
servo_rec["note"] = "daemon_send_angles 例外: {}".format(e)
return j0_current, False, loop_count, "send_angle_error"
time.sleep(PICK_SERVO_LOOP_WAIT_SEC)
# 6. 実測 j0 (校正観測用).
measured = daemon_get_angles()
j0_measured = (measured[0]
if measured and len(measured) == 6 else None)
delta_j0_actual = (j0_measured - j0_before_send
if j0_measured is not None else None)
# 7. 履歴に記録.
servo_rec["history"].append({
"iter": loop_count,
"tip_x_mm": round(tip_x_mm, 2),
"target_x_mm": round(target_x_mm, 2),
"diff_x_mm": round(diff_x_mm, 2),
"j0_before": round(j0_before_send, 3),
"delta_deg_raw": round(delta_deg_raw, 3),
"delta_deg_clamped": round(delta_deg, 3),
"j0_sent": round(j0_new, 3),
"j0_measured": (round(j0_measured, 3)
if j0_measured is not None else None),
"delta_j0_actual": (round(delta_j0_actual, 3)
if delta_j0_actual is not None else None),
})
print("[SERVO] iter={} tip_x={:+.1f} target={:+.1f} diff={:+.1f} "
"j0 {:+.2f}→{:+.2f} (raw={:+.2f} clamp={:+.2f})".format(
loop_count, tip_x_mm, target_x_mm, diff_x_mm,
j0_before_send, j0_new, delta_deg_raw, delta_deg))
# 8. 次ループへ状態更新.
j0_current = (j0_measured if j0_measured is not None else j0_new)
def _run_pick_visual_servo(block_xz, log_record, dry_run):
"""stage4_pick 到達後・stage4.5 爪先測定前の PICK 再照準を実行する.
`_run_place_visual_servo` (L1994) を PICK に対称移植.
- stage4_pick 到達姿勢の j0 と j1..j5 を moves から取り出し, サーボループへ
渡す (新規 I/O ゼロ・既存 moves 経路の再利用).
- 上カメラを 1 回だけ open してループ内で毎反復 read.
- servo_rec = log_record["servo_pick"] に history / exit_reason 等を残す
(PLACE の "servo_place" と対称).
- 制御は絶対止めない (失敗時は skip して stage4.5 → stage5 grip へ).
- dry_run は送角しない.
block_xz: 掴む赤ブロックの (unity_x, unity_z) [m] (通常 cur_block_xz).
target_x_mm = block_xz[0] * 1000 が PICK 再照準の目標値.
"""
servo_rec = {
"history": [],
"exit_reason": "",
"converged": False,
"iter_count": 0,
"elapsed_sec": 0.0,
"j0_start": None,
"j0_final": None,
"target_x_mm": None,
"note": "",
}
log_record["servo_pick"] = servo_rec
if dry_run:
servo_rec["exit_reason"] = "dry_run"
servo_rec["note"] = "dry_run のため送角せず"
print("[SERVO] PICK 再照準 dry_run (送角なし)")
return
# stage4_pick 到達姿勢を moves から取得 (握り潰しなし・実測値のみ使用).
angles_after_stage4 = None
for m in reversed(log_record.get("moves") or []):
if str(m.get("label", "")).startswith("stage4_pick"):
pa = m.get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
angles_after_stage4 = list(pa)
break
if angles_after_stage4 is None:
servo_rec["exit_reason"] = "no_stage4_post_actual"
servo_rec["note"] = "stage4_pick の post_actual が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
j0_current = float(angles_after_stage4[0])
servo_base_j1_j5 = [float(v) for v in angles_after_stage4[1:]]
servo_rec["j0_start"] = round(j0_current, 3)
servo_rec["servo_base_j1_j5"] = [round(v, 3) for v in servo_base_j1_j5]
print("[SERVO] stage4_pick 到達姿勢: j0={:+.2f} base_j1..j5={}"
.format(j0_current, [round(v, 2) for v in servo_base_j1_j5]))
# block_xz の有効性.
if not (isinstance(block_xz, (list, tuple)) and len(block_xz) == 2
and block_xz[0] is not None):
servo_rec["exit_reason"] = "no_block_xz"
servo_rec["note"] = "block_xz が取れず再照準スキップ"
print("[SERVO] {}".format(servo_rec["note"]))
return
target_x_mm = float(block_xz[0]) * 1000.0
servo_rec["target_x_mm"] = round(target_x_mm, 2)
print("[SERVO] target_x_mm={:+.2f} (block_xz[0]={:+.4f}m)"
.format(target_x_mm, float(block_xz[0])))
# 上カメラ open (1 回・ループ内で reuse).
try:
cams = resolve_cameras(require_top=True, require_side=False)
except Exception as e:
servo_rec["exit_reason"] = "resolver_error"
servo_rec["note"] = "camera_resolver 例外: {}".format(e)
print("[SERVO] {}".format(servo_rec["note"]))
return
top_idx = cams.get("top") if cams else None
if top_idx is None:
servo_rec["exit_reason"] = "no_top_camera"
servo_rec["note"] = "上カメラ未解決"
print("[SERVO] {}".format(servo_rec["note"]))
return
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
servo_rec["exit_reason"] = "cap_open_fail"
servo_rec["note"] = "cap.open() 失敗 index={}".format(top_idx)
print("[SERVO] {}".format(servo_rec["note"]))
return
try:
# warm-up.
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
# サーボ実行.
(final_j0, converged, loop_count,
exit_reason) = _visual_servo_loop_pick(
cap, j0_current, target_x_mm, servo_base_j1_j5, servo_rec)
servo_rec["exit_reason"] = exit_reason
servo_rec["converged"] = bool(converged)
servo_rec["iter_count"] = int(loop_count)
servo_rec["j0_final"] = round(final_j0, 3)
except Exception as e:
servo_rec["exit_reason"] = "loop_exception"
servo_rec["note"] = "servo_loop 例外: {}".format(e)
print("[SERVO] loop 例外: {}".format(e), file=sys.stderr)
finally:
try:
cap.release()
except Exception:
pass
print("[SERVO] PICK 再照準 完了 converged={} iter={} exit={} "
"j0 {}→{} elapsed={:.2f}s".format(
servo_rec.get("converged"),
servo_rec.get("iter_count"),
servo_rec.get("exit_reason"),
servo_rec.get("j0_start"),
servo_rec.get("j0_final"),
servo_rec.get("elapsed_sec", 0.0)))
def _ask_human_verdict(image_path, vlm_judgment, kind):
"""人による目視判定を取得（pick/place 共通）。
画像パスと VLM 判定を並べて表示し、y/n/u を受ける。
戻り値: "yes" / "no" / "unknown" のいずれか（不正入力は再入力ループ）。
"""
question = "掴めている?" if kind == "pick" else "箱に入った?"
print()
print("=" * 70)
print("[HUMAN ] 目視判定をお願いします（kind={}）".format(kind))
print("=" * 70)
print(" 画像: {}".format(image_path))
print(" VLM : verdict={} confidence={} reason={}".format(
vlm_judgment.get("verdict"),
vlm_judgment.get("confidence"),
vlm_judgment.get("reason")))
while True:
try:
ans = input("{} [y/n/u(unknown)] > ".format(question)).strip().lower()
except EOFError:
print(" stdin EOF → unknown 扱いで進む")
return "unknown"
if ans in ("y", "yes"):
return "yes"
if ans in ("n", "no"):
return "no"
if ans in ("u", "unknown"):
return "unknown"
print(" 入力が無効。y / n / u のいずれかを入力してください")
def _judge_image(image_path, kind, dry_run, dummy_verdict):
"""VLM 判定を呼び出す薄ラッパー。dry_run / image 無し は unknown ダミーを返す.
image_path: str | Path (1 画像) or list/tuple (複数画像。pick の 2 枚比較で利用)。
戻り値の dict には "image" キー（参照画像パス・複数なら最後の 1 枚）を追加して返す."""
if dry_run:
verdict = dummy_verdict if dummy_verdict else "unknown"
return {
"verdict": verdict, "confidence": 0.0,
"reason": "dry_run", "model": None,
"image": None, "raw": None,
}
# 画像参照の正規化 (str/list/tuple 受け入れ)
if isinstance(image_path, (list, tuple)):
if not image_path or any(p is None for p in image_path):
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "no_image", "model": None,
"image": None, "raw": None,
}
paths_for_api = [str(p) for p in image_path]
last_path = paths_for_api[-1]
else:
if image_path is None:
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "no_image", "model": None,
"image": None, "raw": None,
}
paths_for_api = str(image_path)
last_path = str(image_path)
try:
r = vlm_judge_call(paths_for_api, kind)
except Exception as e:
# vlm_judge 側で例外は吸収しているはずだが念のため二重に
return {
"verdict": "unknown", "confidence": 0.0,
"reason": "judge_exception:{}".format(type(e).__name__),
"model": None, "image": last_path, "raw": None,
}
r["image"] = last_path
return r
# ---- 段ごと処理 -------------------------------------------------------------
def stage1_home_and_open(log_record, dry_run):
"""段1: HOME 復帰 + グリッパー開.
DAY265: 失敗完走化. どちらの sub-step が失敗しても記録だけ残して True を返し、
通しを止めない. arrived 情報は _do_move が log_record.moves に記録済み.
"""
print("[STAGE 1] HOME 復帰 + グリッパー開")
log_record["stages"].append("stage1")
ok, _ = _do_move(HOME, "stage1_to_home", TOL_HOME, log_record, dry_run=dry_run)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 1, "label": "stage1_to_home", "kind": "not_reached"})
print("[STAGE 1] HOME 未到達. 記録のみ残して継続")
if not _do_gripper("open", "stage1_grip_open", log_record, dry_run):
log_record.setdefault("stage_failures", []).append(
{"stage": 1, "label": "stage1_grip_open", "kind": "gripper_open_fail"})
print("[STAGE 1] グリッパー開失敗. 記録のみ残して継続")
return True
def stage2_detect_block_and_box(log_record, dry_run, dummy_block_xz, dummy_box_xz,
require_box=True):
"""段2: 赤ブロック + 白箱の **同時検出**（detect_positions(repeat=DETECT_REPEAT)）.
- dry_run: カメラに触れず、block/box ともダミー値（指定なければ block=(0.05,0.30) /
box=BOX_XZ デフォルト）を返す（require_box は無視）
- 実機: detect_positions が camera_resolver → cap → warm-up → homography →
detect_red_block と 06f detect_white_box_once をワンショットで叩き、xz 中央値で集約
- ガード:
block_xz=None → 常に ABORT（赤ブロック未検出）
require_box=True 時:
box_xz=None → ABORT（白箱未検出）
box_score<THRESH → ABORT（低スコア箱位置で place に降りない）
require_box=False 時:
box の 2 ガードはスキップ。box は取れていれば値、取れなければ None を返す
（掴み直しの再検出では block だけ要るため。box は段2 初回の値を保持する想定）
戻り値: (block_xz: tuple|None, box_xz: tuple|None)
"""
print("[STAGE 2] 赤ブロック + 白箱 同時検出（block と box は同回・同 homography）")
log_record["stages"].append("stage2")
if dry_run:
block_xz = (tuple(dummy_block_xz) if dummy_block_xz is not None
else (0.05, 0.30))
box_xz = (tuple(dummy_box_xz) if dummy_box_xz is not None
else tuple(BOX_XZ))
log_record.setdefault("detect", []).append({
"stage": 2, "source": "dry_run_dummy",
"block_xz_m": list(block_xz),
"box_xz_m": list(box_xz),
"block_score": None,
"box_score": None,
"n_captures": 0,
})
print("[DETECT] dry_run dummy block_xz=({:+.4f},{:+.4f}) "
"box_xz=({:+.4f},{:+.4f})".format(
block_xz[0], block_xz[1], box_xz[0], box_xz[1]))
return block_xz, box_xz
# 実機検出（detect_positions が内部で全部やる）
try:
r = detect_positions(repeat=DETECT_REPEAT)
except Exception as e:
_abort(log_record, 2, "detect_positions 例外: {}".format(e))
return None, None
block = r.get("block_xz_m")
box = r.get("box_xz_m")
scores = r.get("detect_score") or {}
block_score = scores.get("block")
box_score = scores.get("box")
rec = {
"stage": 2, "source": "live",
"block_xz_m": list(block) if block else None,
"box_xz_m": list(box) if box else None,
"block_score": block_score,
"box_score": box_score,
"n_captures": r.get("n_captures"),
"top_camera_index": r.get("resolved_top_index"),
}
log_record.setdefault("detect", []).append(rec)
print("[DETECT] block_xz={} block_score={}".format(block, block_score))
print("[DETECT] box_xz={} box_score={}".format(box, box_score))
# 段2 ガード（block は常に必須、box は require_box=True 時のみ必須）
if block is None:
_abort(log_record, 2, "赤ブロック未検出")
return None, None
if require_box:
if box is None:
_abort(log_record, 2, "白箱未検出")
return None, None
if box_score is None or box_score < BOX_SCORE_THRESH:
_abort(log_record, 2,
"white_box score {} < {}（低スコアの箱位置で place に降りない）".format(
box_score, BOX_SCORE_THRESH))
return None, None
return (float(block[0]), float(block[1])), (float(box[0]), float(box[1]))
# require_box=False: block のみ必須。box は取れていれば値・無ければ None
block_t = (float(block[0]), float(block[1]))
box_t = (float(box[0]), float(box[1])) if box is not None else None
return block_t, box_t
def stage_lookup_and_move(phase, target_xz, label, tol, log_record, dry_run, stage_no,
j3_ref=None, j5_ref=None,
ref_joints=None, joint_tol_deg=None,
target_ee_y_m=None,
pick_j0_ref=None,
place_j0_ref=None):
"""lookup → 送信前ガード → 移動。共通段（段3/4/7/8）.
DAY262: j3_ref / j5_ref を受け place_withblock のとき fk_lookup へ転送していたが、
DAY265 で全軸帯 (ref_joints/joint_tol_deg) に統合. place_withblock は ref_joints
経路を使い、j3_ref/j5_ref の受け口は残す (他の用途で使えるように・PICK には無影響).
DAY274 層3: target_ee_y_m (m) を受け, place_withblock のとき fk_lookup へ転送する
(j4 優先スコア・案X). None なら従来 (xz 最近傍・bit-perfect 一致).
DAY277 (A_PICK):
- approach_pick を fk_lookup 経路に追加. ee_y 帯 (上空) と
APPROACH_PICK_J1_REF/J4_REF + PICK_J1_TOL_DEG/J4_TOL_DEG で
姿勢族拘束 (j1=64 等の乱れ姿勢を弾く).
- pick (descend) には pick_j0_ref を受け, approach で確定した j0 を
j0_ref として fk_lookup に渡す (j0 lock で「真下降下」). tol は
PICK_J0_LOCK_TOL_DEG=3° 帯.
- pick_j0_ref=None (デフォルト) は j0 lock 無効 = 従来 fk_lookup と
bit-perfect 一致.
DAY278 (A_PLACE 同型):
- approach_place を fk_lookup 経路に追加 (旧 j0_lookup 円弧経路を置換).
ee_y 上空帯 + APPROACH_PLACE_J1_REF/J4_REF + PLACE_J1_TOL_DEG/J4_TOL_DEG
で姿勢族拘束.
- place_withblock (descend_place) には place_j0_ref を受け, approach_place
で確定した j0 を j0_ref として fk_lookup に渡す (真下降下・place 版).
tol は PLACE_J0_LOCK_TOL_DEG=3° 帯.
- place_j0_ref=None (デフォルト) は j0 lock 無効 = 従来 place_withblock と
bit-perfect 一致.
戻り値: (success: bool, lookup_result: dict | None)
"""
log_record["stages"].append(label)
# pick descend + DAY262 place_withblock + DAY277 approach_pick +
# DAY278 approach_place は FK テーブル全引き.
use_fk = phase in FK_LOOKUP_PHASES
lookup_name = "fk_lookup" if use_fk else "j0_lookup"
try:
if use_fk:
# DAY265: place_withblock は箱高さ帯 + 全軸帯 (approach 連続性) +
# ee_y 下限ガードで候補を絞る. j3_ref/j5_ref は全軸帯に包含されるため
# stage8 呼び出し側からは渡さない (default None).
if phase == "place_withblock":
# DAY278: place_j0_ref があれば approach_place j0 を ref に
# j0 lock (「真下降下」の place 版). tol は
# PLACE_J0_LOCK_TOL_DEG=3° 帯.
_j0_ref_kw = {}
if place_j0_ref is not None:
_j0_ref_kw = dict(j0_ref=place_j0_ref,
j0_tol_deg=PLACE_J0_LOCK_TOL_DEG)
# DAY278: J4_SAFE 内優先 rank (place_withblock 限定).
# box 位置周辺の候補内で SAFE 内 (j4 ∈ [-90,+60]) の姿勢を最優先で
# 選び, SAFE 外候補 (j4=-101 等) を xz ジャストで拾って E2 撥ねる
# 事象を上流で解消. J4_SAFE_MIN/MAX は run_pick_place の定数を渡し,
# fk_lookup 側で焼き付けない. SAFE 内候補が 0 のときは従来 rank に
# フォールバックし E2 backstop が最終防御 (ガード緩和はしない).
r = fk_lookup(phase, target_xz,
ee_y_min=PLACE_EE_Y_MIN,
ee_y_max=PLACE_EE_Y_MAX,
j3_ref=j3_ref,
j5_ref=j5_ref,
ref_joints=ref_joints,
joint_tol_deg=joint_tol_deg,
place_ee_y_floor=PLACE_EE_Y_FLOOR,
place_j1_min_deg=PLACE_J1_MIN_DEG,
target_ee_y_m=target_ee_y_m,
j4_safe_min=J4_SAFE_MIN,
j4_safe_max=J4_SAFE_MAX,
**_j0_ref_kw)
elif phase == "approach_pick":
# DAY277: approach_pick fk_lookup 化. ee_y 上空帯 +
# 姿勢族拘束 (j1/j4 per-axis tol) で「良い approach 族」だけを候補に.
r = fk_lookup(phase, target_xz,
ee_y_min=APPROACH_PICK_EE_Y_MIN,
ee_y_max=APPROACH_PICK_EE_Y_MAX,
j1_ref=APPROACH_PICK_J1_REF,
j1_tol_deg=PICK_J1_TOL_DEG,
j4_ref=APPROACH_PICK_J4_REF,
j4_tol_deg=PICK_J4_TOL_DEG)
elif phase == "approach_place":
# DAY278: approach_place fk_lookup 化 (A_PICK 同型).
# ee_y 上空帯 + 姿勢族拘束 (j1/j4 per-axis tol).
# 引数構成は approach_pick と 1:1 対応 (定数だけ差し替え).
r = fk_lookup(phase, target_xz,
ee_y_min=APPROACH_PLACE_EE_Y_MIN,
ee_y_max=APPROACH_PLACE_EE_Y_MAX,
j1_ref=APPROACH_PLACE_J1_REF,
j1_tol_deg=PLACE_J1_TOL_DEG,
j4_ref=APPROACH_PLACE_J4_REF,
j4_tol_deg=PLACE_J4_TOL_DEG)
else:
# pick (descend). DAY277: pick_j0_ref が指定されていれば
# approach j0 を ref に j0 lock (真下降下). None のときは従来通り
# fk_lookup.py 既定の ee_y 10-40mm 帯・ref_joints 無しで xz 最近傍.
if pick_j0_ref is not None:
r = fk_lookup(phase, target_xz,
j0_ref=pick_j0_ref,
j0_tol_deg=PICK_J0_LOCK_TOL_DEG)
else:
r = fk_lookup(phase, target_xz)
else:
r = j0_lookup(phase, target_xz)
except (ValueError, FileNotFoundError, RuntimeError) as e:
_abort(log_record, stage_no, "{} 失敗: {}".format(lookup_name, e))
return False, None
rec = _record_lookup(r, label, target_xz, log_record)
ok_guard, reason = _check_guard(r, label)
rec["guard_pass"] = bool(ok_guard)
rec["guard_reason"] = reason
# 観測側にもガード結果を反映（ABORT 時も画面に理由が出るようにする）
if isinstance(_OBSERVE_STATE.get("lookup"), dict):
_OBSERVE_STATE["lookup"]["guard_pass"] = bool(ok_guard)
_OBSERVE_STATE["lookup"]["guard_reason"] = reason
if not ok_guard:
# 送信前 ABORT は _do_move を通らないため emit されず、画面から原因が読めなかった。
# 通知専用 emit を 1 発だけ打つ（daemon I/O ゼロ・制御に干渉しない）。
_observe_emit_now()
_observe_stage_record(kind="abort")
_abort(log_record, stage_no, "送信前ガード NG: " + reason)
return False, r
# z_residual はログのみ（ABORT しない）
if r["z_residual_m"] > 0.05:
print("[INFO ] {} z_residual_m={:.4f}m （情報のみ・ABORT しない。"
"工程跨ぎや弧高さ差で構造的に大きいことがある）".format(
label, r["z_residual_m"]))
ok_move, _ = _do_move(
r["target_angles"], label, tol, log_record, dry_run=dry_run)
if not ok_move:
# DAY265: 到達失敗しても継続. arrived の真偽は _do_move が log_record.moves
# の arrived に記録済み. 失敗事実だけ stage_failures / rec に残して True 返し.
rec["arrived_ok"] = False
log_record.setdefault("stage_failures", []).append(
{"stage": stage_no, "label": label, "kind": "not_reached"})
print("[STAGE {}] 到達失敗 (label={}). 記録のみで次 stage へ".format(
stage_no, label))
return True, r
rec["arrived_ok"] = True
return True, r
# ---- メイン --------------------------------------------------------------
def run(dry_run=False, dummy_block_xz=None, dummy_box_xz=None,
dummy_pick_verdict=None, dummy_place_verdict=None,
human_judge=False):
# KIOXIA 未マウント時に Mac 内蔵ディスクへ誤書き込みするのを防ぐ.
# dry_run は撮影/保存しないので素通し. 実機 run のみガード.
if not dry_run and not _ensure_ssd_mounted():
print("[ABORT ] KIOXIA SSD 未マウント。run データの出力先が無いため中断します。"
"（{} を確認）".format(LEDGER_DIR))
return False, None
log_path = _next_log_path()
run_stem = log_path.stem
# 画像も JSON と同じ run フォルダ直下に置く（_frames サブ階層は廃止）
frames_dir = log_path.parent
# 画面のファイル情報表示用に run フォルダパスを観測状態へ
_OBSERVE_STATE["run_dir"] = str(log_path.parent)
# 観測 emit 用の状態を初期化（このプロセスで複数 run() を回す場合の前回残置を消す）
_OBSERVE_STATE["phase"] = None
_OBSERVE_STATE["phase_t0_ms"] = None
_OBSERVE_STATE["gripper"] = None
_OBSERVE_STATE["verdict"] = {"pick": None, "place": None, "side_place": None}
_OBSERVE_STATE["block_xz"] = None
_OBSERVE_STATE["box_xz"] = None
_OBSERVE_STATE["block_place_xz"] = None
_OBSERVE_STATE["tip_pick"] = None
_OBSERVE_STATE["cap_tip"] = None
_OBSERVE_STATE["cap_pick"] = None
_OBSERVE_STATE["cap_place"] = None
_OBSERVE_STATE["last_angles"] = None
_OBSERVE_STATE["lookup"] = None
_OBSERVE_STATE["corr"] = None
_OBSERVE_STATE["captures"] = None
_OBSERVE_STATE["run_stem"] = run_stem
log_record = {
"started_at": datetime.now().isoformat(),
"dry_run": bool(dry_run),
"frames_dir": str(frames_dir),
"constants": {
"HOME": list(HOME), "SPEED": SPEED,
"BOX_XZ_FALLBACK": list(BOX_XZ),
"X_RESID_THRESH": X_RESID_THRESH,
"BOX_SCORE_THRESH": BOX_SCORE_THRESH,
"GRIPPER_SLEEP": GRIPPER_SLEEP,
"TOL_APPROACH": TOL_APPROACH,
"TOL_PICK_PLACE": TOL_PICK_PLACE,
"TOL_HOME": TOL_HOME,
"SEND_WAIT_SEC": SEND_WAIT_SEC,
"DETECT_REPEAT": DETECT_REPEAT,
"SWEPT_J0_FOR_PLACE_JUDGE": SWEPT_J0_FOR_PLACE_JUDGE,
"MAX_PICK_ATTEMPTS": MAX_PICK_ATTEMPTS,
"SWEPT_J0_FOR_PICK_RETREAT": SWEPT_J0_FOR_PICK_RETREAT,
},
"stages": [],
"detect": [],
"lookups": [],
"moves": [],
"gripper": [],
"vlm_captures": [],
"judgments": {},
"pick_attempts": [],
}
print("=" * 70)
print("[RUN] run_pick_place.py dry_run={} log={}".format(
dry_run, log_path.name))
print("=" * 70)
# 実機モードのみ daemon 接続 + fresh_mode=1
if not dry_run:
try:
get_daemon_socket()
except Exception as e:
_abort(log_record, 0, "daemon ping 失敗: {}".format(e))
_atomic_write_json(log_path, log_record)
return False, log_path
try:
daemon_set_fresh_mode(1)
print("[INIT ] fresh_mode=1")
except Exception as e:
print("[WARN ] fresh_mode=1 失敗: {}".format(e))
success = False
user_interrupted = False
try:
# 段1
_observe_phase("stage1")
if not stage1_home_and_open(log_record, dry_run):
return False, log_path
# stage1 で gripper open まで完了
_observe_gripper(0)
# 段2
_observe_phase("stage2")
block_xz, box_xz = stage2_detect_block_and_box(
log_record, dry_run, dummy_block_xz, dummy_box_xz)
if block_xz is None or box_xz is None:
return False, log_path
# 観測状態を更新（メートル/Unity 系のまま、検出値を素通し）
_OBSERVE_STATE["block_xz"] = [block_xz[0], block_xz[1]]
_OBSERVE_STATE["box_xz"] = [box_xz[0], box_xz[1]]
# 非移動 stage の通知 emit（移動なし→画面に stage2 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 段3〜判定①: 「pick 判定 verdict=no なら掴み直し」ループ（最大 MAX_PICK_ATTEMPTS 回）
# 初回ラベル接尾辞は ""（既存ログ解析と互換）、2 回目以降は "_a{attempt}"
cur_block_xz = block_xz
pick_ok = False
final_pick_judgment = None
for attempt in range(1, MAX_PICK_ATTEMPTS + 1):
suf = "" if attempt == 1 else "_a{}".format(attempt)
# 試行間補正の適用 (lookup 呼び出し直前)。
# USE_CORRECTION / KIOXIA / 記憶 が揃っているときだけ block_xz を
# 補正する。素通り時は cur_block_xz と完全一致 (回帰確認可)。
target_xz_pick, corr_meta = _apply_correction("pick", cur_block_xz)
log_record.setdefault("correction", {})["last"] = corr_meta
# 観測 corr の書き込みは stage3 phase 遷移の直後に 1 回だけ行う (下方).
# 「補正記憶を参照した stage」だけを corr に載せる方針. stage4/5/6 は
# phase 遷移で corr がクリアされたまま (実際には参照していないため).
# 段3 APPROACH_PICK
_observe_phase("stage3_approach_pick" + suf)
# _apply_correction("pick") の結果をこの stage の corr に 1 回だけ記録する.
# updated/update_reason は apply 時点では未確定 → False / None. update 決定は
# PICK 側では観測へは書き戻さない (post-stage6 の判定は phase 変わっていないため
# emit にも載らず、次 stage で phase clear されるため).
_OBSERVE_STATE["corr"] = {
"phase": "pick",
"memory_file": str(CORR_MEM_PATH),
"cell_key": corr_meta.get("cell_key"),
"applied_dx_mm": corr_meta.get("dx_mm"),
"applied_dz_mm": corr_meta.get("dz_mm"),
"target_before": list(corr_meta.get("target_before_mm") or []),
"target_after": list(corr_meta.get("target_after_mm") or []),
"apply_reason": corr_meta.get("reason") or "",
"updated": False,
"update_reason": None,
}
ok, r_app_pick = stage_lookup_and_move(
"approach_pick", target_xz_pick, "stage3_approach_pick" + suf,
TOL_APPROACH, log_record, dry_run, 3)
if not ok:
return False, log_path
approach_pick_target = list(r_app_pick["target_angles"])
# 段4 PICK descend.
# DAY277: approach で確定した j0 を descend へ渡し j0 lock
# (真下降下). approach_pick_target[0] は stage3 の fk_lookup 結果.
_observe_phase("stage4_pick" + suf)
ok, r_pick = stage_lookup_and_move(
"pick", target_xz_pick, "stage4_pick" + suf,
TOL_PICK_PLACE, log_record, dry_run, 4,
pick_j0_ref=float(approach_pick_target[0]))
if not ok:
return False, log_path
# 台帳の pick_pose_id / pick_used_j0..j5 用に lookup の pose_id と
# 直近 _do_move の post_actual を log_record へ寄せる（新規 I/O ゼロ・
# 既存 moves[-1].post_actual を再利用）。attempt loop なので最後の試行が残る。
pose_used = log_record.setdefault("pose_used", {})
pose_used["pick_pose_id"] = (r_pick.get("pose_id")
if r_pick else None)
moves = log_record.get("moves") or []
if moves:
pa = moves[-1].get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
pose_used["pick_used"] = list(pa)
# DAY277: stage4_5a_align (PICK 視覚サーボ) は本線から切り離し.
# 経緯: ran 112705 で block_x_mm=None のまま「収束」を偽装し j0 を
# -22→-34 に悪化させた事例あり. 検出 block_x が None のときの
# 収束判定に穴があり, 害になる. approach fk_lookup + descend j0
# lock (真下降下) で開ループの精度を上げる方針に移行.
# 関数定義 (_detect_tip_from_frame/_visual_servo_loop_pick/
# _run_pick_visual_servo) と定数はコミット履歴保存のため残置.
# 段4.5 爪先(青シール) vs block 差分測定（GRIP前・測定専用・動作非干渉）
# 関節は動かさず pick到達姿勢のまま上カメラ1枚を撮り、同フレームで
# 爪先中点と赤blockを検出。差分は台帳へ。失敗は None 記録で素通し。
tip_img = _capture_for_vlm(
"stage4_pick_tip" + suf, frames_dir, log_record,
dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_tip"] = (str(tip_img) if tip_img else None)
tip_diff = _measure_tip_vs_block(tip_img)
_OBSERVE_STATE["tip_pick"] = tip_diff
log_record.setdefault("tip_measurements", []).append(
{"attempt": attempt, "image": (str(tip_img) if tip_img else None),
"diff": tip_diff})
if tip_diff is not None:
print("[TIP ] attempt={} dist={:.1f}mm "
"(dx={:.1f} dz={:.1f})".format(
attempt, tip_diff["dist_mm"],
tip_diff["dx_mm"], tip_diff["dz_mm"]))
# 補正記憶の更新は、VLM 判定 + tip_dist override 後の
# final_verdict=="yes" 時のみに移動した (この下の判定ブロック後で実行)。
else:
print("[TIP ] attempt={} 測定スキップ（検出不足/前提破綻）"
.format(attempt))
# 段5 グリッパー閉
_observe_phase("stage5" + suf)
log_record["stages"].append("stage5" + suf)
grip5_ok = _do_gripper(
"close", "stage5_grip_close" + suf, log_record, dry_run)
if not grip5_ok:
# DAY265: グリッパー閉失敗しても継続 (掴めなかった事実として記録).
log_record.setdefault("stage_failures", []).append(
{"stage": 5, "label": "stage5_grip_close" + suf,
"kind": "gripper_close_fail", "attempt": attempt})
print("[STAGE 5] グリッパー閉失敗. 記録のみで次 stage へ")
_observe_gripper(1)
# 非移動 stage の通知 emit（移動なし→画面に stage5 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 段6 LIFT（その試行の approach_pick target_angles へ戻る）
_observe_phase("stage6" + suf)
log_record["stages"].append("stage6" + suf)
# C1 (DAY268 案R): stage4_pick (DESCEND) が未到達なら stage6_lift をガード.
# moves から stage4_pick の arrived を引く (stage_lookup_and_move は
# record-and-continue で ok=True を返すため ok では判定できない).
# reversed で直近の試行の move を拾う (regrasp attempt ループの正しい試行).
_pick_not_reached = False
for _m in reversed(log_record.get("moves") or []):
if str(_m.get("label", "")).startswith("stage4_pick"):
_pick_not_reached = (_m.get("arrived") is False)
break
ok, _ = _do_move(
approach_pick_target, "stage6_lift" + suf, TOL_APPROACH,
log_record, dry_run=dry_run,
abort_if_prev_not_reached=_pick_not_reached)
if not ok:
# DAY265: LIFT 到達失敗しても継続 (掴めているかは VLM が判定).
log_record.setdefault("stage_failures", []).append(
{"stage": 6, "label": "stage6_lift" + suf,
"kind": "not_reached", "attempt": attempt})
print("[STAGE 6] LIFT 未到達. 記録のみで判定へ")
# 判定①: stage4_pick_tip (リフト前) と stage6_lift 後の現フレームを撮り、
# 2 枚比較で「赤ブロックが浮いて持ち上がったか」を判定する (DAY257 後段)。
# その後 tip_dist 二重チェックで VLM yes でも dist > PICK_DIST_MAX_MM なら
# no に倒す。最終 verdict が yes のときだけ補正記憶を更新する。
vlm_label = "pick" if attempt == 1 else "pick_a{}".format(attempt)
pick_img = _capture_for_vlm(
vlm_label, frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_pick"] = (str(pick_img) if pick_img else None)
# tip_img は stage4 直後に撮影済み (L1351 付近・_OBSERVE_STATE["cap_tip"])。
# 両画像が揃っていれば 2 枚渡し、欠けていれば従来通り pick_img だけ。
pick_imgs_for_vlm = (
[tip_img, pick_img] if (tip_img is not None and pick_img is not None)
else pick_img)
pj = _judge_image(
pick_imgs_for_vlm, "pick", dry_run, dummy_pick_verdict)
vlm_verdict = pj["verdict"]
human_verdict = None
if human_judge and not dry_run:
human_verdict = _ask_human_verdict(pick_img, pj, kind="pick")
# DAY257 後段 + DAY273 案A: tip_dist 二重チェック (3 分岐).
# dist <= PICK_DIST_MAX_MM (30) → override せず VLM を信じる
# 30 < dist <= PICK_DIST_BREAKDOWN_MM (150) → override 発火 no (掴めていない)
# dist > PICK_DIST_BREAKDOWN_MM (150) → override せず VLM を信じる
# (測定破綻・DAY273 案A)
# tip 測定失敗 (tip_diff is None) のときは override 判定保留 (False).
# dist_override_suppressed は破綻値で override を無効にした事実を残す
# (台帳は 67 列ヘッダ不変・pick_attempts の JSON 側にのみ記録).
dist_override = False
dist_override_suppressed = False
final_verdict = vlm_verdict
if (vlm_verdict == "yes" and tip_diff is not None
and tip_diff.get("dist_mm") is not None):
_dist_mm = float(tip_diff["dist_mm"])
if _dist_mm > PICK_DIST_BREAKDOWN_MM:
# 破綻値: override 発火させず VLM を信じる
dist_override_suppressed = True
print("[OVERRIDE] tip_dist={:.1f}mm > {}mm 測定破綻とみなし "
"override 無効・VLM 判定 ({}) を採用".format(
_dist_mm, PICK_DIST_BREAKDOWN_MM, vlm_verdict))
elif _dist_mm > PICK_DIST_MAX_MM:
# 従来どおり掴み不良: no に倒す
final_verdict = "no"
dist_override = True
print("[OVERRIDE] vlm=yes だが tip_dist={:.1f}mm > {}mm "
"→ pick_verdict=no に倒す".format(
_dist_mm, PICK_DIST_MAX_MM))
adopted = final_verdict # 補正記憶の判定はこの final を見る
_observe_verdict("pick", adopted, pj["confidence"], pj["reason"])
log_record["pick_attempts"].append({
"attempt": attempt,
"verdict": adopted,
"vlm_verdict": vlm_verdict,
"human_verdict": human_verdict,
"dist_override": dist_override,
# DAY273 案A: 破綻値で override を無効化した事実.
# 台帳 67 列ヘッダは触らず JSON にのみ残す (後段で診断可能).
"dist_override_suppressed": dist_override_suppressed,
"confidence": pj["confidence"],
"reason": pj["reason"],
"block_xz_m": list(cur_block_xz),
"x_residual_m": r_pick["x_residual_m"],
"z_residual_m": r_pick["z_residual_m"],
})
print("[JUDGE ] pick attempt={} adopted={} (vlm={} human={} "
"dist_override={}) confidence={} reason={}".format(
attempt, adopted, vlm_verdict, human_verdict,
dist_override, pj["confidence"], pj["reason"]))
# judgments["pick"] は最終試行の dict を保存（verdict は adopted・VLM/人を併記）
final_pick_judgment = dict(pj)
final_pick_judgment["verdict"] = adopted
final_pick_judgment["vlm_verdict"] = vlm_verdict
final_pick_judgment["human_verdict"] = human_verdict
final_pick_judgment["dist_override"] = dist_override
# 補正記憶の更新: 最終 yes (掴み成功確定) かつ tip 測定成功時のみ。
# override で no に倒した試行 / VLM no / tip 測定失敗 のΔは
# 記憶に入れない (掴めていない試行で学習を汚さない)。
# DAY274 修正1 (codex ❌): dist_override_suppressed (tip_dist > 150mm の
# 測定破綻で override 無効化した試行) は adopted=yes でも tip_diff は破綻値.
# そのΔで記憶を更新すると PICK 補正が汚染される (前回 codex B1 未解消分).
# 破綻無効化した試行では PICK 補正記憶を更新しない.
# KIOXIA 未マウントや USE_CORRECTION=False 時は _corr_save がスキップ。
if (USE_CORRECTION and adopted == "yes"
and tip_diff is not None
and tip_diff.get("dx_mm") is not None
and tip_diff.get("dz_mm") is not None
and not dist_override_suppressed):
key = _corr_key("pick", cur_block_xz[0], cur_block_xz[1])
mem = _corr_load()
e_new = _corr_update(mem, key,
tip_diff["dx_mm"], tip_diff["dz_mm"])
if _corr_save(mem):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・最終 yes のみ更新)".format(
key, e_new["dx_mm"], e_new["dz_mm"],
e_new["n"], EMA_ALPHA))
if isinstance(_OBSERVE_STATE.get("corr"), dict):
_OBSERVE_STATE["corr"]["updated"] = True
_OBSERVE_STATE["corr"]["update_reason"] = None
elif (USE_CORRECTION and adopted == "yes"
and dist_override_suppressed):
# 破綻無効化で adopted=yes になった試行 (VLM yes だが tip_dist > 150mm).
# 記憶更新を skip した事実をログに残す (見えなくなる事態を防ぐ).
print("[CORR ] PICK update skip: reason=dist_override_suppressed "
"(tip_dist 測定破綻・記憶を汚染しないため学習しない)")
if isinstance(_OBSERVE_STATE.get("corr"), dict):
_OBSERVE_STATE["corr"]["updated"] = False
_OBSERVE_STATE["corr"]["update_reason"] = "dist_override_suppressed"
else:
# PICK update スキップの残り経路 (disabled / not_yes / tip_missing).
# 画面で「なぜ学習していないか」を明示するため update_reason を必ず入れる.
if isinstance(_OBSERVE_STATE.get("corr"), dict):
if not USE_CORRECTION:
_reason = "disabled"
elif adopted != "yes":
_reason = "adopted_not_yes"
else:
_reason = "tip_diff_missing"
_OBSERVE_STATE["corr"]["updated"] = False
_OBSERVE_STATE["corr"]["update_reason"] = _reason
if adopted == "yes":
pick_ok = True
break
if adopted != "no":
# unknown 等（VLM/撮影/API 失敗 or 人が unknown 入力）は「掴めていない証拠」ではない。
# ここで open すると掴めていたブロックを落とす機序があるため regrasp しない。
# ループを抜けて pick_failed 経路（HOME 復帰）へ流す。
print("[REGRASP] verdict={} は no ではないため掴み直さず終了"
.format(adopted))
break
if attempt == MAX_PICK_ATTEMPTS:
# 最終試行で yes でなければ break（外側で pick 失敗処理）
break
# ---- 掴み直し準備: open → 退避 → 再検出 ----
if not _do_gripper(
"open", "regrasp_open_a{}".format(attempt),
log_record, dry_run):
# DAY265: 掴み直しの open 失敗. 台帳に記録し掴み直し中止して外側へ抜ける.
log_record.setdefault("stage_failures", []).append(
{"stage": "regrasp_open", "attempt": attempt,
"kind": "gripper_open_fail"})
print("[REGRASP] グリッパー open 失敗. 掴み直し中止・PLACE 以降へ")
break
_observe_gripper(0)
# 退避（approach_pick base の j0 を ±85 に振る・place 退避と同形）
retreat = ([SWEPT_J0_FOR_PICK_RETREAT]
+ list(r_app_pick["base_angles"][1:]))
ok, _ = _do_move(
retreat, "regrasp_retreat_a{}".format(attempt),
TOL_APPROACH, log_record, dry_run=dry_run)
if not ok:
# 退避失敗 → アームがカメラ前に残り安全に再検出不可。break して外側 HOME 復帰へ
print("[REGRASP] 退避失敗。掴み直し中止")
break
# block のみ再検出（box は段2 初回の値を保持）
new_block_xz, _ = stage2_detect_block_and_box(
log_record, dry_run, dummy_block_xz, dummy_box_xz,
require_box=False)
if new_block_xz is None:
print("[REGRASP] 再検出でブロック無し。掴み直し中止")
break
cur_block_xz = new_block_xz
_OBSERVE_STATE["block_xz"] = [new_block_xz[0], new_block_xz[1]]
_observe_phase("stage2_redetect" + suf) # regrasp 中の再検出を履歴に出す
_observe_emit_now()
_observe_stage_record(kind="stage")
# ループ次イテレーション
# 既存キー互換: judgments["pick"] は最終試行の判定で埋める
log_record["judgments"]["pick"] = final_pick_judgment
# DAY265: pick 失敗でも PLACE 以降まで進める. 自動 HOME 復帰は無効化.
# 失敗は log_record.pick_failed / stage_failures / pick_attempts に残る.
# PLACE は空グリッパーで走り、VLM が実態 (no 等) を返す. 台帳は下段の finally
# で必ず 1 行追記される.
if not pick_ok:
log_record["pick_failed"] = {
"attempts": len(log_record.get("pick_attempts", [])),
"final_verdict": (final_pick_judgment["verdict"]
if final_pick_judgment else None),
}
log_record.setdefault("stage_failures", []).append(
{"stage": "pick_overall", "kind": "pick_failed",
"final_verdict": (final_pick_judgment.get("verdict")
if final_pick_judgment else None),
"attempts": log_record["pick_failed"]["attempts"]})
print("[RESULT] pick 失敗（{}回試行）. PLACE 以降を空グリッパーで完走"
.format(log_record["pick_failed"]["attempts"]))
# NOTE: 自動 HOME 送信は削除. エイイチが手動で HOME に戻す方針 (DAY265).
# 段7 APPROACH_PLACE（段2 で実検出した box_xz を使う。BOX_XZ 固定値は使わない）
_observe_phase("stage7_approach_place")
ok, r_app_place = stage_lookup_and_move(
"approach_place", box_xz, "stage7_approach_place",
TOL_APPROACH_PLACE, log_record, dry_run, 7)
if not ok:
return False, log_path
approach_place_target = list(r_app_place["target_angles"])
# DAY265: stage7 到達後に横カメラ (Iriun / index=1) で 1 枚撮影 (判定は行わない・
# 記録のみ). 上カメラ撮影は無し (approach_place の上視点は既存フローで撮ってない).
ap_place_side_img = _capture_for_vlm(
"ap_place_side", frames_dir, log_record, dry_run,
run_stem=run_stem, camera_role="side")
_observe_capture_push(log_record)
# 段8 PLACE降下（place_withblock。実検出 box_xz を使う）.
# DAY262: place 補正記憶を適用. pick の "last" は上書きせず "place_last" に格納.
# approach_place (段7) は raw box_xz のまま (approach は円弧概略位置決めなので補正不要).
target_xz_place, place_corr_meta = _apply_correction("place", box_xz)
# 観測 corr の書き込みは stage8 phase 遷移の直後に 1 回だけ行う (下方).
# updated/update_reason は stage9b の update 決定サイトで別途 1 回書く.
# DAY265: PLACE の FK 点選択に姿勢連続性を入れる.
# approach_place 6 軸から ±PLACE_JOINT_TOL_DEG 以内の点のみ候補にする
# (j3/j5 個別帯を全軸帯に統合. 曲げ軸 j1/j4 に歪みが逃げるのを防ぐ).
# approach_place_target 全 6 要素が float 化できるか呼び出し側で確認し、
# 1 つでも欠損なら None を渡す (fk_lookup 側型ガードでも二重に守る).
_ref_joints_place = None
try:
_rj = [float(v) for v in approach_place_target]
if len(_rj) == 6:
_ref_joints_place = _rj
except (TypeError, ValueError, IndexError):
_ref_joints_place = None
# DAY274 層2: PLACE 高さ補正記憶を引く. XZ 補正とは別記憶・独立.
# box_xz からセルキーを組み, dee_y_mm を取得. base_ee_y は fk_lookup が
# 実際に選ぶ点の ee_y を後で参照する (現行 target_ee_y なしで暫定 base 選択→
# base_ee_y + dee_y を狙う二段構成). 実装簡略化のため base_ee_y は
# (PLACE_EE_Y_MIN + PLACE_EE_Y_MAX) / 2 = 100mm を使う (帯中央・保守的).
# 帯全体は 85-115mm なので, dee_y の効果は 100mm 中央から上に稼ぐことに相当.
# 実機で「箱に当たらないラチェット学習」の期待挙動: dee_y=0 → 100mm 中央,
# dee_y=15mm → 115mm 上端付近, dee_y=+30mm → 130mm (帯外・fk は帯拡張候補も評価).
_place_hgt_mem = _height_load() if USE_CORRECTION else {}
_hgt_key = _height_key(float(box_xz[0]), float(box_xz[1]))
_dee_y_mm = _height_get_dee_y_mm(_place_hgt_mem, _hgt_key)
_base_ee_y_m = 0.5 * (PLACE_EE_Y_MIN + PLACE_EE_Y_MAX) # 100mm 帯中央
# target_ee_y は None で「層3 無効」・dee_y>0 で有効化. 焼き付け回避.
_target_ee_y_m = (_base_ee_y_m + _dee_y_mm / 1000.0
if _dee_y_mm > 0.0 else None)
if _dee_y_mm > 0.0:
print("[HGT ] PLACE apply: cell={} dee_y={:.1f}mm "
"→ target_ee_y={:.4f}m".format(
_hgt_key, _dee_y_mm, _target_ee_y_m))
# DAY274: 補正が適用された場合, 補正後 target で fk_lookup を事前検査し
# joint_ok=True かつ j4 が [J4_SAFE_MIN, J4_SAFE_MAX] 内にあることを確認.
# 満たさなければ補正を破棄し raw box_xz で降下 (servo が深い j4 領域=
# swept_retreat スタール帯へ補正で押し込まれるのを防ぐ). 破綻ガードとは別レイヤー.
if place_corr_meta.get("applied"):
_corr_safety_ok = False
_corr_safety_note = ""
try:
_r_safety = fk_lookup(
"place_withblock", target_xz_place,
ee_y_min=PLACE_EE_Y_MIN, ee_y_max=PLACE_EE_Y_MAX,
ref_joints=_ref_joints_place,
joint_tol_deg=PLACE_JOINT_TOL_DEG,
place_ee_y_floor=PLACE_EE_Y_FLOOR,
place_j1_min_deg=PLACE_J1_MIN_DEG,
target_ee_y_m=_target_ee_y_m)
_tgt_angles = _r_safety.get("target_angles") or []
if len(_tgt_angles) == 6:
_tgt_j4 = float(_tgt_angles[4])
_joint_ok = bool(_r_safety.get("joint_limit_ok"))
if not _joint_ok:
_corr_safety_note = "joint_limit_ok=False"
elif not (PLACE_CORR_J4_SAFE_MIN <= _tgt_j4
<= PLACE_CORR_J4_SAFE_MAX):
_corr_safety_note = ("j4={:+.2f} out of [{}, {}]"
.format(_tgt_j4,
PLACE_CORR_J4_SAFE_MIN,
PLACE_CORR_J4_SAFE_MAX))
else:
_corr_safety_ok = True
else:
_corr_safety_note = "target_angles 不正 (len!=6)"
except Exception as e:
# DAY274 修正3 (codex ⚠️): fk_lookup 事前検査の失敗は全て安全側
# (補正破棄・raw box_xz で進む) に倒す. Exception は BaseException
# 直系の KeyboardInterrupt / SystemExit を除外するので, Ctrl+C と
# 正常終了は生きる (握りつぶさない). 想定外例外での中途停止を防ぐ.
_corr_safety_note = "fk_precheck_exc:" + type(e).__name__
if not _corr_safety_ok:
# rollback: 素の box target に戻し, meta もリセット + フラグ残す.
# DAY274 原則1: cell_key は rollback でも残す (どのセルの試行かは
# 補正の成否と無関係な事実). box_xz からセルキー算出し記録.
print("[CORR ] PLACE 補正後 target が不安全 ({}). 補正を破棄し "
"raw box_xz で降下.".format(_corr_safety_note))
_bx_raw = float(box_xz[0])
_bz_raw = float(box_xz[1])
target_xz_place = (_bx_raw, _bz_raw)
place_corr_meta = {
"phase": "place",
"target_before_mm": (_bx_raw * 1000.0, _bz_raw * 1000.0),
"target_after_mm": (_bx_raw * 1000.0, _bz_raw * 1000.0),
"cell_key": _corr_key("place", _bx_raw, _bz_raw),
"dx_mm": None, "dz_mm": None, "n": 0,
"applied": False,
"safety_rollback": True,
"safety_reason": _corr_safety_note,
"reason": "safety_rollback",
}
log_record.setdefault("correction", {})["place_last"] = place_corr_meta
# DAY274 層2 meta を log_record に格納 (台帳・ラチェット更新側で使う).
log_record.setdefault("correction", {})["place_height_last"] = {
"cell_key": _hgt_key,
"dee_y_mm": _dee_y_mm,
"target_ee_y_m": _target_ee_y_m,
}
# DAY278: place_withblock (descend_place) に approach_place の j0 を
# place_j0_ref として渡し, 「真上→真下」を成立させる (A_PICK と同型).
# approach_place_target は stage7 の fk_lookup 結果.
_observe_phase("stage8_place")
# _apply_correction("place") の結果をこの stage の corr に 1 回だけ記録する.
# place_corr_meta は上流で rollback された場合もその値を反映済み (safety fail 時は
# target_before == target_after で reason に "rollback_*" 相当が入る).
# updated/update_reason は stage9b の update 決定サイトで別途書く.
_OBSERVE_STATE["corr"] = {
"phase": "place",
"memory_file": str(CORR_MEM_PATH),
"cell_key": place_corr_meta.get("cell_key"),
"applied_dx_mm": place_corr_meta.get("dx_mm"),
"applied_dz_mm": place_corr_meta.get("dz_mm"),
"target_before": list(place_corr_meta.get("target_before_mm") or []),
"target_after": list(place_corr_meta.get("target_after_mm") or []),
"apply_reason": place_corr_meta.get("reason") or "",
"updated": False,
"update_reason": None,
}
ok, r_place = stage_lookup_and_move(
"place_withblock", target_xz_place, "stage8_place",
TOL_PICK_PLACE, log_record, dry_run, 8,
ref_joints=_ref_joints_place,
joint_tol_deg=PLACE_JOINT_TOL_DEG,
target_ee_y_m=_target_ee_y_m,
place_j0_ref=float(approach_place_target[0]))
# DAY274 層2: stage8 の fk_lookup が実際に選んだ j4 を記録 (台帳
# place_height_j4_selected 用). r_place=None (失敗) は j4=None のまま.
if isinstance(r_place, dict):
_j4_sel = r_place.get("j4_selected_deg")
log_record["correction"]["place_height_last"]["j4_selected_deg"] = (
float(_j4_sel) if isinstance(_j4_sel, (int, float)) else None)
if not ok:
return False, log_path
# ========== stage 8.5 (DAY272 案T): PLACE 再照準 =====================
# stage8 到達後・stage9 リリース前に上カメラで保持ブロック X と箱中心 X
# のズレを j0 だけで締める. Z は触らない (DAY200 地雷回避).
# PICK 視覚サーボの複製移植 (visual_servo_pick.visual_servo_loop 相当).
# PICK 本線関数は 1 バイトも改変しない. 補正記憶とは独立の実時間制御なので
# 計測モード PLACE_CORR_MEASURE_MODE=True でも動く. 失敗時は制御停止せず
# stage9 リリースへ進む (fire-and-forget 相当). place_pre 撮影より前で
# 実施するので, 補正学習の入力は「再照準後の実際の落下位置」になる.
_observe_phase("stage8_5_replace_align")
log_record["stages"].append("stage8_5_replace_align")
_run_place_visual_servo(box_xz, log_record, dry_run)
# DAY265: stage8 到達後に横カメラで 1 枚撮影し PLACE 状態を VLM 判定.
# 上カメラの既存 place 判定 (下段の stage9b で撮影/判定) は無変更・並列に追加.
# 姿勢は stage8 到達姿 (箱内降下位置) のまま. 送角/グリッパー変更なし (観測のみ).
place_side_img = _capture_for_vlm(
"place_side", frames_dir, log_record, dry_run,
run_stem=run_stem, camera_role="side")
_observe_capture_push(log_record)
if place_side_img is not None and not dry_run:
side_pj = _judge_image(
place_side_img, "place_side", dry_run, None)
log_record.setdefault("judgments", {})["place_side"] = side_pj
print("[JUDGE ] place_side: verdict={} confidence={} reason={}".format(
side_pj.get("verdict"), side_pj.get("confidence"),
side_pj.get("reason")))
_observe_verdict("side_place", side_pj.get("verdict"),
side_pj.get("confidence"), side_pj.get("reason"))
else:
# 撮影失敗 / dry_run のとき judge は呼ばない. 台帳側は空欄で入る.
log_record.setdefault("judgments", {})["place_side"] = {
"verdict": "unknown", "confidence": 0.0,
"reason": ("dry_run" if dry_run else "capture_failed"),
"model": None, "image": None, "raw": None}
# 台帳の place_pose_id / place_used_j0..j5 用に lookup の pose_id と
# 直近 _do_move の post_actual を寄せる（離す直前の関節角。新規 I/O ゼロ）。
pose_used = log_record.setdefault("pose_used", {})
pose_used["place_pose_id"] = (r_place.get("pose_id")
if r_place else None)
moves = log_record.get("moves") or []
if moves:
pa = moves[-1].get("post_actual")
if isinstance(pa, (list, tuple)) and len(pa) == 6:
pose_used["place_used"] = list(pa)
# DAY262 (仕様1): 補正記憶の学習入力を「離す前」に測る (置きズレ純度を上げる).
# stage9 open より前・stage8 到達姿勢のまま上カメラで1枚撮影しブロック位置を
# 検出し block_place_pre_xz へ. dry_run 時は _capture_for_vlm が None を返し、
# _detect_block_in_place_img も None を返すので自動で学習 skip.
# 送角/グリッパー/sleep は追加しない (fire-and-forget の観測処理).
# 保存先: frames_dir / "{run_stem}_place_pre.jpg".
# VLM 成否判定 (stage9b) は退避後 place.jpg のまま・触らない.
place_pre_img = _capture_for_vlm(
"place_pre", frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
block_place_pre_xz = _detect_block_in_place_img(place_pre_img)
if block_place_pre_xz is not None:
print("[PLACE_PRE] block xz_m=({:+.4f},{:+.4f})".format(
block_place_pre_xz[0], block_place_pre_xz[1]))
else:
print("[PLACE_PRE] block 検出失敗 (補正学習は skip・制御は継続)")
# 段9 グリッパー開（離す）
_observe_phase("stage9")
log_record["stages"].append("stage9")
stage9_ok = _do_stage9_release_gripper(log_record, dry_run)
if not stage9_ok:
# DAY265: 離す動作失敗しても継続. 台帳/JSON に記録.
log_record.setdefault("stage_failures", []).append(
{"stage": 9, "label": "stage9_release", "kind": "gripper_open_fail"})
print("[STAGE 9] グリッパー離す失敗. 記録のみで判定へ")
_observe_gripper(0)
# 非移動 stage の通知 emit（移動なし→画面に stage9 行を出す）
_observe_emit_now()
_observe_stage_record(kind="stage")
# 判定②: 段9 と段10 の間に挟む。退避（j0=+85, approach_place base）→ 撮影 → 判定。
# 退避移動が沈黙/ABORT → place 判定 skip・ログ記録のみ。stage 10 (HOME 復帰) は続行。
_observe_phase("stage9b_judge_place")
log_record["stages"].append("stage9b_judge_place")
swept_target = [SWEPT_J0_FOR_PLACE_JUDGE] + list(r_app_place["base_angles"][1:])
ok_swept, _ = _do_move(
swept_target, "stage9b_swept_retreat", TOL_SWEPT_RETREAT,
log_record, dry_run=dry_run)
if not ok_swept:
log_record["judgments"]["place"] = {
"verdict": "unknown", "confidence": 0.0,
"reason": "swept_retreat_failed", "model": None,
"image": None, "raw": None,
}
_observe_verdict("place", "unknown", 0.0, "swept_retreat_failed")
# 非移動 stage の通知 emit（stage9b 失敗ブランチ→画面に verdict=unknown 行）
_observe_emit_now()
_observe_stage_record(kind="stage")
print("[JUDGE ] place: swept_retreat 失敗のため判定スキップ。"
"HOME 復帰へ続行")
else:
place_img = _capture_for_vlm(
"place", frames_dir, log_record, dry_run, run_stem=run_stem)
_observe_capture_push(log_record)
_OBSERVE_STATE["cap_place"] = (str(place_img) if place_img else None)
# 観測: 同じ画像に赤ブロック HSV 検出を脇でかける（既存判定/分岐は不変）
_OBSERVE_STATE["block_place_xz"] = _detect_block_in_place_img(place_img)
# DAY274: 同じ画像に白箱検出も脇でかける (stage2 と同一検出器 06f).
# 撮影・送角・グリッパー・sleep は追加しない (画像は既に手元にある).
# 検出失敗 (None) は _append_ledger 側で box_shift 3 列を空欄にする.
_OBSERVE_STATE["box_xz_last"] = _detect_white_box_in_place_img(place_img)
# DAY274 層2: box_shift_dist を計算し, 高さ補正記憶をラチェット更新.
# box_xz (stage2 検出) と box_xz_last (退避後) 両方揃った時のみ.
# 検出失敗 (None) や box_xz 欠損時は更新スキップ (現状維持).
# PLACE 補正 (XZ) と独立記憶 (別 json). USE_CORRECTION=False で完全停止.
_bxl = _OBSERVE_STATE.get("box_xz_last")
_box_shift_dist_mm = None
if (USE_CORRECTION
and isinstance(box_xz, (list, tuple)) and len(box_xz) == 2
and isinstance(box_xz[0], (int, float))
and isinstance(box_xz[1], (int, float))
and isinstance(_bxl, (list, tuple)) and len(_bxl) == 2
and isinstance(_bxl[0], (int, float))
and isinstance(_bxl[1], (int, float))):
_dx = (float(_bxl[0]) - float(box_xz[0])) * 1000.0
_dz = (float(_bxl[1]) - float(box_xz[1])) * 1000.0
_box_shift_dist_mm = (_dx * _dx + _dz * _dz) ** 0.5
try:
_hmem = _height_load()
_hkey = _height_key(float(box_xz[0]), float(box_xz[1]))
_new_entry = _height_update_ratchet(
_hmem, _hkey, _box_shift_dist_mm)
if _height_save(_hmem):
print("[HGT ] update {} dist={:.1f}mm → dee_y={:.1f}mm "
"(n={})".format(
_hkey, _box_shift_dist_mm,
_new_entry.get("dee_y_mm", 0.0),
_new_entry.get("n", 0)))
except Exception as e:
print("[HGT ] [WARN] ratchet update 例外: {}: {}".format(
type(e).__name__, e))
place_vlm = _judge_image(
place_img, "place", dry_run, dummy_place_verdict)
place_vlm_verdict = place_vlm["verdict"]
place_human_verdict = None
if human_judge and not dry_run:
place_human_verdict = _ask_human_verdict(
place_img, place_vlm, kind="place")
place_adopted = place_vlm_verdict # 分岐は常に VLM。記録のみ人
_observe_verdict("place", place_adopted, place_vlm["confidence"], place_vlm["reason"])
# 非移動 stage の通知 emit（stage9b 成功ブランチ→画面に verdict 行）
_observe_emit_now()
_observe_stage_record(kind="stage")
place_judgment = dict(place_vlm)
place_judgment["verdict"] = place_adopted
place_judgment["vlm_verdict"] = place_vlm_verdict
place_judgment["human_verdict"] = place_human_verdict
log_record["judgments"]["place"] = place_judgment
print("[JUDGE ] place: adopted={} (vlm={} human={}) "
"confidence={} reason={}".format(
place_adopted, place_vlm_verdict, place_human_verdict,
place_vlm["confidence"], place_vlm["reason"]))
# DAY274 案: PLACE 補正記憶の起動 (PICK と対称移植).
# 更新条件: 最終 pick_verdict == "yes" の全試行 (place verdict 非依存).
# 掴めていれば「置きズレ」は有効な学習情報で、外した試行こそズレ方向を持つ.
# place=yes 限定にすると学習が始まらない (swept_retreat 抜けで判定 skip も
# 起きるため).
# 破綻ガード 3 種 (どれかに当たったら update skip・apply は通常どおり動く):
# 1. block_place_pre_xz is None (検出失敗)
# 2. sign(block_place_x) != sign(box_x) かつ abs(block_place_x - box_x)
# > PLACE_CORR_SIGN_FLIP_GAP_MM (150mm) → PICK 残像/別物拾い
# 3. dist(block_place_xz, box_xz) > PLACE_CORR_MAX_DIST_MM (120mm) → 外れ値
# Δ = (離す前 - 箱) = 「実測 - 目標」・PICK と符号整合.
# EMA α=0.5 + 1 更新 ±15mm クランプ は _corr_update 側で PICK と共通に効く.
# KIOXIA 未マウント時は _corr_save がスキップし自動素通り.
# DAY268 手0 (案S) の measure mode 分岐は互換のため残置 (通常 False 運用).
# DAY274 原則2: update 各経路で reason を place_last meta に必ず埋める.
# 判定ロジックは無改変. reason を log_record.correction.place_last に追記するだけ.
_place_update_reason = None
if PLACE_CORR_MEASURE_MODE:
print("[CORR ] PLACE measure mode: apply/update skipped")
_place_update_reason = "measure_mode"
elif not USE_CORRECTION:
_place_update_reason = "disabled" # correction 完全無効時は素通り
else:
# 更新条件: 最終 pick_verdict == "yes" (override 後の adopted を採用).
_final_pick_verdict = (
final_pick_judgment.get("verdict")
if isinstance(final_pick_judgment, dict) else None)
if _final_pick_verdict != "yes":
print("[CORR ] PLACE update skip: reason=pick_not_yes "
"(final_verdict={})".format(_final_pick_verdict))
_place_update_reason = "pick_not_yes"
elif (block_place_pre_xz is None
or not isinstance(block_place_pre_xz, (list, tuple))
or len(block_place_pre_xz) != 2
or block_place_pre_xz[0] is None
or block_place_pre_xz[1] is None):
# 破綻ガード 1: 検出失敗.
print("[CORR ] PLACE update skip: reason=detect_none")
_place_update_reason = "detect_none"
elif (not isinstance(box_xz, (list, tuple))
or len(box_xz) != 2
or box_xz[0] is None or box_xz[1] is None):
print("[CORR ] PLACE update skip: reason=box_xz_none")
_place_update_reason = "box_xz_none"
else:
_bp_x_mm = float(block_place_pre_xz[0]) * 1000.0
_bp_z_mm = float(block_place_pre_xz[1]) * 1000.0
_bx_mm = float(box_xz[0]) * 1000.0
_bz_mm = float(box_xz[1]) * 1000.0
_dx_mm_place = _bp_x_mm - _bx_mm
_dz_mm_place = _bp_z_mm - _bz_mm
_dist_mm = math.hypot(_dx_mm_place, _dz_mm_place)
# 破綻ガード 2: sign flip かつ 大ギャップ (>150mm) → PICK 残像判定.
_sign_flip = (
((_bp_x_mm >= 0.0) != (_bx_mm >= 0.0))
and abs(_bp_x_mm - _bx_mm) > PLACE_CORR_SIGN_FLIP_GAP_MM)
# 破綻ガード 3: 距離が閾値超 → 外れ値.
_dist_over = (_dist_mm > PLACE_CORR_MAX_DIST_MM)
if _sign_flip:
print("[CORR ] PLACE update skip: reason=sign_flip "
"block_x={:+.1f} box_x={:+.1f} gap={:.1f}mm > {:.0f}mm"
.format(_bp_x_mm, _bx_mm,
abs(_bp_x_mm - _bx_mm),
PLACE_CORR_SIGN_FLIP_GAP_MM))
_place_update_reason = "sign_flip"
elif _dist_over:
print("[CORR ] PLACE update skip: reason=dist_over "
"dist={:.1f}mm > {:.0f}mm"
.format(_dist_mm, PLACE_CORR_MAX_DIST_MM))
_place_update_reason = "dist_over"
else:
key_place = _corr_key("place", box_xz[0], box_xz[1])
mem_place = _corr_load()
e_new_place = _corr_update(
mem_place, key_place, _dx_mm_place, _dz_mm_place)
if _corr_save(mem_place):
print("[CORR ] mem key={} dx={:+.2f} dz={:+.2f} n={} "
"(EMA α={}・pick=yes 全試行・dist={:.1f}mm)".format(
key_place, e_new_place["dx_mm"],
e_new_place["dz_mm"], e_new_place["n"],
EMA_ALPHA, _dist_mm))
_place_update_reason = "updated"
# place_last.update_reason に記録 (判定はしない・記録だけ). ledger で参照.
_pl = log_record.setdefault("correction", {}).get("place_last")
if isinstance(_pl, dict) and _place_update_reason is not None:
_pl["update_reason"] = _place_update_reason
# 観測: _corr_update / update skip の判定が確定した箇所で corr を 1 回だけ書く.
# updated / update_reason を含めた完全な dict を書き直す (mutation ではなく再構築).
# apply 情報は place_corr_meta から復元. この時点で phase は stage9b_judge_place の
# まま (stage10 遷移前), 直近の emit は判定分岐内で完了済み. ここで書いた corr は
# 次 emit を伴わないが「実際に _corr_update / skip を判断した stage」を示す情報として
# _OBSERVE_STATE に残る.
if isinstance(place_corr_meta, dict):
_OBSERVE_STATE["corr"] = {
"phase": "place",
"memory_file": str(CORR_MEM_PATH),
"cell_key": place_corr_meta.get("cell_key"),
"applied_dx_mm": place_corr_meta.get("dx_mm"),
"applied_dz_mm": place_corr_meta.get("dz_mm"),
"target_before": list(place_corr_meta.get("target_before_mm") or []),
"target_after": list(place_corr_meta.get("target_after_mm") or []),
"apply_reason": place_corr_meta.get("reason") or "",
"updated": (_place_update_reason == "updated"),
"update_reason": (None if _place_update_reason == "updated"
else _place_update_reason),
}
# 段10 lift_back → HOME. DAY265 で自動 HOME 復帰を全撤廃した後、
# 正常完走フロー末尾の HOME 復帰まで失われていたため復活 (2026-07-03 追記).
# 区別: 「失敗した瞬間に勝手に HOME」(abort_recover_home / pickfail_to_home) は
# 撤廃したまま. 「通しを走り切った最後に HOME で締める」は残す.
# 失敗完走 (stage_failures 混じり) でも stage10 まで到達したらここで HOME に戻る.
_observe_phase("stage10")
log_record["stages"].append("stage10")
# C1 (DAY268 案R): stage8_place (PLACE 降下) が未到達なら stage10_lift_back を
# ガード. moves から stage8_place の arrived を引く (stage_lookup_and_move は
# record-and-continue で ok=True を返すため ok では判定できない).
_place_not_reached = False
for _m in reversed(log_record.get("moves") or []):
if str(_m.get("label", "")).startswith("stage8_place"):
_place_not_reached = (_m.get("arrived") is False)
break
ok, _ = _do_move(
approach_place_target, "stage10_lift_back", TOL_LIFT_BACK,
log_record, dry_run=dry_run,
abort_if_prev_not_reached=_place_not_reached)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 10, "label": "stage10_lift_back", "kind": "not_reached"})
print("[STAGE 10] lift_back 未到達. 記録のみで HOME 締めへ")
# 通し末尾の HOME 締め. HOME=[0,0,0,0,0,-30] (SPEED=20 は _do_move / daemon 側).
# arrived=False でも記録のみで完走判定へ進める (record-and-continue).
ok, _ = _do_move(
HOME, "stage10_to_home", TOL_HOME,
log_record, dry_run=dry_run)
if not ok:
log_record.setdefault("stage_failures", []).append(
{"stage": 10, "label": "stage10_to_home", "kind": "not_reached"})
print("[STAGE 10] HOME 未到達. 記録のみで完走判定へ")
# 通し完走. 途中で stage_failures があれば result にその旨を残す.
stage_failures = log_record.get("stage_failures") or []
if stage_failures:
log_record["result"] = "completed_with_failures"
print("[RESULT] 完走 (失敗 {} 件を含む)".format(len(stage_failures)))
else:
log_record["result"] = "completed"
print("[RESULT] 完走")
success = True
return True, log_path
except KeyboardInterrupt:
user_interrupted = True
_abort(log_record, log_record["stages"][-1] if log_record["stages"] else 0,
"KeyboardInterrupt")
print("[INTERRUPT] Ctrl-C 検出。ログを保存して終了")
return False, log_path
finally:
# DAY265: 自動 HOME 復帰は無効化. エイイチが目視で判断して手動で HOME に戻す.
# 途中失敗でも通しを完走させ 1 行追記させるため、finally での自動送信は行わない.
log_record["home_recovery"] = {
"attempted": False, "note": "auto_home_recover_disabled_DAY265"}
log_record["ended_at"] = datetime.now().isoformat()
if not success and "result" not in log_record:
log_record["result"] = "aborted"
if not dry_run:
try:
daemon_set_fresh_mode(0)
except Exception:
pass
try:
close_daemon_socket()
except Exception:
pass
try:
_atomic_write_json(log_path, log_record)
except Exception as e:
print("[WARN ] ログ書き出し失敗: {}".format(e))
print("[LOG ] {}".format(log_path))
# 試行台帳 CSV を KIOXIA SSD に追記（fire-and-forget・未マウントなら skip）.
# 既存 JSON 出力は維持・CSV は別物.
# dry_run の試行は実機データではないため台帳に残さない（CSV を清浄に保つ）.
if not dry_run:
_append_ledger(log_record, run_stem)
def main():
ap = argparse.ArgumentParser(
description="通し Pick & Place（j0 対応表方式）")
ap.add_argument(
"--dry-run", action="store_true",
help="送信せず、各段の lookup 結果とガード判定だけを出力",
)
ap.add_argument(
"--dummy-block-x", type=float, default=None,
help="dry_run 用 block_xz の x（指定時は --dummy-block-z とセット）",
)
ap.add_argument(
"--dummy-block-z", type=float, default=None,
help="dry_run 用 block_xz の z（指定時は --dummy-block-x とセット）",
)
ap.add_argument(
"--dummy-box-x", type=float, default=None,
help="dry_run 用 box_xz の x（指定時は --dummy-box-z とセット）。"
"未指定なら BOX_XZ={} をデフォルト".format(BOX_XZ),
)
ap.add_argument(
"--dummy-box-z", type=float, default=None,
help="dry_run 用 box_xz の z（指定時は --dummy-box-x とセット）",
)
ap.add_argument(
"--dummy-pick-verdict", choices=["yes", "no", "unknown"], default=None,
help="dry_run 用 pick 判定の verdict 注入（デフォルト unknown）",
)
ap.add_argument(
"--dummy-place-verdict", choices=["yes", "no", "unknown"], default=None,
help="dry_run 用 place 判定の verdict 注入（デフォルト unknown）",
)
ap.add_argument(
"--human-judge", action="store_true",
help="pick/place 判定で人による目視判定を入力させる（dry_run 時は無効）。"
"校正フェーズ向け。VLM 判定は記録のみで分岐は人の verdict を採用",
)
args = ap.parse_args()
dummy_block = None
dummy_box = None
if args.dry_run:
if args.dummy_block_x is not None and args.dummy_block_z is not None:
dummy_block = (args.dummy_block_x, args.dummy_block_z)
else:
dummy_block = (0.05, 0.30)
print("[INIT ] dry_run dummy block_xz default = {}".format(dummy_block))
if args.dummy_box_x is not None and args.dummy_box_z is not None:
dummy_box = (args.dummy_box_x, args.dummy_box_z)
else:
dummy_box = tuple(BOX_XZ)
print("[INIT ] dry_run dummy box_xz default (BOX_XZ) = {}".format(dummy_box))
success, log_path = run(
dry_run=args.dry_run,
dummy_block_xz=dummy_block,
dummy_box_xz=dummy_box,
dummy_pick_verdict=args.dummy_pick_verdict,
dummy_place_verdict=args.dummy_place_verdict,
human_judge=args.human_judge,
)
sys.exit(0 if success else 1)
if __name__ == "__main__":
main()observe/emit.py
observe/emit.py — datagram 定義。lookup / corr / captures の3キーを受け口に追加。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""observe/emit.py — UDP fire-and-forget テレメトリ送信
制御ループからの観測値を localhost UDP 5601 に「1 datagram = 1 JSON 行」で投げる。
**fire-and-forget・例外を全て握る・制御の振る舞いに絶対干渉しない**。
スキーマ（ステップ A の keeper 側と完全一致）:
{"t": <unix_ms:int>, "seq": <int>, "source": "control_loop"|...,
"angles": [j1..j6]|null, "ee": [x,y,z]|null,
"gripper": 0|1|null, "phase": <str>|null, "phase_t0": <ms>|null,
"trial": {"id": <str>, "task": <str>}|null,
"verdict": <dict>|null,
"block_xz": [x,z]|null, "box_xz": [x,z]|null,
"block_place_xz": [x,z]|null,
"in_box": true|false|null,
"tip_pick": <dict>|null, "run_dir": <str>|null,
"lookup": <dict>|null, # 参照元 + 選択理由（_record_lookup の rec）
"corr": <dict>|null, # 補正記憶の適用/更新（PICK/PLACE）
"captures": <list>|null, # その stage で撮影した画像リスト（vlm_captures の直近分）
# 要素は {label, camera_role, image}
}
設計:
- ソケットはモジュールロードで 1 個生成して使い回す（毎回 open しない）
- seq はモジュール内グローバルで単調増加、emit ごとに +1
- どの例外も raise しない（制御を絶対に止めない）
- localhost のみ。外部公開しない
"""
from __future__ import print_function
import json
import socket
import time
_UDP_HOST = "127.0.0.1"
_UDP_PORT = 5601
# ソケットはモジュールロードで 1 個生成して使い回す。
# 失敗しても emit() は raise しない（None チェックで握る）。
# nonblocking: 送信バッファ満杯時に sendto がブロックして制御ループを止めないため。
# 取りこぼした datagram は次の emit で回復する（観測の最新主義）。
try:
_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
_sock.setblocking(False)
except Exception:
_sock = None
# seq はモジュール内グローバル。プロセス開始から単調増加。
# telemetry_probe.py との競合は source 識別子で見分ける運用。
_seq = 0
def emit(angles=None, phase=None, phase_t0_ms=None, gripper=None,
ee=None, trial=None, verdict=None, block_xz=None, box_xz=None,
block_place_xz=None, in_box=None,
tip_pick=None, run_dir=None,
cap_tip=None, cap_pick=None, cap_place=None,
lookup=None, corr=None, captures=None, source="control_loop"):
"""テレメトリ 1 発を UDP 5601 へ送る。
全例外を握る・raise しない・制御を 1 ミリ秒も止めない。
UDP 送信失敗・JSON 化失敗・ソケット未生成、どれでも例外を投げない。
"""
global _seq
try:
_seq += 1
payload = {
"t": int(time.time() * 1000),
"seq": _seq,
"source": source,
"angles": list(angles) if angles is not None else None,
"ee": list(ee) if ee is not None else None,
"gripper": gripper,
"phase": phase,
"phase_t0": phase_t0_ms,
"trial": trial,
"verdict": verdict,
"block_xz": list(block_xz) if block_xz is not None else None,
"box_xz": list(box_xz) if box_xz is not None else None,
"block_place_xz": list(block_place_xz) if block_place_xz is not None else None,
"in_box": in_box,
"tip_pick": tip_pick if tip_pick is not None else None,
"run_dir": run_dir,
"cap_tip": cap_tip,
"cap_pick": cap_pick,
"cap_place": cap_place,
"lookup": lookup,
"corr": corr,
"captures": captures,
}
if _sock is None:
return
data = (json.dumps(payload) + "\n").encode("utf-8")
_sock.sendto(data, (_UDP_HOST, _UDP_PORT))
except Exception:
# 制御を絶対に止めない。失敗は黙って捨てる
passobserve_server.py
observe_server.py — UDP 受信 → WebSocket 配信。最新1個を保持する _latest 構造。本 log の対象期間では不変（記録の正は emitter 側 JSONL へ移したため、配信は従来のまま）。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""observe_server.py — keeper: UDP 受信 → WebSocket push
UDP 5601 で telemetry datagram を受け、**最新 1 個だけ** 保持。
WebSocket /ws で接続中ブラウザに ~100ms 間隔で最新値を push。
設計の性質:
- UDP 受信は fire-and-forget。古い datagram は捨てる（最新主義）
- 受信が無くてもタイマで現在時刻＋最後の値を送り、ブラウザが「固まり」を識別できる
- 全バインド **localhost 限定**。外部公開しない
- daemon を触らない・制御ループを触らない
依存（個別 pip）:
pip install fastapi uvicorn
CLI:
python3 observe_server.py
→ http://127.0.0.1:8000/ で UI
"""
import asyncio
import json
import socket
import sys
import time
from pathlib import Path
try:
from fastapi import FastAPI, WebSocket, WebSocketDisconnect
from fastapi.responses import FileResponse, JSONResponse
import uvicorn
except ImportError as e:
print("[ERROR] 依存パッケージが無い: {}".format(e), file=sys.stderr)
print(" pip install fastapi uvicorn", file=sys.stderr)
sys.exit(1)
# ---- 定数 -------------------------------------------------------------------
HTTP_HOST = "127.0.0.1"
HTTP_PORT = 8000
UDP_LISTEN_HOST = "127.0.0.1"
UDP_LISTEN_PORT = 5601
WS_PUSH_INTERVAL_SEC = 0.1
_STATIC_DIR = Path(__file__).resolve().parent / "static"
# Phase 2: run 一覧と telemetry.jsonl 読み出し用（読み取り専用）.
# 実行時 run のディレクトリは KIOXIA 配下. 未マウント時は空配列を返す.
LOG_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")
_TELEMETRY_JSONL_NAME = "telemetry.jsonl"
# ---- 最新値ストア（プロセス内グローバル）-----------------------------------
_latest = {"datagram": None, "received_at_ms": None}
# ---- UDP listener（バックグラウンドタスク）---------------------------------
async def _udp_listener():
"""UDP 5601 を非同期で受信し続け、_latest を更新。
例外は記録のみ。listener は落ちないことを優先する。"""
loop = asyncio.get_event_loop()
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.setblocking(False)
try:
sock.bind((UDP_LISTEN_HOST, UDP_LISTEN_PORT))
except OSError as e:
print("[SRV] UDP bind 失敗 {}:{}: {}".format(
UDP_LISTEN_HOST, UDP_LISTEN_PORT, e), file=sys.stderr)
return
print("[SRV] UDP listen {}:{}".format(UDP_LISTEN_HOST, UDP_LISTEN_PORT))
while True:
try:
data, _addr = await loop.sock_recvfrom(sock, 65535)
except Exception as e:
print("[SRV] UDP recv 例外: {}".format(e), file=sys.stderr)
await asyncio.sleep(0.1)
continue
try:
line = data.decode("utf-8").strip()
payload = json.loads(line)
except Exception as e:
print("[SRV] UDP parse 例外: {} raw={!r}".format(e, data[:200]),
file=sys.stderr)
continue
_latest["datagram"] = payload
_latest["received_at_ms"] = int(time.time() * 1000)
# ---- FastAPI ----------------------------------------------------------------
app = FastAPI()
@app.on_event("startup")
async def _startup():
asyncio.create_task(_udp_listener())
@app.get("/")
def index():
html_path = _STATIC_DIR / "index.html"
if not html_path.exists():
return JSONResponse(
status_code=500,
content={"error": "static/index.html not found",
"path": str(html_path)})
return FileResponse(str(html_path), media_type="text/html")
@app.get("/runs")
def list_runs():
"""run 一覧（新しい順・最大 50）。KIOXIA 未マウント時は空配列を返す.
telemetry.jsonl の有無と行数（= STAGE 数）だけ返す. 中身は読まない.
UDP/WS には一切影響しない読み取り専用エンドポイント."""
try:
if not LOG_ROOT.exists():
return {"runs": [], "mounted": False}
items = []
for d in sorted(LOG_ROOT.iterdir(), reverse=True):
if not d.is_dir():
continue
jl = d / _TELEMETRY_JSONL_NAME
has_jl = jl.exists()
stages = 0
if has_jl:
try:
with open(jl, "r", encoding="utf-8") as f:
stages = sum(1 for _ in f)
except Exception:
stages = 0
items.append({
"stem": d.name,
"has_telemetry": has_jl,
"stages": stages,
})
if len(items) >= 50:
break
return {"runs": items, "mounted": True}
except Exception as e:
return JSONResponse(status_code=500, content={"error": str(e)})
@app.get("/runs/{stem}/stages")
def get_stages(stem: str):
"""telemetry.jsonl を JSON 配列で返す（記録の正）.
パストラバーサル防止のため stem に区切り文字は許さない."""
if "/" in stem or "\\" in stem or ".." in stem:
return JSONResponse(status_code=400, content={"error": "bad stem"})
jl = LOG_ROOT / stem / _TELEMETRY_JSONL_NAME
if not jl.exists():
return JSONResponse(status_code=404,
content={"error": "no telemetry", "stem": stem})
rows = []
try:
with open(jl, "r", encoding="utf-8") as f:
for ln in f:
ln = ln.strip()
if ln:
rows.append(json.loads(ln))
except Exception as e:
return JSONResponse(status_code=500, content={"error": str(e)})
return {"stem": stem, "rows": rows}
@app.get("/healthz")
def healthz():
"""ヘルスチェック。daemon/UDP 状況問わず常に 200 を返す（keeper 自身の生存）."""
return {
"ok": True,
"server_now_ms": int(time.time() * 1000),
"last_received_at_ms": _latest["received_at_ms"],
"has_datagram": _latest["datagram"] is not None,
}
@app.websocket("/ws")
async def ws_endpoint(ws: WebSocket):
await ws.accept()
print("[SRV] WS connected")
try:
while True:
now_ms = int(time.time() * 1000)
msg = {
"server_now_ms": now_ms,
"datagram": _latest["datagram"],
"received_at_ms": _latest["received_at_ms"],
}
await ws.send_text(json.dumps(msg))
await asyncio.sleep(WS_PUSH_INTERVAL_SEC)
except WebSocketDisconnect:
print("[SRV] WS disconnected")
except Exception as e:
print("[SRV] WS 例外: {}".format(e), file=sys.stderr)
# ---- メイン -----------------------------------------------------------------
def main():
print("=" * 60)
print("[SRV] observe_server.py")
print("[SRV] HTTP/WS = {}:{}".format(HTTP_HOST, HTTP_PORT))
print("[SRV] UDP in = {}:{}".format(UDP_LISTEN_HOST, UDP_LISTEN_PORT))
print("[SRV] open http://{}:{}/".format(HTTP_HOST, HTTP_PORT))
print("[SRV] health http://{}:{}/healthz".format(HTTP_HOST, HTTP_PORT))
print("=" * 60)
uvicorn.run(app, host=HTTP_HOST, port=HTTP_PORT, log_level="warning")
if __name__ == "__main__":
main()static/index.html
static/index.html — 表示層。STAGE HISTORY の JSONL 由来化・VERDICT 3スロット・REFERENCE LANES マトリクス・撮影レーン(cam:top / cam:side)を含む。
<!doctype html>
<html lang="ja">
<head>
<meta charset="utf-8">
<title>observe — グランス＋ドリルイン</title>
<meta name="viewport" content="width=device-width,initial-scale=1">
<link rel="preconnect" href="https://fonts.googleapis.com">
<link rel="preconnect" href="https://fonts.gstatic.com" crossorigin>
<link href="https://fonts.googleapis.com/css2?family=IBM+Plex+Mono:wght@400;500;700&family=Noto+Sans+JP:wght@400;500;700&display=swap" rel="stylesheet">
<style>
:root{
--bg:#f5f5f0;
--fg:#0d0d0d;
--edge:1.5px;
}
*{margin:0;padding:0;box-sizing:border-box}
html,body{
background:var(--bg);
color:var(--fg);
font-family:'Noto Sans JP', system-ui, sans-serif;
min-height:100vh;
-webkit-font-smoothing:antialiased;
}
body{
display:block;
width:100%;
max-width:none;
margin:0;
/* PC: 上下40 / 左右80（タブレット40・SP20 は下部メディアクエリ） */
padding:40px 80px;
}
/* ---- レイアウト: 左サイドバー + メイン ------------------------------- */
.shell{
display:flex;
align-items:stretch;
gap:3vw;
}
.main{
flex:1 1 auto;
min-width:0;
display:flex;
flex-direction:column;
gap:3vh;
}
/* ---- 左 縦メニュー（サイドバー） ------------------------------------- */
.sidebar{
flex:0 0 230px;
width:230px;
align-self:stretch;
min-height:0;
display:flex;
flex-direction:column;
gap:1.6em;
border:var(--edge) solid var(--fg);
padding:1.2em 1.2em 1.4em;
}
.sb-toggle{
display:none;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.78rem;
font-weight:500;
letter-spacing:0.2em;
text-transform:uppercase;
background:transparent;
color:var(--fg);
border:var(--edge) solid var(--fg);
padding:0.5em 0.8em;
cursor:pointer;
width:100%;
text-align:left;
}
.sb-body{
flex:1 1 auto;
min-height:0;
display:flex;
flex-direction:column;
gap:1.6em;
}
.sb-section{ display:flex; flex-direction:column; gap:0.7em; }
/* TRIALS（最後のセクション）が残りの縦スペースを埋め、リスト内でスクロール */
.sb-body > .sb-section:last-child{ flex:1 1 auto; min-height:0; }
.sb-section > .label{ margin-bottom:0.1em; }
/* パネル表示/非表示トグル */
.panel-toggles{ list-style:none; display:flex; flex-direction:column; gap:0.5em; }
.panel-toggles li{ margin:0; }
.panel-toggles label{
display:flex; align-items:center; gap:0.6em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.82rem;
letter-spacing:0.04em;
cursor:pointer;
user-select:none;
}
.panel-toggles input[type="checkbox"]{
appearance:none; -webkit-appearance:none;
width:14px; height:14px; flex:0 0 14px;
border:var(--edge) solid var(--fg);
background:transparent;
display:inline-block; position:relative; cursor:pointer;
}
.panel-toggles input[type="checkbox"]:checked{ background:var(--fg); }
.panel-toggles input[type="checkbox"]:focus-visible{ outline:2px solid var(--fg); outline-offset:2px; }
.panel-toggles label.off{ opacity:0.45; }
/* TRIAL 履歴ナビ */
.trial-live-btn{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.8rem; font-weight:500; letter-spacing:0.08em;
text-transform:uppercase;
background:var(--fg); color:var(--bg);
border:var(--edge) solid var(--fg);
padding:0.45em 0.7em; cursor:pointer; width:100%; text-align:left;
margin-bottom:0.6em;
}
.trial-live-btn.dim{ background:transparent; color:var(--fg); }
.trial-list{ list-style:none; display:flex; flex-direction:column; gap:0.4em; flex:1 1 auto; min-height:0; overflow-y:auto; }
.trial-item{
width:100%; text-align:left; cursor:pointer;
background:transparent; color:var(--fg);
border:var(--edge) solid var(--fg);
padding:0.5em 0.6em;
display:flex; flex-direction:column; gap:0.15em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
}
.trial-item:hover{ background:rgba(13,13,13,0.06); }
.trial-item.active{ background:var(--fg); color:var(--bg); }
.trial-item .ti-id{ font-size:0.8rem; font-weight:700; letter-spacing:0.04em; word-break:break-all; }
.trial-item .ti-task{ font-size:0.72rem; opacity:0.7; word-break:break-all; }
.trial-item.active .ti-task{ opacity:0.85; }
.trial-item .ti-meta{ font-size:0.68rem; opacity:0.55; letter-spacing:0.1em; text-transform:uppercase; }
.trial-item.active .ti-meta{ opacity:0.8; }
.trial-empty{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.78rem; opacity:0.55; padding:0.4em 0;
}
/* 過去 TRIAL レビュー中のバナー */
.review-banner{
display:none;
align-items:center; justify-content:space-between; gap:1em;
border:var(--edge) solid var(--fg);
background:var(--fg); color:var(--bg);
padding:0.6em 1em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.82rem; letter-spacing:0.08em; text-transform:uppercase;
}
.review-banner b{ font-weight:700; }
.review-banner button{
font-family:inherit; font-size:0.78rem; letter-spacing:0.08em; text-transform:uppercase;
background:var(--bg); color:var(--fg); border:var(--edge) solid var(--bg);
padding:0.35em 0.7em; cursor:pointer;
}
body.reviewing .review-banner{ display:flex; }
/* ---- 共通 ---------------------------------------------------------- */
.mono{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
}
.label{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.85rem;
font-weight:500;
text-transform:uppercase;
letter-spacing:0.22em;
opacity:0.55;
}
.divider{
border-top:var(--edge) solid var(--fg);
}
/* ---- 上段: CONNECTION バー ---------------------------------------- */
.connbar{
display:flex;
justify-content:space-between;
align-items:baseline;
gap:2em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.92rem;
font-variant-numeric:tabular-nums;
border-bottom:var(--edge) solid var(--fg);
padding-bottom:0.9em;
}
.connbar .k{opacity:0.55;text-transform:uppercase;letter-spacing:0.16em;font-size:0.78rem}
.connbar .group{display:flex;gap:0.6em;align-items:baseline}
.conn-dot{
display:inline-block;width:10px;height:10px;
border:var(--edge) solid var(--fg);background:var(--fg);
vertical-align:middle;margin-right:8px;
}
.conn-down .conn-dot{background:transparent}
/* ---- 中段: グランス --------------------------------------- */
.glance{
display:grid;
grid-template-columns: 1.4fr 1fr;
grid-template-rows: auto auto auto;
gap:3vh 3vw;
align-items:stretch;
}
.cell{
display:flex;
flex-direction:column;
gap:0.5em;
padding:1.4em 1.6em;
border:var(--edge) solid var(--fg);
}
.cell .label{margin-bottom:0.3em}
.cell.phase{grid-row:1; grid-column:1}
.cell.gripper{grid-row:1; grid-column:2}
/* joints / meta / lanes-section は .glance グリッドから外に出して独立ブロック化した
（Phase 2-c で grid-row を弄ったところ, relayoutGlance の inline gridRow と衝突し
joints と lanes-section が同一領域に重畳する事故を招いたため）.
通常フロー（block）で「STAGE HISTORY+GRIPPER → REFERENCE LANES → JOINTS → META」
の順に縦積みし、間隔だけ margin-top で確保する. */
.main > .lanes-section,
.main > .cell.joints,
.main > .cell.meta{
margin-top:3vh;
}
.phase-name{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:clamp(1.25rem, 2.6vw, 1.75rem); /* jointsに合わせて 28px 上限 */
font-weight:700;
letter-spacing:-0.01em;
line-height:1.2;
word-break:normal;
overflow-wrap:anywhere;
}
.phase-elapsed{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:clamp(2.6rem, 8vw, 5rem);
font-weight:500;
letter-spacing:-0.03em;
line-height:1;
margin-top:0.2em;
}
.phase-elapsed .unit{font-size:0.5em;opacity:0.55;margin-left:0.2em;letter-spacing:0.15em}
/* STAGE 履歴リスト（古いものが上・最新が下に積む） */
.stage-history{
display:flex;
flex-direction:column;
gap:0.35em;
margin-top:0.5em;
}
.stage-history-row{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.82rem;
font-variant-numeric:tabular-nums;
display:grid;
grid-template-columns: 13ch 1fr 6ch;
gap:0.2em 1.2em;
padding:0.4em 0;
border-bottom:var(--edge) dashed var(--fg);
}
.stage-history-row:last-child{border-bottom:var(--edge) solid var(--fg)}
.stage-history-row .t{opacity:0.55; font-size:0.78rem}
.stage-history-row .s{font-weight:700; letter-spacing:0.04em; word-break:break-all}
.stage-history-row .g{font-weight:500; text-align:right}
.stage-history-row .a{
grid-column: 1 / -1;
font-size:0.74rem;
opacity:0.7;
word-break:break-all;
margin-top:0.1em;
}
.stage-history-empty{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.85rem; opacity:0.55;
padding:1em 0;
}
.gripper-val{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:clamp(1.5rem, 3vw, 2rem); /* 英字なので少し大きめ 32px 上限 */
font-weight:700;
line-height:1;
letter-spacing:-0.02em;
}
.gripper-val.open{}
.gripper-val.close{
background:var(--fg);
color:var(--bg);
padding:0 0.25em;
display:inline-block;
}
/* GRIPPER セル内に出す赤ブロック/白箱座標（mm）. 一目で読める. */
.gripper-coords{
margin-top:1em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.9rem;
font-variant-numeric:tabular-nums;
display:grid;
grid-template-columns:max-content 1fr;
gap:0.3em 1em;
}
.gripper-coords .gc-k{opacity:0.55}
.gripper-coords .gc-v{text-align:right}
.gripper-coords .gc-row{display:contents}
.gripper-coords .gc-path{
text-align:right;
word-break:break-all;
font-size:11px;
opacity:0.75;
line-height:1.35;
}
/* J0..J5 大カラム（6軸グリッド・モバイル対応）.
デフォルトで横並び 6 列、狭い画面では 3 列 / 2 列 / 1 列に折り返し. */
.joints-grid{
display:grid;
grid-template-columns: repeat(6, minmax(0, 1fr));
gap:1.2em 0.8em;
margin-top:0.3em;
}
@media (max-width: 900px){
.joints-grid{ grid-template-columns: repeat(3, minmax(0, 1fr)); }
}
@media (max-width: 540px){
.joints-grid{ grid-template-columns: repeat(2, minmax(0, 1fr)); }
}
.joint-cell{
display:flex;
flex-direction:column;
align-items:flex-start;
gap:0.2em;
min-width:0;
}
.joint-cell .joint-label{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.78rem; font-weight:500;
letter-spacing:0.18em; opacity:0.55;
text-transform:uppercase;
}
.joint-cell .joint-val{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:clamp(1.25rem, 2.6vw, 1.75rem); /* base/上限 28px。タブレット・SPは vw で自動縮小 */
font-weight:700;
line-height:1;
letter-spacing:-0.03em;
white-space:nowrap;
}
.joint-cell .joint-unit{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.65rem; opacity:0.55;
letter-spacing:0.2em; text-transform:uppercase;
margin-top:0.1em;
}
.cell.meta{justify-content:center}
.metalist{
display:grid;
grid-template-columns:max-content 1fr;
gap:0.55em 1.6em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.92rem;
font-variant-numeric:tabular-nums;
}
.metalist .k{opacity:0.55;text-transform:uppercase;letter-spacing:0.16em;font-size:0.78rem}
/* ---- 下段: ドリルイン -------------------------------------------- */
.drill{
border:var(--edge) solid var(--fg);
padding:0;
}
.drill > .drill-head{
padding:0.9em 1.4em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.85rem;
font-weight:500;
text-transform:uppercase;
letter-spacing:0.22em;
display:flex;
align-items:center;
justify-content:space-between;
user-select:none;
border-bottom:var(--edge) solid var(--fg);
}
.drill-body{
padding:1.4em 1.6em;
display:grid;
grid-template-columns:1fr 1fr;
gap:1.8em 2.4em;
}
.drill-section{
display:flex;
flex-direction:column;
gap:0.6em;
}
.drill-section.wide{grid-column:1 / -1}
.drill-section .label{margin-bottom:0.25em}
.angles-grid{
display:grid;
grid-template-columns:repeat(2, 1fr);
gap:0.3em 1.4em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:1rem;
}
.angles-grid .jk{opacity:0.55;letter-spacing:0.15em;margin-right:0.4em}
.angles-grid .jv{font-weight:500}
.kv{
display:grid;
grid-template-columns:max-content 1fr;
gap:0.4em 1.2em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:0.95rem;
}
.kv .k{opacity:0.55;text-transform:uppercase;letter-spacing:0.16em;font-size:0.78rem}
.kv .v{word-break:break-all}
/* ---- verdict 3 スロット ------------------------------------------- */
.verdict-grid{
display:grid;
grid-template-columns:max-content max-content max-content 1fr;
gap:0.5em 1.2em;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:0.95rem;
align-items:baseline;
}
.verdict-grid .vh{
opacity:0.55;text-transform:uppercase;letter-spacing:0.16em;font-size:0.78rem;
border-bottom:1px solid var(--fg);
padding-bottom:0.35em;
}
.verdict-grid .vs-name{
font-weight:700; letter-spacing:0.06em;
}
.verdict-grid .vs-v{ font-weight:600; }
.verdict-grid .vs-c{ }
.verdict-grid .vs-r{
white-space:pre-wrap;
word-break:break-word;
overflow-wrap:anywhere;
line-height:1.4;
}
/* ---- lookup / corr パネル （STAGE HISTORY 拡張表示）----------- */
.stage-history-row.abort{
color:var(--fg);
outline:1px solid var(--fg);
outline-offset:-2px;
}
.stage-history-row .lk,
.stage-history-row .cr{
font-size:0.78rem;
opacity:0.72;
white-space:pre-wrap;
word-break:break-word;
overflow-wrap:anywhere;
grid-column:1 / -1;
padding-left:0.6em;
border-left:2px solid var(--fg);
margin-top:0.2em;
}
.stage-history-row .cr.safety-rollback{
opacity:1;
font-weight:700;
}
/* ---- 参照レーン図（matrix） ------------------------------------- */
/* STAGE = 列, 参照先 = 行. 記号は · / ● / ◇ / ✕.
縦横は等幅 mono で厳密に揃える. 一度も ● が付かないレーンは薄字. */
.lanes-section{
margin-top:1em;
padding:1.1em 1.2em 1.2em;
border:var(--edge) solid var(--fg);
}
.lanes-section > .lanes-head{
font-size:0.78rem;
opacity:0.55;
text-transform:uppercase;
letter-spacing:0.16em;
margin-bottom:0.6em;
}
.lanes-wrap{
overflow-x:auto;
padding-bottom:0.3em;
}
.lanes-grid{
display:grid;
/* セル列幅を 3.2ch → 4.2ch へ広げる（約 130%）. tabular-nums + 固定列幅で
枠線が無くても縦は揃う. */
grid-template-columns:max-content repeat(var(--ncols, 1), 4.2ch);
gap:0.1em 0;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
font-size:0.9rem;
line-height:1.5;
}
.lanes-grid .lane-corner{
font-size:0.72rem;
opacity:0.55;
text-transform:uppercase;
letter-spacing:0.14em;
padding:0.15em 1em 0.4em 0.2em;
/* 枠線削除: 縦横の揃いはグリッド + tabular-nums で担保する. */
text-align:right;
white-space:nowrap;
}
.lanes-grid .stage-head{
font-size:0.75rem;
opacity:0.72;
padding:0.15em 0 0.4em 0;
text-align:center;
white-space:nowrap;
cursor:pointer;
user-select:none;
}
.lanes-grid .stage-head.abort{
font-weight:700;
text-decoration:underline;
}
.lanes-grid .lane-name{
padding:0.15em 1em 0.15em 0.2em;
text-align:right;
opacity:0.72;
white-space:nowrap;
font-size:0.85rem;
}
.lanes-grid .cell{
/* Phase 2-e: グリッド子は class="cell dot/mark/..." にしていたため .cell 基底
クラス（border/padding/display:flex）を継承していた. lanes-grid 配下だけ
明示的に打ち消す（他パネルの .cell.phase / .cell.joints 等には影響しない）. */
display:block;
padding:0.15em 0;
border:none;
gap:0;
text-align:center;
color:var(--fg);
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-variant-numeric:tabular-nums;
}
/* · = 未参照（薄いドット）*/
.lanes-grid .cell.dot{ opacity:0.28; }
/* ● = 参照した（濃い丸）*/
.lanes-grid .cell.mark{ font-weight:700; opacity:1; }
/* ◇ = safety_rollback（白抜き菱形）*/
.lanes-grid .cell.diamond{ font-weight:500; opacity:1; }
/* ✕ = guard_pass=false / kind=abort（失敗）*/
.lanes-grid .cell.cross{ font-weight:700; opacity:1; }
/* ◉ = 撮影あり (cam:top / cam:side). ● (mark) と字形で区別できるが少しだけ強調. */
.lanes-grid .cell.cap{ font-weight:500; opacity:1; cursor:help; }
/* 全 STAGE で · しか付かないレーンは行全体を薄く. FK 化で j0_lookup が
使われなくなったことを目で確認するための「消灯確認レーン」. */
.lanes-grid .lane-name.unused,
.lanes-grid .cell.unused{ opacity:0.35; }
/* 選択列ハイライト（LIVE 中は現 phase, レビュー中はクリック列）*/
.lanes-grid .stage-head.hi{
background:rgba(13,13,13,0.08);
font-weight:700;
opacity:1;
}
.lanes-grid .cell.hi{
background:rgba(13,13,13,0.08);
}
.raw-json{
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.8rem;
line-height:1.45;
padding:1em 1.2em;
border:var(--edge) solid var(--fg);
background:transparent;
white-space:pre-wrap;
word-break:break-all;
overflow-wrap:anywhere;
}
/* ---- stale 状態 --------------------------------------------------- */
body.stale .glance,
body.stale .drill{opacity:0.32}
body.stale .conn-dot{background:var(--fg);opacity:0.3}
/* idle 中は stale の減光を打ち消し、待機表示をはっきり出す */
body.idle .glance,
body.idle .drill{opacity:1}
/* idle 中だけ phase 名 (待機中) を出し、履歴リストを隠す */
body.idle #phase{display:block !important}
body.idle #stage-history{display:none}
body.idle #stage-history-empty{display:none}
/* held 中も stale の減光を打ち消す（古さは HELD バッジと DELTA で示す） */
body.held .glance,
body.held .drill{opacity:1}
/* ---- HELD バッジ（保持中＝最後の phase 値をラッチ表示中） ------------- */
.held-flag{
display:none;
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.72rem;
font-weight:700;
letter-spacing:0.18em;
padding:0.15em 0.5em;
border:var(--edge) solid var(--fg);
margin-left:0.8em;
text-transform:uppercase;
}
body.held .held-flag{ display:inline-block; }
/* idle 中は HELD を出さない（待機状態が優先表示） */
body.idle .held-flag{ display:none; }
/* ---- 過去 TRIAL レビュー中の表示オーバーライド（idle ルールより後に置く） ---- */
/* レビュー中は履歴パネルを常に出し、idle の「待機中」表示や減光に勝たせる */
body.reviewing #stage-history{ display:block !important; }
body.reviewing #phase{ display:none !important; }
body.reviewing #stage-history-empty{ display:none !important; }
body.reviewing .glance,
body.reviewing .drill{ opacity:1 !important; }
/* ---- レスポンシブ: コンテンツ幅100% / 上下40・左右可変 ---------------- */
/* タブレット: 上下40 / 左右40 */
@media (max-width:1024px){
body{ padding:40px 40px; }
.sidebar{ flex-basis:200px; width:200px; }
}
/* SP: 上下40 / 左右20。サイドバーは上部のドロワーへ */
@media (max-width:640px){
body{ padding:40px 20px; }
.shell{ flex-direction:column; gap:3vh; }
.sidebar{
flex:0 0 auto; width:100%; position:static; top:auto;
gap:1em; padding:1em;
}
.sb-toggle{ display:block; }
.sb-body.collapsed{ display:none; }
.glance{ grid-template-columns:1fr; }
.trial-list{ max-height:none; }
}
/* ---- パスのクリックで Finder リビール（未配線時はコピー） ---- */
.gc-path.linkable{ cursor:pointer; }
.gc-path.linkable:hover{ text-decoration:underline; }
#path-toast{
position:fixed; bottom:18px; left:50%; transform:translateX(-50%);
background:var(--fg); color:var(--bg);
font-family:'IBM Plex Mono', ui-monospace, monospace;
font-size:0.78rem; letter-spacing:0.04em;
padding:0.6em 1em; border:var(--edge) solid var(--fg);
max-width:90vw; white-space:nowrap; overflow:hidden; text-overflow:ellipsis;
opacity:0; pointer-events:none; transition:opacity 0.15s; z-index:50;
}
#path-toast.show{ opacity:1; }
</style>
</head>
<body>
<div class="shell">
<!-- 左 縦メニュー -->
<aside class="sidebar" id="sidebar">
<button class="sb-toggle" id="sb-toggle" type="button" aria-expanded="true">☰ menu</button>
<div class="sb-body" id="sb-body">
<div class="sb-section">
<div class="label">panels</div>
<ul class="panel-toggles" id="panel-toggles">
<li><label data-panel-label="phase"><input type="checkbox" data-panel="phase" checked> stage history</label></li>
<li><label data-panel-label="gripper"><input type="checkbox" data-panel="gripper" checked> gripper</label></li>
<li><label data-panel-label="joints"><input type="checkbox" data-panel="joints" checked> joints j0..j5</label></li>
<li><label data-panel-label="trial"><input type="checkbox" data-panel="trial" checked> trial</label></li>
<li><label data-panel-label="drill"><input type="checkbox" data-panel="drill" checked> drill-in</label></li>
</ul>
</div>
<div class="sb-section">
<div class="label">trials</div>
<button class="trial-live-btn" id="trial-live-btn" type="button">● live</button>
<ul class="trial-list" id="trial-list"></ul>
<div class="trial-empty" id="trial-empty">no trials yet</div>
</div>
</div>
</aside>
<!-- メイン -->
<main class="main">
<!-- 過去 TRIAL レビュー中バナー -->
<div class="review-banner" id="review-banner">
<span>reviewing trial <b id="review-id">—</b></span>
<button id="review-exit" type="button">← back to live</button>
</div>
<!-- 上段: CONNECTION バー -->
<div class="connbar">
<div class="group">
<span class="k">conn</span>
<span id="conn"><span class="conn-dot"></span>connecting…</span>
</div>
<div class="group">
<span class="k">last</span><span id="last">—</span>
<span class="k">delta</span><span id="delta">—</span>
<span class="k">seq</span><span id="seq">—</span>
<span id="held-flag" class="held-flag">HELD</span>
</div>
</div>
<!-- 中段: グランス -->
<div class="glance">
<div class="cell phase">
<div class="label">stage history — oldest top</div>
<!-- 現 phase 名（idle 時に「待機中」を出す唯一の場所として残す。
履歴モードでは通常 hidden で運用するが、idle 中は表示する CSS で制御） -->
<div id="phase" class="phase-name" style="display:none">—</div>
<span id="elapsed" hidden>—</span>
<div id="stage-history" class="stage-history"></div>
<div id="stage-history-empty" class="stage-history-empty">no stages yet</div>
</div>
<div class="cell gripper">
<div class="label">gripper</div>
<div id="gripper" class="gripper-val">—</div>
<div class="gripper-coords">
<div class="gc-row"><span class="gc-k">block pick</span><span class="gc-v"><span id="g-block-pick-x">—</span>, <span id="g-block-pick-z">—</span></span></div>
<div class="gc-row"><span class="gc-k">block place</span><span class="gc-v"><span id="g-block-place-x">—</span>, <span id="g-block-place-z">—</span></span></div>
<div class="gc-row"><span class="gc-k">box</span><span class="gc-v"><span id="g-box-x">—</span>, <span id="g-box-z">—</span></span></div>
<div class="gc-row"><span class="gc-k">in_box</span><span class="gc-v" id="g-in-box">—</span></div>
<div class="gc-row"><span class="gc-k">tip Δdist</span><span class="gc-v" id="g-tip-dist">—</span></div>
<div class="gc-row"><span class="gc-k">tip Δx, Δz</span><span class="gc-v"><span id="g-tip-dx">—</span>, <span id="g-tip-dz">—</span></span></div>
<div class="gc-row"><span class="gc-k">place Δdist</span><span class="gc-v" id="g-place-dist">—</span></div>
<div class="gc-row"><span class="gc-k">place Δx, Δz</span><span class="gc-v"><span id="g-place-dx">—</span>, <span id="g-place-dz">—</span></span></div>
<div class="gc-row"><span class="gc-k">run dir</span><span class="gc-v gc-path" id="g-run-dir">—</span></div>
<div class="gc-row"><span class="gc-k">tip jpg</span><span class="gc-v gc-path" id="g-cap-tip">—</span></div>
<div class="gc-row"><span class="gc-k">pick jpg</span><span class="gc-v gc-path" id="g-cap-pick">—</span></div>
<div class="gc-row"><span class="gc-k">place jpg</span><span class="gc-v gc-path" id="g-cap-place">—</span></div>
</div>
</div>
</div><!-- /glance -->
<!-- REFERENCE LANES: STAGE × 参照先 マトリクス.
独立ブロック（どのグリッドにも所属しない）として glance と JOINTS の間に配置.
Phase 2-c で glance グリッドに入れて grid-row を割り当てたところ, JS 側の
relayoutGlance() が joints に inline gridRow を上書きしてしまい、行が衝突して
lanes-section と joints が同じ描画領域に重畳する事故が起きた（Phase 2-d で修正）.
block 通常フローで縦積みし margin-top で間隔だけ確保する. -->
<section class="lanes-section">
<div class="lanes-head">reference lanes</div>
<div class="lanes-wrap">
<div id="lanes-grid" class="lanes-grid" style="--ncols: 1"></div>
</div>
<div id="lanes-empty" class="stage-history-empty">no lanes yet</div>
</section>
<div class="cell joints">
<div class="label">joints — j0..j5</div>
<div class="joints-grid">
<div class="joint-cell">
<div class="joint-label">j0</div>
<div class="joint-val" id="g-j0">—</div>
<div class="joint-unit">deg</div>
</div>
<div class="joint-cell">
<div class="joint-label">j1</div>
<div class="joint-val" id="g-j1">—</div>
<div class="joint-unit">deg</div>
</div>
<div class="joint-cell">
<div class="joint-label">j2</div>
<div class="joint-val" id="g-j2">—</div>
<div class="joint-unit">deg</div>
</div>
<div class="joint-cell">
<div class="joint-label">j3</div>
<div class="joint-val" id="g-j3">—</div>
<div class="joint-unit">deg</div>
</div>
<div class="joint-cell">
<div class="joint-label">j4</div>
<div class="joint-val" id="g-j4">—</div>
<div class="joint-unit">deg</div>
</div>
<div class="joint-cell">
<div class="joint-label">j5</div>
<div class="joint-val" id="g-j5">—</div>
<div class="joint-unit">deg</div>
</div>
</div>
</div>
<div class="cell meta">
<div class="label">trial</div>
<div class="metalist">
<div class="k">id</div> <div id="trial-id" class="v">—</div>
<div class="k">task</div> <div id="trial-task" class="v">—</div>
</div>
</div>
<!-- 下段: ドリルイン -->
<section class="drill">
<div class="drill-head"><span>drill-in — joints / ee / verdict / meta / raw</span></div>
<div class="drill-body">
<div class="drill-section">
<div class="label">angles (j1..j6) deg</div>
<div class="angles-grid">
<div><span class="jk">j1</span><span id="j1" class="jv">—</span></div>
<div><span class="jk">j2</span><span id="j2" class="jv">—</span></div>
<div><span class="jk">j3</span><span id="j3" class="jv">—</span></div>
<div><span class="jk">j4</span><span id="j4" class="jv">—</span></div>
<div><span class="jk">j5</span><span id="j5" class="jv">—</span></div>
<div><span class="jk">j6</span><span id="j6" class="jv">—</span></div>
</div>
</div>
<div class="drill-section">
<div class="label">ee (x,y,z) mm</div>
<div class="kv">
<div class="k">x</div> <div id="ee-x" class="v">—</div>
<div class="k">y</div> <div id="ee-y" class="v">—</div>
<div class="k">z</div> <div id="ee-z" class="v">—</div>
</div>
</div>
<div class="drill-section wide">
<div class="label">verdict</div>
<!-- 3 スロット表示. 各スロットは raw_verdict / confidence / reason.
reason は折り返して全文表示（切らない）. null スロットは「—」. -->
<div class="verdict-grid">
<div class="vh">slot</div>
<div class="vh">verdict</div>
<div class="vh">confidence</div>
<div class="vh">reason</div>
<div class="vs-name">PICK</div>
<div id="v-pick-raw" class="vs-v">—</div>
<div id="v-pick-conf" class="vs-c">—</div>
<div id="v-pick-reason" class="vs-r">—</div>
<div class="vs-name">PLACE</div>
<div id="v-place-raw" class="vs-v">—</div>
<div id="v-place-conf" class="vs-c">—</div>
<div id="v-place-reason" class="vs-r">—</div>
<div class="vs-name">SIDE_PLACE</div>
<div id="v-side-raw" class="vs-v">—</div>
<div id="v-side-conf" class="vs-c">—</div>
<div id="v-side-reason" class="vs-r">—</div>
</div>
</div>
<div class="drill-section">
<div class="label">coords (mm)</div>
<div class="kv">
<div class="k">block(pick) X</div> <div id="c-block-pick-x" class="v">—</div>
<div class="k">block(pick) Z</div> <div id="c-block-pick-z" class="v">—</div>
<div class="k">block(place) X</div> <div id="c-block-place-x" class="v">—</div>
<div class="k">block(place) Z</div> <div id="c-block-place-z" class="v">—</div>
<div class="k">box X</div> <div id="c-box-x" class="v">—</div>
<div class="k">box Z</div> <div id="c-box-z" class="v">—</div>
<div class="k">in_box</div> <div id="c-in-box" class="v">—</div>
</div>
</div>
<div class="drill-section">
<div class="label">datagram meta</div>
<div class="kv">
<div class="k">source</div> <div id="r-source" class="v">—</div>
<div class="k">t (ms)</div> <div id="r-t" class="v">—</div>
<div class="k">t (time)</div> <div id="r-t-time" class="v">—</div>
<div class="k">phase_t0</div> <div id="r-phase-t0" class="v">—</div>
<div class="k">phase_t0 (time)</div><div id="r-phase-t0-time" class="v">—</div>
<div class="k">trial.id</div> <div id="r-trial-id" class="v">—</div>
<div class="k">trial.task</div> <div id="r-trial-task" class="v">—</div>
</div>
</div>
<div class="drill-section wide">
<div class="label">raw json</div>
<pre id="r-json" class="raw-json">—</pre>
</div>
</div>
</section>
</main>
</div><!-- /shell -->
<script>
(function(){
// ---- 状態保持(datagram 谷間でも経過秒を進めるためローカルに持つ)----
var state = {
phase: null,
phase_t0: null, // ms (unix)
last_dg: null,
last_received_at: null,
server_now: null,
// gripper デバウンス: 受信値（gripperPending）が DEBOUNCE_MS 連続で同じなら
// gripperConfirmed に commit. 表示は常に Confirmed を見る. null は即 commit.
gripperConfirmed: null,
gripperPending: null,
gripperPendingSince: null,
// IDLE: WS 切断 / 未受信 / 長時間放置 のいずれかで真. ライブ値を全て「—」に.
// HELD: idle ではないが最後の受信から STALE_MS 超 → 値はラッチ保持、HELD バッジ表示.
// 谷間（phase 完了ごとに 1 発の emit）で前 phase の値を消さない設計の核.
idle: false,
held: false,
// WS 接続状態（onopen=true, onclose/onerror=false）. idle 判定に使う.
wsOpen: false,
// STAGE 履歴: phase が前と変わったら 1 行 append. 古い順.
// 新しい run（trial.id 変化）でリセット.
stageHistory: [],
historyLastPhase: null,
historyLastTrialId: null,
// in_box の last known: stage9b で true/false が来たら保持し、
// 後段（stage10/abort）の null 上書きで「—」に戻らないようにする.
// 新 run（trial.id 変化）でリセット.
lastInBox: null,
// ---- 過去 TRIAL 履歴（サイドバーから追える）----
// trial.id が変わるたびに新レコードを作って積む. 各レコードは
// {id, task, stages[], lastDg, inBox, startedAt, lastAt}.
trials: [],
trialsById: {},
currentTrialId: undefined,
currentRecord: null,
// レビュー中: 過去 trial を選択して表示を凍結している状態.
// ライブ datagram は背景で state を更新し続けるが、パネル描画は止める.
reviewing: false,
reviewId: null,
// 参照レーン図の現在ハイライト STAGE 名. LIVE 中は state.phase に追従し、
// レビュー中はクリックで別 STAGE をハイライト可能.
highlightedStage: null,
// パネル表示/非表示（true=非表示）. glance 4 枠 + drill.
panelHidden: { phase:false, gripper:false, joints:false, trial:false, drill:false }
};
// 履歴の最大保持行数（古い行から捨てる. メモリ/DOM 過剰防止）.
var STAGE_HISTORY_MAX = 200;
// 過去 TRIAL の最大保持数（古い trial から捨てる）.
var TRIALS_MAX = 50;
// 鮮度系の定数 ---------------------------------------------------------
// STALE_MS: 受信から これを超えたら HELD（保持中）バッジを出す.
// HOLD_IDLE_MS: HELD のままさらに これを超えたら idle（待機）に落とす.
// run 中の谷間（数十秒）は保持されるが、run 終了後 2 分放置で待機へ.
var STALE_MS = 2000;
var HOLD_IDLE_MS = 120000;
var ELAPSED_TICK_MS = 100;
// gripper コマンド値の往復ノイズ除去（実機状態 API が無い前提の表示側デバウンス）.
// 600ms 連続で同じ値を保持したら表示更新. ここ 1 か所で調整可.
var GRIPPER_DEBOUNCE_MS = 600;
var GRIPPER_TICK_MS = 200;
// ---- DOM 参照 ----
var els = {
conn: document.getElementById("conn"),
last: document.getElementById("last"),
delta: document.getElementById("delta"),
seq: document.getElementById("seq"),
heldFlag: document.getElementById("held-flag"),
phase: document.getElementById("phase"),
elapsed: document.getElementById("elapsed"),
stageHistory: document.getElementById("stage-history"),
stageHistoryEmpty: document.getElementById("stage-history-empty"),
lanesGrid: document.getElementById("lanes-grid"),
lanesEmpty: document.getElementById("lanes-empty"),
gripper: document.getElementById("gripper"),
gJoints: [
document.getElementById("g-j0"),
document.getElementById("g-j1"),
document.getElementById("g-j2"),
document.getElementById("g-j3"),
document.getElementById("g-j4"),
document.getElementById("g-j5")
],
trialId: document.getElementById("trial-id"),
trialTask: document.getElementById("trial-task"),
j1: document.getElementById("j1"),
j2: document.getElementById("j2"),
j3: document.getElementById("j3"),
j4: document.getElementById("j4"),
j5: document.getElementById("j5"),
j6: document.getElementById("j6"),
eeX: document.getElementById("ee-x"),
eeY: document.getElementById("ee-y"),
eeZ: document.getElementById("ee-z"),
// verdict 3 スロット (pick / place / side_place).
// 各スロットは raw_verdict / confidence / reason を独立に持つ.
vPickRaw: document.getElementById("v-pick-raw"),
vPickConf: document.getElementById("v-pick-conf"),
vPickReason: document.getElementById("v-pick-reason"),
vPlaceRaw: document.getElementById("v-place-raw"),
vPlaceConf: document.getElementById("v-place-conf"),
vPlaceReason: document.getElementById("v-place-reason"),
vSideRaw: document.getElementById("v-side-raw"),
vSideConf: document.getElementById("v-side-conf"),
vSideReason: document.getElementById("v-side-reason"),
rSource: document.getElementById("r-source"),
rT: document.getElementById("r-t"),
rTTime: document.getElementById("r-t-time"),
rPhaseT0: document.getElementById("r-phase-t0"),
rPhaseT0Time: document.getElementById("r-phase-t0-time"),
rTrialId: document.getElementById("r-trial-id"),
rTrialTask: document.getElementById("r-trial-task"),
rJson: document.getElementById("r-json"),
cBlockPickX: document.getElementById("c-block-pick-x"),
cBlockPickZ: document.getElementById("c-block-pick-z"),
cBlockPlaceX: document.getElementById("c-block-place-x"),
cBlockPlaceZ: document.getElementById("c-block-place-z"),
cBoxX: document.getElementById("c-box-x"),
cBoxZ: document.getElementById("c-box-z"),
cInBox: document.getElementById("c-in-box"),
// glance 内（GRIPPER セル下）にも同じ座標を出す（DRILL-IN は維持）
gBlockPickX: document.getElementById("g-block-pick-x"),
gBlockPickZ: document.getElementById("g-block-pick-z"),
gBlockPlaceX: document.getElementById("g-block-place-x"),
gBlockPlaceZ: document.getElementById("g-block-place-z"),
gBoxX: document.getElementById("g-box-x"),
gBoxZ: document.getElementById("g-box-z"),
gInBox: document.getElementById("g-in-box"),
gTipDist: document.getElementById("g-tip-dist"),
gTipDx: document.getElementById("g-tip-dx"),
gTipDz: document.getElementById("g-tip-dz"),
gPlaceDist: document.getElementById("g-place-dist"),
gPlaceDx: document.getElementById("g-place-dx"),
gPlaceDz: document.getElementById("g-place-dz"),
gRunDir: document.getElementById("g-run-dir"),
gCapTip: document.getElementById("g-cap-tip"),
gCapPick: document.getElementById("g-cap-pick"),
gCapPlace: document.getElementById("g-cap-place"),
// ---- サイドバー / レイアウト ----
glance: document.querySelector(".glance"),
drill: document.querySelector(".drill"),
cellPhase: document.querySelector(".cell.phase"),
cellGripper: document.querySelector(".cell.gripper"),
cellJoints: document.querySelector(".cell.joints"),
cellTrial: document.querySelector(".cell.meta"),
sidebar: document.getElementById("sidebar"),
sbToggle: document.getElementById("sb-toggle"),
sbBody: document.getElementById("sb-body"),
panelToggles: document.getElementById("panel-toggles"),
trialList: document.getElementById("trial-list"),
trialEmpty: document.getElementById("trial-empty"),
trialLiveBtn:document.getElementById("trial-live-btn"),
reviewBanner:document.getElementById("review-banner"),
reviewId: document.getElementById("review-id"),
reviewExit: document.getElementById("review-exit"),
body: document.body
};
// ---- フォーマット ----
function pad(n,w){ n=String(n); while(n.length<(w||2)) n="0"+n; return n; }
function fmtTime(ms){
if(ms==null) return "—";
var d=new Date(ms);
return pad(d.getHours())+":"+pad(d.getMinutes())+":"+pad(d.getSeconds())+"."+pad(d.getMilliseconds(),3);
}
function fmtPhase(raw){
// 整形は表示だけ。生文字列は state.phase に保持。
// 例: "stage3_approach_pick_a2" → "STAGE3 APPROACH_PICK (a2)"
// "stage5_a2" → "STAGE5 (a2)"
// "pickfail_to_home" → "PICKFAIL_TO_HOME"
if(!raw) return "—";
try{
var s = String(raw);
var suf = "";
var sufMatch = s.match(/^(.+?)_(a\d+)$/);
if(sufMatch){
s = sufMatch[1];
suf = " (" + sufMatch[2] + ")";
}
var m = s.match(/^(stage\d+[a-z]*)(?:_(.+))?$/);
if(m){
var head = m[1].toUpperCase();
var body = m[2] ? " " + m[2].toUpperCase() : "";
return head + body + suf;
}
return s.toUpperCase() + suf;
}catch(e){
return String(raw);
}
}
function fmtNum(v, digits){
if(v==null || typeof v!=="number" || !isFinite(v)) return "—";
return v.toFixed(digits!=null?digits:2);
}
function fmtGripper(g){
if(g===0) return {text:"OPEN", cls:"open"};
if(g===1) return {text:"CLOSE", cls:"close"};
return {text:"—", cls:""};
}
// datagram の block_xz / box_xz 等は Unity m。表示は mm。
function fmtMM(arr, idx){
if(!Array.isArray(arr) || arr.length<2) return "—";
var v = arr[idx];
return (typeof v==="number" && isFinite(v)) ? (v*1000).toFixed(1) : "—";
}
function baseName(p){
if(typeof p!=="string" || !p) return "—";
var i = p.lastIndexOf("/");
return (i>=0) ? p.slice(i+1) : p;
}
// 表示テキストと「絶対パス」を持たせ、クリックでFinderを開けるようにする.
function absCap(pth, runDir){
if(typeof pth!=="string" || !pth) return null;
if(pth.charAt(0)==="/") return pth; // 既に絶対パス
if(runDir) return runDir.replace(/\/+$/,"") + "/" + baseName(pth);
return null;
}
function setPathEl(el, text, absPath){
el.textContent = text;
if(absPath){
el.setAttribute("data-path", absPath);
el.classList.add("linkable");
el.title = "クリックで Finder を開く（サーバー未配線時はパスをコピー）";
} else {
el.removeAttribute("data-path");
el.classList.remove("linkable");
el.removeAttribute("title");
}
}
var _toastTimer = null;
function showToast(msg){
var t = document.getElementById("path-toast");
if(!t) return;
t.textContent = msg;
t.classList.add("show");
if(_toastTimer) clearTimeout(_toastTimer);
_toastTimer = setTimeout(function(){ t.classList.remove("show"); }, 2200);
}
function copyFallback(absPath){
if(navigator.clipboard && navigator.clipboard.writeText){
navigator.clipboard.writeText(absPath).then(
function(){ showToast("パスをコピー（Finder で Cmd+Shift+G → 貼り付け）"); },
function(){ showToast(absPath); }
);
} else {
showToast(absPath);
}
}
// まず Mac 側 /reveal を試し、応答が無ければコピーにフォールバック.
function revealPath(absPath){
if(!absPath) return;
fetch("/reveal?path=" + encodeURIComponent(absPath))
.then(function(r){ return r.ok ? r.json() : Promise.reject(); })
.then(function(j){ if(j && j.revealed){ showToast("Finder で開きました"); } else { return Promise.reject(); } })
.catch(function(){ copyFallback(absPath); });
}
// gripper の値を直接描く（デバウンスを介さない. レビュー再現用）.
function paintGripperValue(v){
var g = fmtGripper((v===0||v===1)?v:null);
els.gripper.textContent = g.text;
els.gripper.className = "gripper-val " + g.cls;
}
// verdict 3 スロットの 1 スロットを描く. dg.verdict は 3 スロット dict:
// {"pick": {...}, "place": {...}, "side_place": {...}}
// スロット値が dict なら raw_verdict / confidence / reason を出す. null は "—".
// 旧単一スロット (raw_verdict などをトップレベルに持つ) にも一応耐える形で書く.
function _paintVerdictSlot(vd, key, elRaw, elConf, elReason){
var slot = null;
if(vd && typeof vd==="object" && vd[key] && typeof vd[key]==="object"){
slot = vd[key];
}
if(!slot){
elRaw.textContent = "—";
elConf.textContent = "—";
elReason.textContent = "—";
return;
}
elRaw.textContent = (slot.raw_verdict!=null) ? String(slot.raw_verdict) : "—";
elConf.textContent = (typeof slot.confidence==="number" && isFinite(slot.confidence))
? slot.confidence.toFixed(2) : "—";
elReason.textContent = (slot.reason!=null) ? String(slot.reason) : "—";
}
// datagram 1 個からパネル群を描く「純描画」関数（state を書き換えない）.
// ライブ・過去レビューの両方から呼ぶ. gripper・seq・stage 履歴・phase は含めない
// （それらは呼び出し側で扱う）. inBoxText は呼び出し側が確定させて渡す.
function paintFromDatagram(dg, inBoxText){
if(!dg) dg = {};
var angles = Array.isArray(dg.angles) ? dg.angles : null;
// JOINTS j0..j5
for(var ji=0; ji<6; ji++){
var av = (angles && angles.length>ji && typeof angles[ji]==="number" && isFinite(angles[ji]))
? angles[ji].toFixed(2) : "—";
els.gJoints[ji].textContent = av;
}
// TRIAL（小）
if(dg.trial && typeof dg.trial==="object"){
els.trialId.textContent = (dg.trial.id!=null) ? String(dg.trial.id) : "—";
els.trialTask.textContent = (dg.trial.task!=null) ? String(dg.trial.task) : "—";
} else {
els.trialId.textContent = "—";
els.trialTask.textContent = "—";
}
// 座標（mm）: DRILL と GRIPPER セル下の両方
els.cBlockPickX.textContent = fmtMM(dg.block_xz, 0);
els.cBlockPickZ.textContent = fmtMM(dg.block_xz, 1);
els.cBlockPlaceX.textContent = fmtMM(dg.block_place_xz, 0);
els.cBlockPlaceZ.textContent = fmtMM(dg.block_place_xz, 1);
els.cBoxX.textContent = fmtMM(dg.box_xz, 0);
els.cBoxZ.textContent = fmtMM(dg.box_xz, 1);
els.cInBox.textContent = inBoxText;
els.gBlockPickX.textContent = fmtMM(dg.block_xz, 0);
els.gBlockPickZ.textContent = fmtMM(dg.block_xz, 1);
els.gBlockPlaceX.textContent = fmtMM(dg.block_place_xz, 0);
els.gBlockPlaceZ.textContent = fmtMM(dg.block_place_xz, 1);
els.gBoxX.textContent = fmtMM(dg.box_xz, 0);
els.gBoxZ.textContent = fmtMM(dg.box_xz, 1);
els.gInBox.textContent = inBoxText;
// TIP 差分・ファイル名
var tp = dg.tip_pick;
if(tp && typeof tp.dist_mm === "number"){
els.gTipDist.textContent = tp.dist_mm.toFixed(1) + " mm";
els.gTipDx.textContent = (typeof tp.dx_mm==="number") ? tp.dx_mm.toFixed(1) : "—";
els.gTipDz.textContent = (typeof tp.dz_mm==="number") ? tp.dz_mm.toFixed(1) : "—";
}else{
els.gTipDist.textContent = "—";
els.gTipDx.textContent = "—";
els.gTipDz.textContent = "—";
}
// PLACE 差分 (block_place - box). PICK 側 tip Δ と対称.
// run_pick_place.py の _ledger_in_box_geometry と同じ差分ロジック
// (dx = (block_place - box) * 1000). block_place_xz か box_xz が
// 欠けている / 数値でない場合は空欄 (tip Δ と同挙動).
var _bpx = Array.isArray(dg.block_place_xz) ? dg.block_place_xz : null;
var _bxx = Array.isArray(dg.box_xz) ? dg.box_xz : null;
if(_bpx && _bxx
&& typeof _bpx[0]==="number" && typeof _bpx[1]==="number"
&& typeof _bxx[0]==="number" && typeof _bxx[1]==="number"){
var _pdx = (_bpx[0] - _bxx[0]) * 1000.0;
var _pdz = (_bpx[1] - _bxx[1]) * 1000.0;
var _pdist = Math.sqrt(_pdx*_pdx + _pdz*_pdz);
els.gPlaceDist.textContent = _pdist.toFixed(1) + " mm";
els.gPlaceDx.textContent = _pdx.toFixed(1);
els.gPlaceDz.textContent = _pdz.toFixed(1);
}else{
els.gPlaceDist.textContent = "—";
els.gPlaceDx.textContent = "—";
els.gPlaceDz.textContent = "—";
}
var _rd = (typeof dg.run_dir==="string" && dg.run_dir) ? dg.run_dir : null;
setPathEl(els.gRunDir, _rd || "—", _rd);
setPathEl(els.gCapTip, baseName(dg.cap_tip), absCap(dg.cap_tip, _rd));
setPathEl(els.gCapPick, baseName(dg.cap_pick), absCap(dg.cap_pick, _rd));
setPathEl(els.gCapPlace, baseName(dg.cap_place),absCap(dg.cap_place,_rd));
// DRILL: angles 6軸
var jEls = [els.j1, els.j2, els.j3, els.j4, els.j5, els.j6];
for(var i=0; i<6; i++){
var v6 = (angles && angles.length>i && typeof angles[i]==="number") ? angles[i] : null;
jEls[i].textContent = (v6==null) ? "—" : v6.toFixed(2);
}
// DRILL: ee
var ee = Array.isArray(dg.ee) ? dg.ee : null;
els.eeX.textContent = (ee && typeof ee[0]==="number") ? ee[0].toFixed(1) : "—";
els.eeY.textContent = (ee && typeof ee[1]==="number") ? ee[1].toFixed(1) : "—";
els.eeZ.textContent = (ee && typeof ee[2]==="number") ? ee[2].toFixed(1) : "—";
// DRILL: verdict — 3 スロット (pick / place / side_place) を独立に描画.
// 各スロットは raw_verdict / confidence / reason. null スロットは全て "—".
// reason は CSS 側で white-space:pre-wrap で全文表示（切らない）.
_paintVerdictSlot(
dg.verdict, "pick",
els.vPickRaw, els.vPickConf, els.vPickReason);
_paintVerdictSlot(
dg.verdict, "place",
els.vPlaceRaw, els.vPlaceConf, els.vPlaceReason);
_paintVerdictSlot(
dg.verdict, "side_place",
els.vSideRaw, els.vSideConf, els.vSideReason);
// DRILL: datagram meta
els.rSource.textContent = (dg.source!=null) ? String(dg.source) : "—";
els.rT.textContent = (dg.t!=null) ? String(dg.t) : "—";
els.rTTime.textContent = (typeof dg.t==="number") ? fmtTime(dg.t) : "—";
els.rPhaseT0.textContent = (dg.phase_t0!=null) ? String(dg.phase_t0) : "—";
els.rPhaseT0Time.textContent = (typeof dg.phase_t0==="number") ? fmtTime(dg.phase_t0) : "—";
var tr = (dg.trial && typeof dg.trial==="object") ? dg.trial : null;
els.rTrialId.textContent = (tr && tr.id!=null) ? String(tr.id) : "—";
els.rTrialTask.textContent = (tr && tr.task!=null) ? String(tr.task) : "—";
// DRILL: 全 JSON
try{
els.rJson.textContent = JSON.stringify(dg, null, 2);
}catch(e){
els.rJson.textContent = "(stringify error: " + e + ")";
}
}
// ---- STAGE 履歴: phase 切替ごとに 1 行 append ----------------------------
// 古いものが上、最新が下に積む. trial.id 変化でリセット.
function _escapeHtml(s){
return String(s).replace(/[&<>"']/g, function(c){
return ({"&":"&","<":"<",">":">","\"":""","'":"'"})[c];
});
}
// ファイル絶対パスから末尾セグメントだけ返す（表示スペース節約）.
function _basename(p){
if(p==null) return "";
var s = String(p);
var i = s.lastIndexOf("/");
var j = s.lastIndexOf("\\");
var k = Math.max(i, j);
return (k >= 0) ? s.slice(k+1) : s;
}
function _fmtXZ(arr){
if(!Array.isArray(arr) || arr.length < 2) return "";
var a = (typeof arr[0]==="number" && isFinite(arr[0])) ? arr[0].toFixed(1) : "—";
var b = (typeof arr[1]==="number" && isFinite(arr[1])) ? arr[1].toFixed(1) : "—";
return "(" + a + ", " + b + ")";
}
function _historyRowHTML(rec){
// rec: {t, phaseRaw, gripperRaw, angles, kind?, lookup?, corr?}
// kind === "abort" のとき abort マーク. lookup/corr が非 null なら小行を追加.
var timeStr = fmtTime(rec.t);
var stageStr = fmtPhase(rec.phaseRaw);
var g = fmtGripper(rec.gripperRaw);
var isAbort = (rec.kind === "abort");
var anglesStr = "—";
if(Array.isArray(rec.angles) && rec.angles.length === 6 &&
rec.angles.every(function(x){return typeof x === "number" && isFinite(x);})){
anglesStr = "[" + rec.angles.map(function(x){return x.toFixed(2);}).join(", ") + "]";
}
var extra = "";
// lookup サブ行
var lk = rec.lookup;
if(lk && typeof lk === "object"){
var parts = [];
parts.push("lookup " + _basename(lk.source_file || "") +
" · " + (lk.method || "—") +
" · pose#" + (lk.pose_id != null ? lk.pose_id : "—"));
var guard = "guard=" + (lk.guard_pass === true ? "PASS" :
lk.guard_pass === false ? "FAIL" : "—");
var reason = lk.guard_reason || lk.abort_reason;
if(reason) guard += " (" + reason + ")";
parts.push(guard);
extra += '<div class="lk">' + _escapeHtml(parts.join(" · ")) + '</div>';
}
// corr サブ行
var cr = rec.corr;
if(cr && typeof cr === "object"){
var cparts = [];
cparts.push("corr[" + (cr.phase || "—") + "] " +
(cr.cell_key || "—"));
var dx = (typeof cr.applied_dx_mm==="number") ? cr.applied_dx_mm.toFixed(2) : "null";
var dz = (typeof cr.applied_dz_mm==="number") ? cr.applied_dz_mm.toFixed(2) : "null";
cparts.push("dx=" + dx + " dz=" + dz);
cparts.push(_fmtXZ(cr.target_before) + " → " + _fmtXZ(cr.target_after));
cparts.push("apply=" + (cr.apply_reason || "—"));
var upd = "updated=" + String(!!cr.updated);
if(cr.update_reason) upd += " (" + cr.update_reason + ")";
cparts.push(upd);
var crCls = "cr";
if(cr.apply_reason === "safety_rollback") crCls += " safety-rollback";
extra += '<div class="' + crCls + '">' +
_escapeHtml(cparts.join(" · ")) + '</div>';
}
var rowCls = "stage-history-row" + (isAbort ? " abort" : "");
// data-stage を持たせて参照レーン図クリック連動に使う（STAGE 履歴行のクリックで
// その列がハイライトされる）.
return '<div class="' + rowCls + '" data-stage="' + _escapeHtml(rec.phaseRaw || "") + '">' +
'<span class="t">' + _escapeHtml(timeStr) + '</span>' +
'<span class="s">' + (isAbort ? "◼ " : "") + _escapeHtml(stageStr) + '</span>' +
'<span class="g">' + _escapeHtml(g.text) + '</span>' +
'<span class="a">J:' + _escapeHtml(anglesStr) + '</span>' +
extra +
'</div>';
}
function _renderStageList(rows){
if(!rows || rows.length === 0){
els.stageHistory.innerHTML = "";
els.stageHistoryEmpty.style.display = "";
return;
}
els.stageHistoryEmpty.style.display = "none";
els.stageHistory.innerHTML = rows.map(_historyRowHTML).join("");
}
// ---- 参照レーン図: 行 = 参照先, 列 = STAGE のマトリクス ---------------
// 行はレーンとして自動収集する:
// - lookup.lookup_name (例: fk_lookup / j0_lookup)
// - lookup.source_file の basename (例: fk_scan_motion_based_v2.csv)
// - corr.memory_file の basename (例: correction_memory.json)
// 固定行:
// - vlm_judge — verdict 発火 stage
// - cam:top / cam:side — 撮影レーン (Phase 3-B)
// 記号:
// · 未参照
// ● 参照した（lookup 一般 / corr apply=applied / vlm_judge 発火）
// ◇ corr apply_reason == "safety_rollback"（破棄）
// ✕ lookup.guard_pass == false または kind == "abort"
// ◉ 撮影あり（cam:top / cam:side）title 属性に image パス
// STAGE 名を短縮ラベルへ ("stage3_approach_pick" → "3AP" 等).
function _shortStageLabel(stage){
if(!stage) return "?";
var m = /^stage([\d]+[a-z]?(?:_\d+)?)/i.exec(stage);
if(!m) return String(stage);
var num = m[1].replace("_", ".");
var rest = stage.slice(m[0].length);
if(!rest) return num;
var initials = rest.replace(/^_/, "").split("_").map(function(w){
return w.length > 0 ? w[0].toUpperCase() : "";
}).join("");
return num + initials;
}
// 各 STAGE 行に verdict が付いているか判定（vlm_judge レーン発火条件）.
function _rowHasVerdict(r){
var vd = r.verdict;
if(!vd || typeof vd !== "object") return false;
return ["pick","place","side_place"].some(function(k){
return vd[k] && typeof vd[k] === "object";
});
}
// 各 STAGE 行が「abort」相当か.
function _rowIsAbort(r){
if(r.kind === "abort") return true;
var lk = r.lookup;
if(lk && typeof lk === "object" && lk.guard_pass === false) return true;
return false;
}
// JSONL では 1 stage が複数行 (kind="stage"/"capture") に分かれるため
// stage 名で集約して 1 列に纏める. 出現順を保つ.
// - lookup / corr / verdict: 最初の非 null を採用
// - captures: 全行から concat
// - kind: 1 つでも "abort" があれば "abort"
function _aggregateStages(rows){
var order = [];
var byStage = {};
rows.forEach(function(r){
var s = r.phaseRaw;
if(!s) return;
if(!byStage[s]){
byStage[s] = {
phaseRaw: s, kind: "stage",
lookup: null, corr: null, verdict: null,
captures: [], angles: null,
};
order.push(s);
}
var agg = byStage[s];
if(r.kind === "abort") agg.kind = "abort";
if(r.lookup && !agg.lookup) agg.lookup = r.lookup;
if(r.corr && !agg.corr) agg.corr = r.corr;
if(r.verdict) agg.verdict = r.verdict;
if(Array.isArray(r.captures)){
// capture 行は state.captures が累積して書き込まれるため、複数行に同じ
// label が現れる. UI 側で label 単位に de-dup する（tooltip 重複防止）.
var already = {};
agg.captures.forEach(function(c){ if(c && c.label) already[c.label] = true; });
r.captures.forEach(function(c){
if(c && c.label && !already[c.label]){
agg.captures.push(c);
already[c.label] = true;
}
});
}
if(Array.isArray(r.angles) && !agg.angles) agg.angles = r.angles;
});
return order.map(function(s){ return byStage[s]; });
}
// レーンの種別と参照キー. name はデータ由来（basename か lookup_name）.
// 種別: "lookup_name" | "csv" | "corr" | "vlm" | "cap_top" | "cap_side"
function _makeLane(kind, name){
return {kind: kind, name: name, key: kind + ":" + name};
}
// (lane, row) から表示すべき記号 + クラス [+ title] を返す.
function _cellForLane(lane, r){
var lk = r.lookup, cr = r.corr;
var isAbort = _rowIsAbort(r);
var guardFail = (lk && lk.guard_pass === false);
var referenced = false;
var isRollback = false;
var title = null;
if(lane.kind === "lookup_name" && lk){
if(lk.lookup_name === lane.name) referenced = true;
} else if(lane.kind === "csv" && lk){
if(lk.source_file && _basename(lk.source_file) === lane.name) referenced = true;
} else if(lane.kind === "corr" && cr){
if(cr.memory_file && _basename(cr.memory_file) === lane.name){
referenced = true;
if(cr.apply_reason === "safety_rollback") isRollback = true;
}
} else if(lane.kind === "vlm"){
if(_rowHasVerdict(r)) referenced = true;
} else if(lane.kind === "cap_top" || lane.kind === "cap_side"){
var wantRole = (lane.kind === "cap_top") ? "top" : "side";
var caps = (Array.isArray(r.captures) ? r.captures : [])
.filter(function(c){ return c && c.camera_role === wantRole; });
if(caps.length > 0){
referenced = true;
// title 属性用: label + image パス (dry-run で image=null なら明示).
title = caps.map(function(c){
var lbl = c.label || "?";
var img = c.image ? String(c.image) : "no image (dry-run)";
return lbl + " → " + img;
}).join("\n");
return {sym: "◉", cls: "cell cap", title: title};
}
}
if(!referenced) return {sym: "·", cls: "cell dot"};
// 参照済み → 失敗マークが優先
if(isAbort || (guardFail && lane.kind !== "vlm" && lane.kind !== "corr")){
return {sym: "✕", cls: "cell cross"};
}
if(isRollback){
return {sym: "◇", cls: "cell diamond"};
}
return {sym: "●", cls: "cell mark"};
}
function _renderLanes(rowsRaw){
if(!els.lanesGrid) return;
if(!rowsRaw || rowsRaw.length === 0){
els.lanesGrid.innerHTML = "";
els.lanesGrid.style.setProperty("--ncols", "1");
if(els.lanesEmpty) els.lanesEmpty.style.display = "";
return;
}
if(els.lanesEmpty) els.lanesEmpty.style.display = "none";
// JSONL は 1 stage が複数行に分かれるため集約して 1 列 = 1 stage にする.
var rows = _aggregateStages(rowsRaw);
if(rows.length === 0){
els.lanesGrid.innerHTML = "";
els.lanesGrid.style.setProperty("--ncols", "1");
if(els.lanesEmpty) els.lanesEmpty.style.display = "";
return;
}
// レーン集合を出現順で収集. vlm_judge / cam:top / cam:side は固定行.
var lanes = [];
var seen = {};
function addLane(kind, name){
if(!name) return;
var key = kind + ":" + name;
if(seen[key]) return;
seen[key] = true;
lanes.push(_makeLane(kind, name));
}
// 1) fk_lookup / j0_lookup 等の lookup_name はデータに現れた分だけ拾う.
// j0_lookup は FK 化で全 STAGE 消灯している (役目終了) ため固定行化を撤回.
// 再び使われればデータに現れて自動で行が復活する.
rows.forEach(function(r){
var lk = r.lookup;
if(lk && typeof lk === "object" && lk.lookup_name){
addLane("lookup_name", String(lk.lookup_name));
}
});
// 2) csv basename
rows.forEach(function(r){
var lk = r.lookup;
if(lk && typeof lk === "object" && lk.source_file){
addLane("csv", _basename(lk.source_file));
}
});
// 3) corr memory basename
rows.forEach(function(r){
var cr = r.corr;
if(cr && typeof cr === "object" && cr.memory_file){
addLane("corr", _basename(cr.memory_file));
}
});
// 4) vlm_judge レーンは固定
addLane("vlm", "vlm_judge");
// 5) 撮影レーンは固定 2 行 (Phase 3-B). 一度も撮っていなければ薄字.
addLane("cap_top", "cam:top");
addLane("cap_side", "cam:side");
// レーンごとに「一度でも参照されたか」を判定（未参照レーンは薄字にする）.
var laneUsed = lanes.map(function(lane){
return rows.some(function(r){
var c = _cellForLane(lane, r);
return c.sym !== "·";
});
});
// 列数 = STAGE 数を CSS 変数へ.
els.lanesGrid.style.setProperty("--ncols", String(rows.length));
// ---- ヘッダ行: 左端 "STAGE" + 各 STAGE の短縮ラベル ----
var html = '<div class="lane-corner">STAGE</div>';
rows.forEach(function(r, i){
var label = _shortStageLabel(r.phaseRaw);
var cls = "stage-head" + (_rowIsAbort(r) ? " abort" : "");
html += '<div class="' + cls + '" data-stage-idx="' + i + '" ' +
'data-stage="' + _escapeHtml(r.phaseRaw || "") + '" ' +
'title="' + _escapeHtml(r.phaseRaw || "") + '">' +
_escapeHtml(label) + '</div>';
});
// ---- レーン行 ----
lanes.forEach(function(lane, li){
var unused = !laneUsed[li];
var nameCls = "lane-name" + (unused ? " unused" : "");
html += '<div class="' + nameCls + '">' + _escapeHtml(lane.name) + '</div>';
rows.forEach(function(r, si){
var c = _cellForLane(lane, r);
var cls = c.cls + (unused ? " unused" : "");
var titleAttr = c.title
? ' title="' + _escapeHtml(c.title) + '"'
: '';
html += '<div class="' + cls +
'" data-stage="' + _escapeHtml(r.phaseRaw || "") + '" ' +
'data-stage-idx="' + si + '"' + titleAttr + '>' +
c.sym + '</div>';
});
});
els.lanesGrid.innerHTML = html;
// 現在のハイライトを再適用（再描画で class が消えるため）
if(state.highlightedStage){
_setLaneHighlight(state.highlightedStage);
}
}
// 指定 STAGE 名の列（stage-head + cell 群）に .hi を付ける.
// stage=null / 空文字 は解除のみ.
function _setLaneHighlight(stage){
var grid = els.lanesGrid;
if(!grid) return;
var prev = grid.querySelectorAll(".hi");
for(var i = 0; i < prev.length; i++) prev[i].classList.remove("hi");
state.highlightedStage = stage || null;
if(!stage) return;
// 属性セレクタでは特殊文字を含む可能性があるので querySelectorAll で
// 直接指定するより、全 [data-stage] を走査して一致するものだけ toggle する.
var all = grid.querySelectorAll("[data-stage]");
for(var j = 0; j < all.length; j++){
if(all[j].getAttribute("data-stage") === stage){
all[j].classList.add("hi");
}
}
}
function _renderStageHistory(){
if(state.reviewing) return; // レビュー中はライブ履歴で表示を上書きしない
_renderStageList(state.stageHistory);
_renderLanes(state.stageHistory);
}
function resetStageHistory(){
state.stageHistory = [];
state.historyLastPhase = null;
_renderStageHistory();
}
// 新 run（trial.id 変化）で過去 trial レコードを確定し、新レコードを開始する.
function _startNewTrialRecord(trialId, dg){
var rec = {
id: trialId,
task: (dg && dg.trial && dg.trial.task!=null) ? String(dg.trial.task) : null,
stages: [],
lastDg: null,
inBox: null,
startedAt: Date.now(),
lastAt: Date.now()
};
state.currentTrialId = trialId;
state.currentRecord = (trialId !== null) ? rec : null;
if(trialId !== null){
state.trials.push(rec);
state.trialsById[trialId] = rec;
if(state.trials.length > TRIALS_MAX){
var removed = state.trials.splice(0, state.trials.length - TRIALS_MAX);
removed.forEach(function(r){ if(r.id!=null) delete state.trialsById[r.id]; });
}
}
// ライブ表示用 stageHistory を新レコードの stages に張り替える（参照）.
state.stageHistory = rec.stages;
state.historyLastPhase = null;
renderTrialNav();
}
function appendStageHistory(dg){
// 新 run（trial.id 変化）で過去 trial を確定し、新レコードへ切替
var trialId = (dg && dg.trial && dg.trial.id != null) ? String(dg.trial.id) : null;
if(trialId !== state.historyLastTrialId){
state.historyLastTrialId = trialId;
_startNewTrialRecord(trialId, dg);
// in_box の last known も新 run でリセット（次の run の stage9b で再確定）
state.lastInBox = null;
_renderStageHistory();
}
var phaseRaw = (dg && dg.phase != null) ? String(dg.phase) : null;
if(phaseRaw === null) return; // phase 無しはスキップ
// 同じ phase の UDP frame でも, capture 発火など mid-stage 更新は取り込む.
// stage_lookup_and_move で発火する _observe_capture_push は同 phase のまま
// captures を積んで emit するので、LIVE 側でもその captures を最終行にマージする.
if(phaseRaw === state.historyLastPhase){
if(dg && Array.isArray(dg.captures) && dg.captures.length > 0
&& state.stageHistory.length > 0){
var last = state.stageHistory[state.stageHistory.length - 1];
if(last && last.phaseRaw === phaseRaw){
var seenLabels = {};
(last.captures || []).forEach(function(c){ if(c) seenLabels[c.label] = true; });
var merged = (last.captures || []).slice();
dg.captures.forEach(function(c){
if(c && !seenLabels[c.label]){
merged.push(c);
seenLabels[c.label] = true;
}
});
last.captures = merged;
renderTrialNav();
_renderStageHistory();
}
}
return;
}
state.historyLastPhase = phaseRaw;
var angles = (dg && Array.isArray(dg.angles)) ? dg.angles.slice(0, 6) : null;
// LIVE 行にも lookup/corr/captures を含める. UDP datagram には kind が無いので
// "stage" 固定. abort 情報は JSONL fetch 後に上書きされて正しく表示される.
state.stageHistory.push({
t: (typeof dg.t === "number") ? dg.t : Date.now(),
phaseRaw: phaseRaw,
gripperRaw: dg.gripper,
angles: angles,
kind: "stage",
lookup: (dg && dg.lookup && typeof dg.lookup === "object") ? dg.lookup : null,
corr: (dg && dg.corr && typeof dg.corr === "object") ? dg.corr : null,
captures: (dg && Array.isArray(dg.captures)) ? dg.captures.slice() : null
});
// 上限超過は古い行を捨てる
if(state.stageHistory.length > STAGE_HISTORY_MAX){
state.stageHistory.splice(0, state.stageHistory.length - STAGE_HISTORY_MAX);
}
renderTrialNav(); // stage 数表示を更新
_renderStageHistory();
}
// ---- サイドバー: 過去 TRIAL ナビ ---------------------------------------
function renderTrialNav(){
var trials = state.trials;
if(!trials || trials.length === 0){
els.trialList.innerHTML = "";
els.trialEmpty.style.display = "";
} else {
els.trialEmpty.style.display = "none";
// 新しい trial を上に
var html = "";
for(var i = trials.length - 1; i >= 0; i--){
var r = trials[i];
var isActive = state.reviewing && (r.id === state.reviewId);
var isLive = (!state.reviewing) && (r.id === state.currentTrialId);
var cls = "trial-item" + (isActive ? " active" : "");
var idLabel = (r.id!=null ? "#"+r.id : "#—") + (isLive ? " ●" : "");
var taskLabel = r.task ? r.task : "—";
// 種蒔き（server seed）で stages 未フェッチのときは stagesCount を優先.
var n = (r.stages && r.stages.length) ? r.stages.length :
(typeof r.stagesCount === "number") ? r.stagesCount : 0;
html += '<li><button type="button" class="'+cls+'" data-trial-id="'+_escapeHtml(r.id)+'">' +
'<span class="ti-id">'+_escapeHtml(idLabel)+'</span>' +
'<span class="ti-task">'+_escapeHtml(taskLabel)+'</span>' +
'<span class="ti-meta">'+n+' stage'+(n===1?'':'s')+'</span>' +
'</button></li>';
}
els.trialList.innerHTML = html;
}
els.trialLiveBtn.classList.toggle("dim", state.reviewing);
}
// JSONL 行を _historyRowHTML が読める形に正規化する.
// 実測スキーマ:
// {t, kind, stage, phase_t0, trial_id, angles, gripper,
// lookup, corr, block_xz, box_xz, block_place_xz, tip_pick, verdict}
function _jsonlRowToHistory(r){
return {
t: (typeof r.t === "number") ? r.t : null,
phaseRaw: (r.stage != null) ? String(r.stage) : null,
gripperRaw: r.gripper,
angles: Array.isArray(r.angles) ? r.angles.slice(0, 6) : null,
kind: (r.kind != null) ? String(r.kind) : "stage",
lookup: (r.lookup && typeof r.lookup === "object") ? r.lookup : null,
corr: (r.corr && typeof r.corr === "object") ? r.corr : null,
// Phase 3-B: captures を残す（各要素 {label, camera_role, image}）.
captures: Array.isArray(r.captures) ? r.captures : null,
// レーン図で使うため verdict / box 座標も残す
verdict: r.verdict || null,
block_xz: r.block_xz || null,
box_xz: r.box_xz || null,
block_place_xz: r.block_place_xz || null,
};
}
// GET /runs/{stem}/stages で JSONL を取り、正規化して返す（Promise）.
// ネットワーク失敗・404 は null にフォールバック（呼び出し側でローカル参照）.
function _fetchStagesJSONL(stem){
return fetch("/runs/" + encodeURIComponent(stem) + "/stages", {
cache: "no-store"
}).then(function(res){
if(!res.ok) return null;
return res.json();
}).then(function(js){
if(!js || !Array.isArray(js.rows)) return null;
return js.rows.map(_jsonlRowToHistory);
}).catch(function(){ return null; });
}
// ページ初回ロード時に KIOXIA の run 一覧を取り、サイドバーに種を蒔く.
// LIVE trial のレコードは trial.id 変化で別途生成される. すでに live で作られた
// レコードは上書きしない（同じ stem のとき早期 return）.
// これで「ブラウザリロード後もサイドバーから過去 run を選べる」を成立させる.
function _seedTrialFromServer(stem, stagesCount){
if(state.trialsById[stem]) return;
var rec = {
id: stem,
task: null,
stages: [], // enterReview で JSONL fetch により埋める
lastDg: null,
inBox: null,
startedAt: null,
lastAt: null,
stagesCount: stagesCount // 表示用の総数（未フェッチのため stages.length は 0）
};
state.trials.unshift(rec); // 古い順で並べるため先頭挿入（renderTrialNav が反転表示）
state.trialsById[stem] = rec;
}
function _loadServerRuns(){
fetch("/runs", {cache:"no-store"}).then(function(res){
if(!res.ok) return null;
return res.json();
}).then(function(js){
if(!js || !Array.isArray(js.runs)) return;
// /runs は新しい順. 種蒔きは古い順に seed して先頭挿入することで
// renderTrialNav の逆順表示で新しい方が上に来る.
var runs = js.runs.slice().reverse();
runs.forEach(function(r){
_seedTrialFromServer(r.stem, r.stages);
});
renderTrialNav();
}).catch(function(){});
}
// 過去 trial を選択して凍結表示（レビュー）.
// まずローカル state（WS 由来）で即時描画し、その後 JSONL fetch が成功したら
// JSONL 版で置き換える（記録の正）. リロードしても消えないのは JSONL が原本のため.
function enterReview(trialId){
var rec = state.trialsById[trialId];
if(!rec) return;
state.reviewing = true;
state.reviewId = trialId;
els.body.classList.add("reviewing");
els.reviewId.textContent = (rec.id!=null) ? String(rec.id) : "—";
// レビュー開始時はハイライトを解除（ユーザーが列を選ぶまで無強調）.
_setLaneHighlight(null);
// 履歴リスト（凍結・ローカル版で即時描画）
_renderStageList(rec.stages);
_renderLanes(rec.stages);
// パネル群をレコードの最終 datagram から再現
var inBoxText = (rec.inBox === null) ? "—" : String(rec.inBox);
paintGripperValue(rec.lastDg ? rec.lastDg.gripper : null);
paintFromDatagram(rec.lastDg || {}, inBoxText);
els.seq.textContent = (rec.lastDg && rec.lastDg.seq!=null) ? String(rec.lastDg.seq) : "—";
renderTrialNav();
// JSONL からの正版を取りに行き、成功したら差し替える（記録の正）.
_fetchStagesJSONL(trialId).then(function(rows){
// fetch 中にレビュー抜けた/別 trial 選択 の場合は無視.
if(!state.reviewing || state.reviewId !== trialId) return;
if(rows == null) return;
_renderStageList(rows);
_renderLanes(rows);
});
}
// ライブ表示へ戻す.
function exitReview(){
if(!state.reviewing) return;
state.reviewing = false;
state.reviewId = null;
els.body.classList.remove("reviewing");
// ライブ履歴を再描画
_renderStageList(state.stageHistory);
_renderLanes(state.stageHistory);
// LIVE 復帰: 現 phase 列をハイライト（あれば）
_setLaneHighlight(state.phase);
if(state.idle){
clearAllLiveValues();
} else if(state.last_dg){
renderGripper();
var inBoxText = (state.lastInBox === null) ? "—" : String(state.lastInBox);
paintFromDatagram(state.last_dg, inBoxText);
els.seq.textContent = (state.last_dg.seq!=null) ? String(state.last_dg.seq) : "—";
}
renderTrialNav();
}
// ---- サイドバー: パネル表示/非表示 + レイアウト再構成 ---------------------
var mqNarrow = window.matchMedia("(max-width: 640px)");
function isNarrow(){ return mqNarrow.matches; }
// 表示中の glance セルだけを詰めて配置（非表示で穴が空かないようにする）.
// 広い画面: 半幅(phase/gripper)を2列に詰め、全幅(joints/trial)は行を専有.
// 狭い画面(SP): 全セルを縦1列で順に並べる.
function relayoutGlance(){
var order = [
{ key:"phase", el:els.cellPhase, type:"half" },
{ key:"gripper", el:els.cellGripper, type:"half" },
{ key:"joints", el:els.cellJoints, type:"full" },
{ key:"trial", el:els.cellTrial, type:"full" }
];
var narrow = isNarrow();
var row = 1, pending = [];
function place(el, col, r){ el.style.gridColumn = col; el.style.gridRow = String(r); }
function flush(){
if(pending.length === 1){ place(pending[0].el, "1 / -1", row); row++; }
else if(pending.length === 2){ place(pending[0].el, "1", row); place(pending[1].el, "2", row); row++; }
pending = [];
}
order.forEach(function(o){
if(!o.el) return;
if(state.panelHidden[o.key]){ o.el.style.display = "none"; return; }
o.el.style.display = ""; // .cell の display:flex に戻す
if(narrow){ place(o.el, "1 / -1", row); row++; return; }
if(o.type === "half"){ pending.push(o); if(pending.length === 2) flush(); }
else { flush(); place(o.el, "1 / -1", row); row++; }
});
if(!narrow) flush();
}
function applyPanelVisibility(){
relayoutGlance();
if(els.drill) els.drill.style.display = state.panelHidden.drill ? "none" : "";
}
function setPanelHidden(key, hidden){
if(!(key in state.panelHidden)) return;
state.panelHidden[key] = !!hidden;
var lbl = els.panelToggles.querySelector('[data-panel-label="'+key+'"]');
if(lbl) lbl.classList.toggle("off", !!hidden);
applyPanelVisibility();
}
// ---- gripper デバウンス --------------------------------------------------
// 受信のたびに即表示すると open↔close 高速切替で逆値が一瞬見えてちらつくため、
// 同じ値が GRIPPER_DEBOUNCE_MS 連続したら commit する.
// null（未受信）は仕様により即 commit（従来挙動）.
function processGripperRx(v){
var nv = (v === 0 || v === 1) ? v : null;
if(nv === null){
// 未受信は即反映（従来挙動）
state.gripperConfirmed = null;
state.gripperPending = null;
state.gripperPendingSince = null;
return;
}
if(nv === state.gripperConfirmed){
// 既に確定値と同じ → pending を消して安定状態に戻す
state.gripperPending = null;
state.gripperPendingSince = null;
return;
}
if(nv !== state.gripperPending){
// 新しい pending 値を観測開始
state.gripperPending = nv;
state.gripperPendingSince = Date.now();
}
// else: 同じ pending が続いている → since は据え置き
}
function tryCommitGripper(){
if(state.gripperPending === null) return false;
if(state.gripperPendingSince === null) return false;
if((Date.now() - state.gripperPendingSince) < GRIPPER_DEBOUNCE_MS) return false;
state.gripperConfirmed = state.gripperPending;
state.gripperPending = null;
state.gripperPendingSince = null;
return true;
}
function renderGripper(){
var g = fmtGripper(state.gripperConfirmed);
els.gripper.textContent = g.text;
els.gripper.className = "gripper-val " + g.cls;
}
function setConn(text, down){
els.conn.innerHTML = '<span class="conn-dot"></span>'+text;
if(down) els.conn.classList.add("conn-down"); else els.conn.classList.remove("conn-down");
}
// ---- IDLE: ライブ値を全て空にする＋PHASE に待機明示 --------------------
// LAST / DELTA / SEQ / CONN は触らない（接続メタは生かす）.
// gripper デバウンス state も同時にリセット（idle 復帰時に古い pending が commit しない）.
function clearAllLiveValues(){
// PHASE: 待機を明示
els.phase.textContent = "待機中 — no active run";
els.elapsed.textContent = "—";
// GRIPPER: 反転スタイル(.close)も解除
els.gripper.textContent = "—";
els.gripper.className = "gripper-val";
state.gripperConfirmed = null;
state.gripperPending = null;
state.gripperPendingSince = null;
// JOINTS J0..J5
for(var i=0; i<6; i++) els.gJoints[i].textContent = "—";
// TRIAL
els.trialId.textContent = "—";
els.trialTask.textContent = "—";
// DRILL: angles (j1..j6)
[els.j1, els.j2, els.j3, els.j4, els.j5, els.j6].forEach(function(e){
e.textContent = "—";
});
// DRILL: ee
els.eeX.textContent = "—"; els.eeY.textContent = "—"; els.eeZ.textContent = "—";
// DRILL: verdict 3 スロット
[els.vPickRaw, els.vPickConf, els.vPickReason,
els.vPlaceRaw, els.vPlaceConf, els.vPlaceReason,
els.vSideRaw, els.vSideConf, els.vSideReason].forEach(function(e){
if(e) e.textContent = "—";
});
// DRILL: coords
els.cBlockPickX.textContent = "—"; els.cBlockPickZ.textContent = "—";
els.cBlockPlaceX.textContent = "—"; els.cBlockPlaceZ.textContent = "—";
els.cBoxX.textContent = "—"; els.cBoxZ.textContent = "—";
els.cInBox.textContent = "—";
// GLANCE: gripper セル下の coords も idle 時クリア
els.gBlockPickX.textContent = "—"; els.gBlockPickZ.textContent = "—";
els.gBlockPlaceX.textContent = "—"; els.gBlockPlaceZ.textContent = "—";
els.gBoxX.textContent = "—"; els.gBoxZ.textContent = "—";
els.gInBox.textContent = "—";
// DRILL: datagram meta
els.rSource.textContent = "—"; els.rT.textContent = "—";
els.rTTime.textContent = "—"; els.rPhaseT0.textContent = "—";
els.rPhaseT0Time.textContent = "—"; els.rTrialId.textContent = "—";
els.rTrialTask.textContent = "—";
// DRILL: raw json
els.rJson.textContent = "—";
}
// idle / held を評価して DOM クラスを切替.
// idle: WS 切断 or 未受信 or 最後の受信から HOLD_IDLE_MS 超 → 値を全て「—」.
// held: idle ではないが最後の受信から STALE_MS 超 → 値はラッチ保持, HELD バッジを出す.
// 谷間で前 phase の値を消さないのが核. 鮮度のみで idle にしていた旧版を置換.
function evaluateIdle(){
var now = Date.now();
var nowIdle = (!state.wsOpen) ||
(state.last_received_at === null) ||
((now - state.last_received_at) > HOLD_IDLE_MS);
var nowHeld = !nowIdle &&
(state.last_received_at !== null) &&
((now - state.last_received_at) > STALE_MS);
// idle 状態遷移. 落ちた瞬間だけ全クリア.
if(nowIdle !== state.idle){
state.idle = nowIdle;
els.body.classList.toggle("idle", nowIdle);
if(nowIdle){
// レビュー中は凍結表示を保つためクリアしない
if(!state.reviewing) clearAllLiveValues();
}
// 復帰時はここで描画しない: 次の applyDatagram がライブ値で埋める.
}
// held 状態遷移. クリアは絶対にしない（値はラッチ保持）.
if(nowHeld !== state.held){
state.held = nowHeld;
els.body.classList.toggle("held", nowHeld);
}
}
// ---- 経過秒のライブ化(setInterval で再計算)----
// 100ms ティッカー: まず IDLE 判定 → idle なら経過秒を進めず即 return.
// 「データが来ていないのに値だけ動いて見える」状態を消す.
function tickElapsed(){
evaluateIdle();
if(state.idle){
// clearAllLiveValues で elapsed="—" にしてある（遷移時のみ）。
// 念のため毎 tick で確実に空にしておく.
els.elapsed.textContent = "—";
return;
}
if(state.phase_t0==null){
els.elapsed.textContent = "—";
return;
}
var now = Date.now();
var dt = (now - state.phase_t0) / 1000;
if(!isFinite(dt) || dt < 0){
els.elapsed.textContent = "—";
return;
}
els.elapsed.textContent = dt.toFixed(1);
}
setInterval(tickElapsed, ELAPSED_TICK_MS);
// ---- delta/stale 判定(server_now と received_at から)----
function tickStale(){
if(state.server_now!=null && state.last_received_at!=null){
var delta = state.server_now - state.last_received_at;
els.delta.textContent = (delta/1000).toFixed(1)+"s";
if(delta > STALE_MS) els.body.classList.add("stale"); else els.body.classList.remove("stale");
}
}
// ---- datagram 1 個を描く ----
function applyDatagram(dg){
if(!dg) return;
// IDLE / HELD 即時解除（100ms tick を待たない. チラ見えを防ぐ）.
if(state.idle){
state.idle = false;
els.body.classList.remove("idle");
}
if(state.held){
state.held = false;
els.body.classList.remove("held");
}
// ---- グランス: phase 履歴を積む（背景の trial レコードへも積む） ----
// 古い行が上、最新が下. 同 phase の連続 datagram は 1 行のみ. trial.id 変化でリセット.
var phaseRaw = (dg.phase!=null) ? String(dg.phase) : null;
state.phase = phaseRaw;
state.phase_t0 = (typeof dg.phase_t0 === "number") ? dg.phase_t0 : null;
els.phase.textContent = fmtPhase(phaseRaw); // idle 復帰判定用（普段は display:none）
appendStageHistory(dg);
// 参照レーン図の現在列ハイライトを LIVE で追従（レビュー中は上書きしない）
if(!state.reviewing) _setLaneHighlight(phaseRaw);
tickElapsed();
// ---- in_box latch（背景状態. レビュー中でも更新し続ける） ----
if(dg.in_box === true || dg.in_box === false){
state.lastInBox = dg.in_box;
}
var inBoxText = (state.lastInBox === null) ? "—" : String(state.lastInBox);
// ---- 現 trial レコードへ最終 datagram を保存（過去レビュー再現用） ----
if(state.currentRecord){
state.currentRecord.lastDg = dg;
state.currentRecord.lastAt = Date.now();
state.currentRecord.inBox = state.lastInBox;
if(state.currentRecord.task == null && dg.trial && dg.trial.task != null){
state.currentRecord.task = String(dg.trial.task);
}
}
// ---- gripper（デバウンス: 確定値だけ表示） ----
// 状態は常に進める. 描画はレビュー中のみ抑止（凍結表示を保つ）.
processGripperRx(dg.gripper);
tryCommitGripper();
state.last_dg = dg; // exitReview でライブへ戻すために保持
// ---- レビュー中はパネル描画を止める（背景 state だけ更新済み） ----
if(state.reviewing){
renderTrialNav(); // stage 数等の更新は反映
return;
}
renderGripper();
paintFromDatagram(dg, inBoxText);
// ---- seq ----
els.seq.textContent = (dg.seq!=null) ? String(dg.seq) : "—";
}
// ---- WS ----
function connect(){
var url = (location.protocol === "https:" ? "wss://" : "ws://") + location.host + "/ws";
setConn("connecting…", true);
state.wsOpen = false;
evaluateIdle();
var ws = new WebSocket(url);
ws.onopen = function(){
setConn("open", false);
state.wsOpen = true;
evaluateIdle();
};
ws.onmessage = function(evt){
var msg;
try { msg = JSON.parse(evt.data); } catch(e){ return; }
state.server_now = (typeof msg.server_now_ms === "number") ? msg.server_now_ms : null;
var recvAt = (typeof msg.received_at_ms === "number") ? msg.received_at_ms : null;
var dg = msg.datagram;
if(recvAt!=null) state.last_received_at = recvAt;
els.last.textContent = fmtTime(state.last_received_at);
if(dg && (!state.last_dg || dg.seq !== state.last_dg.seq)){
state.last_dg = dg;
applyDatagram(dg);
} else if(dg && !state.last_dg){
state.last_dg = dg;
applyDatagram(dg);
}
tickStale();
};
ws.onclose = function(){
setConn("closed — reconnect 1s", true);
els.body.classList.add("stale");
state.wsOpen = false;
evaluateIdle(); // WS 切断 → 即 idle 判定
setTimeout(connect, 1000);
};
ws.onerror = function(){
setConn("error", true);
state.wsOpen = false;
evaluateIdle();
};
}
// server から push が止まっても stale 判定を独自に進める
setInterval(function(){
if(state.last_received_at!=null){
var now = Date.now();
var localDelta = now - state.last_received_at;
if(localDelta > STALE_MS) els.body.classList.add("stale");
}
}, 250);
// gripper デバウンス commit: datagram の谷間でも DEBOUNCE_MS 経過を判定して表示更新.
// applyDatagram は新 seq でしか走らないため、同値の datagram が続くと commit 機会を失う.
setInterval(function(){
if(tryCommitGripper() && !state.reviewing) renderGripper();
}, GRIPPER_TICK_MS);
// ---- サイドバー配線 ----------------------------------------------------
// パネル表示/非表示トグル
els.panelToggles.addEventListener("change", function(evt){
var cb = evt.target;
if(!cb || cb.getAttribute("type") !== "checkbox") return;
var key = cb.getAttribute("data-panel");
setPanelHidden(key, !cb.checked);
});
// 過去 TRIAL を選んでレビュー
els.trialList.addEventListener("click", function(evt){
var btn = evt.target.closest ? evt.target.closest(".trial-item") : null;
if(!btn) return;
var id = btn.getAttribute("data-trial-id");
if(id != null) enterReview(id);
});
// ライブへ戻す
els.trialLiveBtn.addEventListener("click", exitReview);
els.reviewExit.addEventListener("click", exitReview);
// SP: サイドバー開閉
els.sbToggle.addEventListener("click", function(){
var collapsed = els.sbBody.classList.toggle("collapsed");
els.sbToggle.setAttribute("aria-expanded", String(!collapsed));
});
// 画面幅の変化で glance を組み直す（SP↔広い）
if(mqNarrow.addEventListener) mqNarrow.addEventListener("change", relayoutGlance);
else if(mqNarrow.addListener) mqNarrow.addListener(relayoutGlance);
// パスのクリックで Finder リビール（委譲）
document.addEventListener("click", function(evt){
var el = evt.target.closest ? evt.target.closest(".gc-path.linkable") : null;
if(!el) return;
var pth = el.getAttribute("data-path");
if(pth) revealPath(pth);
});
// 参照レーン図: 列見出し（stage-head）クリックで列ハイライト.
// LIVE 中でも同じセレクタが効くが、実運用は主にレビュー中の使用を想定.
if(els.lanesGrid){
els.lanesGrid.addEventListener("click", function(evt){
var h = evt.target.closest ? evt.target.closest(".stage-head") : null;
if(!h) return;
var stage = h.getAttribute("data-stage");
if(stage) _setLaneHighlight(stage);
});
}
// STAGE HISTORY 行クリックでも同じ列をハイライト（履歴 ⇄ マトリクスの連動）.
if(els.stageHistory){
els.stageHistory.addEventListener("click", function(evt){
var row = evt.target.closest ? evt.target.closest(".stage-history-row") : null;
if(!row) return;
var stage = row.getAttribute("data-stage");
if(stage) _setLaneHighlight(stage);
});
}
// 初期描画
renderTrialNav();
applyPanelVisibility();
// サーバから過去 run を非同期でシード（リロード後もサイドバーから選択可能に）.
// 失敗しても LIVE には影響しない.
_loadServerRuns();
connect();
})();
</script>
<div id="path-toast"></div>
</body>
</html>
vlm_judge.py
vlm_judge.py — VLM-as-Reward 判定器。place_side プロンプトを「箱に収まっているか」から「爪と白箱の隙間（接触の有無）」へ書き換えた版。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""vlm_judge.py — Claude（Anthropic API）ビジョンで pick / place 成否を判定
公開関数:
judge(image_path: str, kind: str) -> dict
戻り: {"verdict": "yes|no|unknown",
"confidence": float (0.0-1.0),
"reason": str,
"model": str,
"raw": str}
挙動:
- ANTHROPIC_API_KEY 環境変数から API キーを読む。未設定なら verdict=unknown / reason="no_api_key"。
例外で落とさない（呼び出し側＝run_pick_place は観察のみで通しを止めないため）。
- HTTP タイムアウト 30s。例外/タイムアウト/parse 失敗 → verdict=unknown（raw 保持）。
- 応答は JSON のみ返すようプロンプト指示。json.loads 失敗時は最初の {…} 抽出で再試行。
CLI（単体テスト）:
python3 vlm_judge.py --image path/to/lift_grip_*.jpg --kind pick
python3 vlm_judge.py --image path/to/box_from_*.jpg --kind place
絶対ルール:
- API キーをコード/ログ/git に残さない（環境変数のみ・raw にもキー文字列を載せない）
- 呼び出し側の関節制御を変えない（判定はあくまで観察情報）
"""
from __future__ import print_function
import argparse
import base64
import json
import os
import re
import sys
import urllib.error
import urllib.request
# ---- 定数 ----
VLM_MODEL = "claude-sonnet-4-6" # ビジョン対応・現行 Sonnet
VLM_API_URL = "https://api.anthropic.com/v1/messages"
VLM_ANTHROPIC_VERSION = "2023-06-01"
VLM_TIMEOUT_SEC = 30.0
VLM_MAX_TOKENS = 512
PROMPTS = {
# 2 画像 (リフト前 + リフト後) で「浮き判定」する通常時のプロンプト。
"pick": (
"2 枚の画像はロボットアームを上から見たもの。"
"1 枚目は掴む直前（リフト前）、2 枚目はリフト後。"
"リフト後、赤ブロックがグリッパーと一緒に上がり床から浮いているかを判定してください。\n"
"以下の JSON のみで返答してください。前置きや後置きの説明は一切含めないこと。\n"
'{"verdict": "yes|no|unknown", "confidence": 0.0-1.0 の数値, "reason": "短い理由"}\n'
"判定基準:\n"
"- yes: 2 本の爪の間に挟まれ赤ブロックが一緒に持ち上がっている\n"
"- no: 床に残っている／ずり落ちている／爪の外側に触れているだけ\n"
"- unknown: 画像が不鮮明・遮蔽等で判別不能"
),
# 片方の画像しか撮れていない場合 (リフト前 or リフト後 のどちらか) の
# フォールバック。浮き判定は不可能なので、爪の挟み込みだけ見る。
"pick_single": (
"画像はロボットアームの爪を上から見たもの。"
"2 本の爪の間に赤ブロックが挟まれて保持されているかを判定してください。\n"
"以下の JSON のみで返答してください。前置きや後置きの説明は一切含めないこと。\n"
'{"verdict": "yes|no|unknown", "confidence": 0.0-1.0 の数値, "reason": "短い理由"}\n'
"判定基準:\n"
"- yes: 2 本の爪の間に赤ブロックが収まり挟み込まれている\n"
"- no: 爪の外側に触れているだけ／床に残っている／爪間に何もない\n"
"- unknown: 画像が不鮮明・遮蔽等で判別不能"
),
"place": (
"画像は白い箱を上から見たもの。"
"箱の中に赤い四角のブロックが入っているか判定してください。\n"
"以下の JSON のみで返答してください。前置きや後置きの説明は一切含めないこと。\n"
'{"verdict": "yes|no|unknown", "confidence": 0.0-1.0 の数値, "reason": "短い理由"}\n'
"判定基準:\n"
"- yes: 箱の中に赤いブロックがある（少なくとも一部が箱の内側）\n"
"- no: 箱は空、または赤いブロックが箱の外にある\n"
"- unknown: 画像が不鮮明・遮蔽等で判別不能"
),
# 横カメラ (Iriun index=1) からの PLACE 状態判定 (release 前).
# 段階1 再修正 (2026-07-11): 判定軸を「接触の有無」1点だけに絞る.
# 前回版は水平位置 (misaligned) や高さ (too_deep) を混ぜたが、横カメラは
# 奥行きが見えないため水平位置は判定不能で、4 枚テストで全滅した.
# 横カメラで確実に見えるのは「爪と箱の縁の隙間があるか (＝接触の有無)」のみ.
# 水平位置は上カメラ (place の top view) の領分. 判定軸を分離する.
# 段階2 でこの no を place_height_memory の dee_y 補正へ接続する予定.
"place_side": (
"画像はロボットアームの作業台を横から見たもの。"
"白い爪（グリッパー）が赤いブロックを掴んで白い箱の近くまで下りてきた状態。"
"判定するのは1点だけ：グリッパーの爪と白い箱の縁との間に、"
"ぶつからない適切な隙間があるか。\n"
"水平方向・奥行き方向の位置は判定しない（それは別カメラの役割）。"
"爪と箱が縦方向に接触しているかどうかだけを見る。\n"
"以下の JSON のみで返答してください。前置きや後置きの説明は一切含めないこと。\n"
'{"verdict": "yes|no|unknown", "confidence": 0.0-1.0 の数値, "reason": "短い理由"}\n'
"判定基準:\n"
"- yes: 爪の底が箱の上縁より上にあり、爪と箱の間に隙間がある。"
"箱にぶつからず安全に離せる\n"
"- no: 爪と箱の間に隙間がない。爪が箱の縁や壁に接触している／乗っている／"
"めり込んでいる、または箱が爪に押されて動いている\n"
"- unknown: 遮蔽・不鮮明で爪と箱の隙間が判別できない"
),
}
# ---- ヘルパ ----
def _unknown(reason, raw=""):
return {
"verdict": "unknown",
"confidence": 0.0,
"reason": reason,
"model": VLM_MODEL,
"raw": raw,
}
def _coerce_verdict(v):
if not isinstance(v, str):
return "unknown"
s = v.strip().lower()
return s if s in ("yes", "no", "unknown") else "unknown"
def _coerce_confidence(c):
try:
f = float(c)
except (TypeError, ValueError):
return 0.0
if f < 0.0:
return 0.0
if f > 1.0:
return 1.0
return f
def _parse_json_from_text(text):
"""まず text 全体を json.loads。失敗時は最初の {…} を greedy に取り出して再試行."""
if not isinstance(text, str):
return None
try:
return json.loads(text)
except (ValueError, TypeError):
pass
# 最初の { から最後の } までを取り出して試す（マルチライン対応）
m = re.search(r"\{.*\}", text, re.DOTALL)
if m:
try:
return json.loads(m.group(0))
except (ValueError, TypeError):
pass
return None
def _media_type_for(image_path):
ext = os.path.splitext(image_path)[1].lower()
if ext == ".png":
return "image/png"
if ext in (".jpg", ".jpeg"):
return "image/jpeg"
if ext == ".webp":
return "image/webp"
if ext == ".gif":
return "image/gif"
return "image/jpeg" # フォールバック
# ---- 公開関数 ----
def judge(image_paths, kind):
"""画像 (1 枚 or 複数) と kind を渡して Claude ビジョン判定を取得する。
image_paths: str (1 画像・後方互換) or list/tuple of str (順序保持で複数画像)。
pick 判定では [tip_image_path, lift_image_path] の 2 枚を渡すこと
(1 枚目=リフト前, 2 枚目=リフト後)。
エラー時は verdict=unknown を返す（例外で落とさない）。
"""
if kind not in PROMPTS:
return _unknown("unknown_kind:{}".format(kind))
# DAY265: 環境変数優先、無ければ ~/.anthropic_key (1 行の平文キー) から読む.
# 実行環境 (Codex snapshot 除外時など) で env が抜けても動くように.
api_key = os.environ.get("ANTHROPIC_API_KEY")
if not api_key:
try:
key_path = os.path.expanduser("~/.anthropic_key")
if os.path.isfile(key_path):
with open(key_path, "r") as _kf:
api_key = _kf.read().strip()
except Exception:
api_key = None
if not api_key:
return _unknown("no_api_key")
# 後方互換: str 1 枚もリストとして扱う
if isinstance(image_paths, (str, bytes, os.PathLike)):
paths = [str(image_paths)]
elif isinstance(image_paths, (list, tuple)) and image_paths:
paths = [str(p) for p in image_paths]
else:
return _unknown("invalid_image_paths")
image_blocks = []
for p in paths:
try:
with open(p, "rb") as f:
image_bytes = f.read()
except Exception as e:
return _unknown("image_read_error:{}".format(type(e).__name__))
image_blocks.append({
"type": "image",
"source": {
"type": "base64",
"media_type": _media_type_for(p),
"data": base64.b64encode(image_bytes).decode("ascii"),
},
})
# pick で 1 画像しか無い場合は「浮き判定不可」なので単画像プロンプトへ
# フォールバック (挟み込み判定だけ)。2 画像が揃う通常時は従来の浮き判定文。
prompt_key = kind
if kind == "pick" and len(paths) == 1:
prompt_key = "pick_single"
payload = {
"model": VLM_MODEL,
"max_tokens": VLM_MAX_TOKENS,
"messages": [{
"role": "user",
"content": image_blocks + [{"type": "text", "text": PROMPTS[prompt_key]}],
}],
}
req = urllib.request.Request(
VLM_API_URL,
data=json.dumps(payload).encode("utf-8"),
headers={
"x-api-key": api_key,
"anthropic-version": VLM_ANTHROPIC_VERSION,
"content-type": "application/json",
},
method="POST",
)
try:
with urllib.request.urlopen(req, timeout=VLM_TIMEOUT_SEC) as resp:
body_bytes = resp.read()
except urllib.error.HTTPError as e:
# 401/429/500 等。エラー本文は raw に詰める（API キー文字列は含まない）
try:
err_body = e.read().decode("utf-8", errors="replace")
except Exception:
err_body = ""
# DAY274 記録細分化: Anthropic API はクレジット残高不足を
# 400 + {"type":"error","error":{"type":"invalid_request_error",
# "message":"Your credit balance is too low..."}}
# で返す (2026-07-06 実測). 台帳の reason 列だけで即判別できるよう
# http_400_no_credit にマップする. 他の 400 (画像・パラメータ等) は
# 従来の "http_400" のまま (raw にサーバ本文が残り原因追跡可能).
# 判定ロジックは無改変 (verdict=unknown で return するのは同じ).
reason_key = "http_{}".format(e.code)
if e.code == 400 and "credit balance" in err_body.lower():
reason_key = "http_400_no_credit"
return _unknown(reason_key, raw=err_body[:500])
except urllib.error.URLError as e:
reason = type(e.reason).__name__ if hasattr(e, "reason") else "URLError"
return _unknown("urlerror:{}".format(reason))
except Exception as e:
return _unknown("request_error:{}".format(type(e).__name__))
try:
body = json.loads(body_bytes.decode("utf-8"))
except (ValueError, TypeError):
snippet = body_bytes[:500].decode("utf-8", errors="replace")
return _unknown("response_parse_error", raw=snippet)
# content[0..N].type=="text" の最初のものを取る
content = body.get("content") or []
text = ""
for blk in content:
if isinstance(blk, dict) and blk.get("type") == "text":
text = blk.get("text", "") or ""
break
if not text:
return _unknown("no_text_in_response",
raw=json.dumps(body)[:500])
parsed = _parse_json_from_text(text)
if parsed is None or not isinstance(parsed, dict):
return _unknown("verdict_parse_error", raw=text[:500])
return {
"verdict": _coerce_verdict(parsed.get("verdict")),
"confidence": _coerce_confidence(parsed.get("confidence")),
"reason": str(parsed.get("reason", ""))[:200],
"model": body.get("model") or VLM_MODEL,
"raw": text[:500],
}
# ---- CLI ----
def main():
ap = argparse.ArgumentParser(
description="Claude（Anthropic API）ビジョンで pick / place 判定")
ap.add_argument("--image", required=True, help="判定画像のパス")
ap.add_argument("--kind", required=True,
choices=["pick", "place", "place_side"])
args = ap.parse_args()
r = judge(args.image, args.kind)
print(json.dumps(r, ensure_ascii=False, indent=2))
if __name__ == "__main__":
main()
