mechArm 270 Pi 動的追従の限界を探る Log#53 | テンプレートマッチングが「白箱の中心」をずっと角で測っていた
PLACE でブロックを箱の中心に置きたい。その置きズレ（place Δdist）がどうしても縮まない。原因を追って、朝から夜まで一日をまるごと使った。結論から言うと、置きズレが縮まなかった一番下の理由には、まだ辿り着いていない。ただしその手前で、もっと大きな地面が崩れていたのが見つかった。ロボットがずっと参照していた「箱の中心」が、実際の中心ではなく箱の角を指していた。最大で 40mm ずれた座標を基準に、補正も高さ制御もサーボも学習も、全部その上に積まれていた。
この記録は、原因の層を一枚ずつ剥がしていった過程そのものだ。Claude（設計・レビュー指示担当）が原因を何度も断定し、そのたびに実機のログか、私の目視かに訂正された。外した診断を消さずに残す。考えが動いた記録として、そこに意味があるから。
止まる、を全部潰す
朝いちばんの症状は「実機が止まる」だった。PICK に入る手前で送信前ガードが走って ABORT する。
[LOOKUP] stage4_pick phase=pick target_xz=(+0.2145,+0.2887) → j0=-36.930° method=out_of_range x_resid=0.02089m z_resid=0.02846m joint_ok=False
[LOOKUP] abort_reason: x-z 最近傍距離 0.0353m > 0.0350m (DIST_MAX 超過)
[ABORT ] stage=4 reason=送信前ガード NG: joint_limit違反: []最初に引っかかったのは joint_limit違反: [] という文言だった。リストは空。関節リミット違反ではない。真因は1行上の method=out_of_range——狙いに対して FK テーブルの最近傍が DIST_MAX（当初 0.030m）を超えていて、それを送信前ガードが巻き添えで弾いていた。見出しに引っ張られると読み違える。
止血として DIST_MAX を上げた。fk_lookup.py の1定数。
DIST_MAX_DEFAULT = 0.030 # m (30mm) 最近傍までを
DIST_MAX_DEFAULT = 0.035に。だがこれは対症療法で、閾値を追いかけているだけだった。実際、この日のうちに 0.030 → 0.035 → 0.045 と三度上げることになる。上げるたびに、少し先の縁でまた止まる。
止まらないことは最優先。だが閾値いじりでは越えられない。台帳を時系列に読むと、PICK の補正記憶が1セルだけ異常に巻き上がっていた。
pick_x180_z260 : dz = -2.9, -3.1, -4.7 （tip残差 -0.5〜0.9）
pick_x200_z240 : dz = -6.3, -8.6 （tip残差 -4.5〜-1.9）
pick_x220_z240 : dz = -5.5 （tip残差 -6.1）
pick_x200_z260 : dz = -23〜-29 ★ （tip残差 -3〜+1.5） ← これだけ異常同じブロック・同じ小さい tip 残差なのに、pick_x200_z260 セルだけ -29mm。隣接セルは全部 -3〜-9mm。真の必要補正は5mm前後で、-29mm は過去の履歴で積分則が巻き上がった腐った値だった。この -29mm が PICK 狙いを z≈289 まで押し、FK テーブルの +z 縁の外に運んでいた。縁の外を狙っても lookup は同じ縁の姿勢を返すので爪はブロックに届く（だから掴めた）。でも狙い自体は帯から離れているので、検出が数mmブレた瞬間、DIST_MAX を超えて out_of_range で止まる。掴めるのに止まる、の正体はこれだった。
対処は腐ったセルのリセットと、1回あたりの更新上限（MAX_CORRECTION_MM）を 40 → 15 に下げること。縁へ巻き上がれないようにした。閾値を追いかけるのをやめて、上流を直す方に戻した。
捻れの犯人
次に、PLACE でグリッパーが j5 を捻り始めた。それまで j5≈-29 でまっすぐ置いていたのが、突然 j5≈0 の捻れ姿勢になる。
台帳を見ると、箱位置は動いていないのに place_j0 で完全に二分されていた。
j0 = 40〜45 → j3≈0, j5≈-30 まっすぐ
j0 = 55〜60 → j3≈20+, j5≈0 捻れ私は最初「PLACE 補正が狙いを −z に押して捻れ解に入った」と考えて、補正を measure モードに戻した。外れだった。measure モードにしても捻れは消えなかった。真因は別の場所にあった。approach_place を fk_lookup 化したときに入れた拘束だ。
APPROACH_PLACE_J4_REF = -76.0
PLACE_J4_TOL_DEG = 1.5j4 帯は [-77.5, -74.5]。FK テーブルでは、まっすぐ解の j4≈-89、捻れ解の j4≈-75.6。この帯はピンポイントで捻れ解だけを許可し、まっすぐ解を範囲外で排除していた。j4 を拘束したつもりが、手首とグリッパーの捻れ姿勢族を強制選択させていた。
修正は、approach_place を円弧の j0_lookup に戻すこと。この日まっすぐ解で何度も完走していた経路への復帰だ。コード内にもこの off スイッチが明記されていた。
FK_LOOKUP_PHASES = ("pick", "place_withblock", "approach_pick",
"approach_place")から "approach_place" を抜いて
FK_LOOKUP_PHASES = ("pick", "place_withblock", "approach_pick")dry-run で狙い通りに戻り、実機でも j5 が -29 付近に戻った。捻れは approach_place の姿勢選択が犯人で、補正でも DIST_MAX でも箱位置でもなかった。
Zサーボを作る
捻れが消えて、本題の place Δz に向き合う。置きズレの内訳を測ると、ほぼ全部が z 方向の系統オフセット（+28mm 前後）だった。place_verdict=yes の Δz がほぼ全てプラスに偏る。ランダムなら±に散らばるはずが、片側に固定して出る。
これを補正記憶（試行またぎ学習）で埋めようとすると発散した。真値28mm が1回あたりのクランプ15mm より大きく、収束できずに限界へ積み上がって、最後は J4_SAFE を割って止まる。
[CORR] place_x-180_z200 dz=+15.00mm target (-187.9,+197.7) → (-202.9,+182.7)
[ABORT] stage=8 j4=-101.00 が J4_SAFE[-90.0,60.0] 外J4_SAFE は物理保護なので緩めない。方針を変えた。Z は試行またぎ学習でなく、置く直前のその場閉ループ（サーボ）で詰める。1試行内で Δz→0 に収束させ、次に持ち越さない。持ち越さなければ巻き上がりが原理的に起きない。
ただし Z の動かし方には過去の地雷がある。関節係数（j1+j2+j4 の連動）で Z を動かすと「関節は動くが爪先が動かない」状態にハマった前例だ。これを避けるため、Z は係数連動でなく FK lookup の狙い z を実測 Δz ぶん動かして再降下する方式にした。動かす姿勢は FK テーブルが返すものだけ。
設計を書き、実装（Claude Code）→ 外部レビュー（codex/gemini）→ 修正 → 実機、の順で進めた。関節が動く姿勢系の変更なので、レビューは飛ばさない。レビューで潰れた主な穴はこれだ。
収束基準の混同：diff_z を「更新される狙い z」から測っていた。基準は不動の箱中心 z でなければならない。
desired_z_mmをループ開始時に1度だけ確定し、狙い z（操作量）と別変数にする。X サーボと Z サーボの j0 取り合い：Z の再 lookup が j0 を stage8 固定値で戻すため、X サーボが締めた j0 が毎反復巻き戻されていた。j0 基準を「その時点の実測 j0」にして協調させる。
同一姿勢の無駄送角：狙いを動かしても FK 最近傍が変わらないとき、同じ姿勢を送って X を壊すだけになる。max|Δj|≈0 なら送らず break。
ガード落ちで run を止めない：J4_SAFE 外・帯外なら送らず
guard_stopで安全姿勢のまま継続。run を ABORT しない。
そして、離した後の系統ズレを狙い側で先回りする学習オフセット（desired_z への release offset・α=0.3・1回5mmクランプ・総量40mm上限）も足した。
ここまでで「止まらないまま Z を詰める」土台はできた。だが、この土台の下がまだ間違っていた。
「箱の中心」が、ずっとずれていた
置き位置を目で確認したくても、位置取得のプレビューが無かった。
位置取得のプレビューは動いてないので分からない。
そこで、上カメラの映像に箱中心・ブロック位置・Δを重畳するプレビュー（preview_place_delta.py）を作った。本線の検出関数をそのまま import して、実機と同じ座標を映す。
映して、すぐ分かった。BOX-T（テンプレマッチが出す箱中心）の十字が、箱の中央ではなく右上の角に乗っていた。 ブロックを箱のほぼ中央に手で置いても、HUD は dz=+56.6 / ddist=60.4 と出る。ブロック検出（緑十字）はブロックの上に正確に乗っているので、ズレているのは箱検出だけだった。
原因はテンプレマッチの中心の出し方にあった。06f_top_white_box_detect_v7.py の該当箇所はこうなっている。
cx = mx + min_x + sw // 2 # マッチ左上x + ROIオフセット + テンプレ幅/2
cy = my + min_y + sh // 2 # マッチ左上y + ROIオフセット + テンプレ高/2matchTemplate が返すのはマッチした矩形の左上。そこにテンプレの幅・高さの半分を足して矩形中心を出している。箱の構造は一切見ていない。テンプレ矩形の真ん中を箱中心とみなしているだけ。しかもテンプレ8枚はサイズがバラバラ（176×178 〜 216×144）で、回転（0〜75°）とスケール（0.85〜1.15）も探索する。どのテンプレがどの角度で当たるかで、矩形中心が箱の実中心から系統的にズレる。
つまり、補正記憶も、高さラチェットも、Zサーボも、release offset 学習も、全部この「角にずれた箱中心」の上に積まれていた。何をどう調整しても箱の真ん中に寄らないのは当然だった。
テンプレの撮り直しや、テンプレ内の箱位置の px 補正も検討したが、実測でどれも筋が悪いと分かった。テンプレ画像内で箱はほぼ中央にあり（実測ズレ +1.2, +2.0px）、影込みでもなかった。問題はテンプレでなく「矩形中心＝箱中心」という方式そのものだった。
輪郭ベースで白箱認識しているなら、その中心を出すの簡単じゃない？
そのとおりだった。箱は白い矩形なのだから、輪郭を取ってその中心を出せば、テンプレのサイズ・回転・スケールに依らず正確な中心が出る。プレビューで検証コードを回すと、白マスク（S<45, V>165）＋正方形（aspect）＋四角（extent>0.75）＋4頂点、で箱だけが1個に絞れた。
bbox=(1276, 663, 162, 176) 頂点数=4 中心=(1357, 751)
→ 白箱が一意に1個だけ残ったさらに、箱が斜めに置かれると外接矩形の aspect が崩れて拾えない問題が出たので、判定を minAreaRect（回転を含む最小矩形）基準にした。aspect = max(w,h)/min(w,h) なら回転しても正方形は 1 前後で安定する。これで平行・斜め・回転の全向きで、黄十字（BOX-C・輪郭）が箱中央に乗った。テンプレ（BOX-T・青十字）との差 d(vs T) は、向きによって +3mm だったり +41mm だったり——テンプレ方式の系統ズレそのものが、青と黄の差として数字で見えた。
アームが箱に被ると輪郭は None を返す。だが箱中心を検出するのは PICK 後の持ち上げ〜A_PLACE の手前、アームがまだ箱に被っていないタイミングで1回だけで、その値を PLACE まで使い回す。被る頃には検出は終わっている。だから被り時の None は実運用では起きない場面で、むしろ「被っても誤った中心をでっち上げない」証明になった。
本線 06f を、輪郭主・テンプレ fallback（案B）に置き換えた。テンプレマッチは残して、箱の粗位置検出・胴体除外・そして輪郭が取れないときの保険にする。中心の出所だけを輪郭に差し替えた。戻り値 center の意味は同じなので、homography も呼び出し側も無改修で動く。レビューを通し、preview で全向き中央を最終確認した。
箱中心の定義が変わったので、旧・角基準で積んだ学習値はリセットした。
place_release_offset.json
- place_x-180_z200: {offset_z_mm: -34.62, n: 8} → {offset_z_mm: 0.0, n: 0}
correction_memory.json
- place_* 3件リセット / pick_* 22件は無変更それでも Δz は縮まなかった
正しい箱中心・汚染ゼロの学習値・レビュー済みの Z サーボ。土台は整った。observe を回した。
Δz は +40〜52mm のまま、縮まなかった。
箱中心を輪郭に直しても、リセットしても、Δz は残る。ここで、この日ずっと積み上げてきた「箱中心が原因」という話が、Δz の主因ではなかったと分かる。箱中心は実際にずれていた（それは直す価値があった）。だが place Δz が縮まない主因は、別の層にあった。
サーボの反復を見る。
z_iter=1: block_z=224.7 diff=-30.7 step=-15 → moved
z_iter=2: block_z=222.7 diff=-28.7 step=-15 → moved
z_iter=3: block_z=220.7 diff=-26.7 step=-15 → moved
z_iter=4: block_z=220.2 diff=-26.2 step=-15 → guard_stop
z_exit_reason = guard_stop狙い（step）を毎回 -15mm 動かしているのに、block_z が 2mm/反復しか動いていない。そして4反復目で guard_stop。ここで私は「サーボでは越えられない J4_SAFE の壁だ」と考えた。これも早すぎた。
PLACE サーボでZ軸が動かないのは何でですか？
本線から切り離した単発診断スクリプト（diag_place_z_move.py）を作った。PLACE 姿勢に置いて、狙い z を一発で箱中心まで動かし、ブロックが実際に動くかだけを裸で見る。最初、狙いを動かしても method=out_of_range / z_residual=inf が返った。だが FK テーブルには、この PLACE 帯（x -0.20〜-0.14, z 0.16〜0.24, ee_y 0.085〜0.115）に SAFE 内の点が 4453 個ある。点があるのに out_of_range——矛盾する。
矛盾の正体は、診断スクリプトが fk_lookup に渡していた ref_joints が現在姿勢（ほぼ HOME）だったことだった。j0 拘束が的外れで候補ゼロ。start-pose を正しい PLACE 姿勢に渡し直すと、out_of_range は消えた。
[STEP 5] fk_lookup(place_withblock, ...) 呼び出し
ref_joints_src = --start-pose
ref_joints: [ +36.215, +57.000, +20.000, -12.000, -86.000, -30.000]
method = fk_j4_priority_j4safe_pref
x_residual_m = 0.007196999999999981
z_residual_m = 0.015472707153320303
lookup target_angles: [ +36.215, +57.000, +20.000, -7.000, -86.000, -30.000]
[STEP 6] _check_guard (本線同一)
[GUARD-PASS]method は fk_nearest 系に戻り、J4_SAFE も通る。壁でもなかった。だがここに、Δz が縮まない本当の手がかりがある。狙い z を 10.9mm 手前へ動かしたのに、返った姿勢は j3 が -12 → -7（5度）変わるだけで、z_residual = 0.0155m（15.5mm）。狙った z の近くの点を返せていない。狙いから15mm離れた点しか無く、それを返している。狙いを動かしてもブロックが2mmしか動かないのは、これが物理に出たものだ。
では z 方向に点が薄いのか。z のヒストグラムを取ると、z=0.16〜0.24 は 0.1mm 刻みでびっしり埋まっていた。z 単独では超密。密度不足でもなかった。
ということは、fk_lookup が z 以外の条件（j0拘束・ee_y帯・j4safe優先・ref_joints の姿勢族）で候補を絞った後、残った中の最近傍が z方向に15mm離れている。多次元で薄い。ここまでは実測で確定した。だが、その15mmが j0拘束由来か、ee_y帯由来か、本当に点が無いのか——それはまだ切り分けていない。
確定したこと、していないこと
この日で確定したのは次のとおり。
「箱の中心」がテンプレ矩形中心方式で最大41mmずれて検出されていた。輪郭ベース（案B・レビュー済み）に置き換え、preview で全向き中央を確認した。実在のズレを直した。
「止まる」系の原因は全て系統だてて潰した。PICK の out_of_range は補正記憶の巻き上がり、PLACE の out_of_range と捻れは approach_place の j4_ref 拘束、PLACE の J4_SAFE 違反は補正・高さラチェットの巻き上がり。共通の根は「補正記憶が収束せず限界へ巻き上がる」ことだった。
Z サーボは、その場閉ループとして正しく動く。到達可能な範囲では離す直前に箱中心5mmまで詰められることも実証した（別 run）。
確定していないのは1点。place Δz が収束しない最終原因。「狙いの姿勢族での FK 最近傍が z方向に15mm離れている」ところまでは掴んだ。その15mmが j0ロック・ee_y帯・点の不在のどれ由来かは、diag で絞り込み条件を1つずつ緩めれば実測で確定する。そこまでは、この日には届かなかった。
この一日、私は原因を「穴だ」「密度不足だ」「J4_SAFEの壁だ」と何度も断定して、そのたびにデータに外された。効いたのは断定ではなく、JSON を突き合わせ、台帳を並べ、ヒストグラムを取り、プレビューで十字を見た、計測の一手一手だけだった。計測手段が先。次に迷うことはない——原因を1つに絞る方法は、もう決まっている。
この日の全コード
記録としての完全性のため、この日に触れたコードを全量で残す。読み物としての体裁より、後から本人や AI が見返して使える再現性を優先する。
run_pick_place.py は 4618 行のため、この日に実装・変更した関数群（補正記憶・箱衝突高さ・release offset・PLACE 視覚サーボ／Z サーボ）を関数単位で全量掲載する。掲載していない箇所はこの日の変更対象外。fk_lookup.py はこの日の変更が DIST_MAX_DEFAULT の 1 行のみのため差分で示す。
fk_lookup.py（差分・DIST_MAX）
変更前 DIST_MAX_DEFAULT = 0.030 → 変更後 DIST_MAX_DEFAULT = 0.045（この日 0.030 → 0.035 → 0.045 と三度上げた最終値）。
変更した定数（run_pick_place.py・最終状態）
# L83
FK_LOOKUP_PHASES = ("pick", "place_withblock", "approach_pick")
# L213
MAX_CORRECTION_MM = 15.0 # 補正量の絶対値クランプ (mm).
# L376-434 (Zサーボ / release offset / PLACE補正モード)
PLACE_Z_ENABLE = True # False で Z サーボ無効 → 従来 X のみ挙動へ即ロールバック
PLACE_Z_OBSERVE_ONCE = False # フルループ解禁。diff_z 収束まで反復
PLACE_Z_CONVERGE_THRESHOLD_MM = 12.0 # |diff_z| これ以下で収束 (箱内 80mm・暫定)
PLACE_Z_MAX_STEP_MM = 15.0 # 1 反復での狙い z 移動量クランプ (暴走保護)
PLACE_Z_MAX_ITER = 5 # step15mm×最大4反復=60mmまで詰め可。38mmのズレを収束閾値以下へ
# ---- 2026-07-12: PLACE Z サーボ修正 (j0 取り合い解消 + same-pose skip + 学習オフセット) ----
# 修正1: Z 再 lookup の j0 基準を「X サーボ完了後の実測 j0」に切替（run 073606 実測で
# stage8 固定 j0 が X 修正 (34.89/34.27) を 36.22 に巻き戻していた欠陥を解消）.
# 修正2: 同一姿勢での無駄送角を回避. 全関節の |Δ| が SAME_POSE_SKIP_DEG 以下なら送らず break.
SAME_POSE_SKIP_DEG = 1.0 # 到達 tol (2.5-3°) より小さい移動は送らない
# 修正3: desired_z への学習オフセット. 離した後のズレ (+z 系統) を毎 run 少しずつ食う.
# correction_memory (XZ) や place_height_memory (dee_y) とは独立ファイル.
# stage8 の初回 lookup 狙い・X サーボ target には触らず、Z サーボの desired_z_mm
# のみに 1 度だけ適用する (二重反映しない).
PLACE_RELEASE_OFFSET_ENABLE = True
PLACE_RELEASE_OFFSET_ALPHA = 0.3 # 最終 Δz の 30% ずつ反映
PLACE_RELEASE_OFFSET_STEP_MAX = 5.0 # 1 run の更新量クランプ [mm]
PLACE_RELEASE_OFFSET_TOTAL_MAX = 40.0 # |offset_z_mm| の総量上限 [mm] (実測 30-46mm を覆う最小)
# DAY272 案T (拡張): 観測モード. True で 1 ループのみ実行し即座に抜ける
# (1 回の j0 送角 → 実測 → 送角後 block_x 追加測定 → verdict 出力で停止, 収束判定を待たない).
# 初回実機で「j0 符号が正しい向きに締まるか」「保持ブロックが箱縁に接触しないか」を
# 目視+ログで確認する用. exit_reason="observe_once" で抜ける.
# フラグ分岐追加のみで既存収束ループ本体は無変更.
PLACE_SERVO_OBSERVE_ONCE = False # X も本稼働（Zへ進む前提を成立させる）
# 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")
# 修正3 (2026-07-12): 学習オフセット (Z サーボ desired_z への 1 箇所適用).
# correction_memory (XZ) / place_height_memory (dee_y) とは独立ファイル.
PLACE_RELEASE_OFFSET_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/place_release_offset.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 = True # PLACE補正が収束せず15mmクランプ張り付き→j4がJ4_SAFE外へ。箱検出/セル分散の未解決を残したまま学習を回すのは早すぎたので停止。狙いを生の箱位置に固定
補正記憶・箱衝突高さ・release offset・_apply_correction（run_pick_place.py L1012-1375・全量）
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]
# ---- 修正3 (2026-07-12): PLACE 学習オフセット (Z サーボ desired_z 適用専用) ----
# correction_memory (XZ 補正・stage8 狙い操作) や place_height_memory (dee_y ラチェット)
# と独立. 「離した後の +z 系統ズレ」を先回りして食うために desired_z_mm へ 1 度だけ加算する.
# ゲイン α=0.3, 1 run クランプ ±5mm, 総量上限 ±40mm. 到達時は WARN で停止.
def _release_offset_load():
"""place_release_offset.json をロード. 失敗時は {} (制御を絶対止めない)."""
if not PLACE_RELEASE_OFFSET_PATH.exists():
return {}
try:
with open(PLACE_RELEASE_OFFSET_PATH, "r", encoding="utf-8") as f:
return json.load(f)
except Exception as e:
print("[OFFSET] [WARN] 読み込み失敗 ({}). 空辞書で続行".format(e))
return {}
def _release_offset_save(mem):
"""学習オフセットを atomic に保存. KIOXIA 未マウント時は False でスキップ."""
if not _ensure_ssd_mounted():
print("[OFFSET] KIOXIA 未マウント → 学習オフセットの保存をスキップ")
return False
try:
PLACE_RELEASE_OFFSET_PATH.parent.mkdir(parents=True, exist_ok=True)
tmp = PLACE_RELEASE_OFFSET_PATH.with_suffix(
PLACE_RELEASE_OFFSET_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_RELEASE_OFFSET_PATH))
return True
except Exception as e:
print("[OFFSET] [WARN] 保存失敗: {}".format(e))
return False
def _release_offset_key(x_m, z_m):
"""(x, z)[m] → セルキー. PLACE の XZ 補正と同一格子 (20mm)."""
return _corr_key("place", x_m, z_m)
def _release_offset_get_mm(mem, key):
"""セルキー → offset_z_mm (未登録なら 0.0). 破損時は 0.0.
修正D (2026-07-12・R4): ロード時にも ±TOTAL_MAX クランプを効かせる.
保存側は 40mm 超を書かないが, 既存 JSON が壊れていた場合の防御.
超えていたら 0 に落として WARN (適用しない・run は継続)."""
e = mem.get(key)
if e is None or not isinstance(e, dict):
return 0.0
try:
v = float(e.get("offset_z_mm", 0.0))
except (TypeError, ValueError):
return 0.0
if abs(v) > PLACE_RELEASE_OFFSET_TOTAL_MAX:
print("[OFFSET] [WARN] loaded offset {:+.2f}mm exceeds TOTAL_MAX "
"(±{:.1f}mm). 破損値と判定して 0 に落とす.".format(
v, PLACE_RELEASE_OFFSET_TOTAL_MAX))
return 0.0
return v
def _release_offset_update(mem, key, final_dz_mm):
"""1 run 分の更新. 戻り値: (new_offset_mm, reason).
reason ∈ {"updated","total_max_reached","save_failed"}.
- step = clamp(-α * final_dz, ±STEP_MAX)
- would_be = cur + step
- abs(would_be) > TOTAL_MAX なら更新停止 (cur を保持), reason="total_max_reached"
→ 上限突破は許さない. 減方向は許可 (would_be が TOTAL_MAX 未満なら普通に更新).
"""
cur = _release_offset_get_mm(mem, key)
step = max(-PLACE_RELEASE_OFFSET_STEP_MAX,
min(PLACE_RELEASE_OFFSET_STEP_MAX,
-PLACE_RELEASE_OFFSET_ALPHA * float(final_dz_mm)))
would_be = cur + step
if abs(would_be) > PLACE_RELEASE_OFFSET_TOTAL_MAX:
print("[OFFSET] [WARN] TOTAL_MAX 到達 (would be {:+.2f}mm, |value|>{:.1f}mm). "
"更新停止 (cur={:+.2f} を保持).".format(
would_be, PLACE_RELEASE_OFFSET_TOTAL_MAX, cur))
# 保存はせず現状維持. cur / step / would_be は履歴として mem に残さない
# (現状維持なので更新不要).
return cur, "total_max_reached"
# 既存 n を保持. 無ければ 0 から.
e = mem.get(key)
try:
n = int(e.get("n", 0)) if isinstance(e, dict) else 0
except (TypeError, ValueError):
n = 0
mem[key] = {"offset_z_mm": float(would_be), "n": n + 1,
"last_final_dz_mm": float(final_dz_mm)}
return float(would_be), "updated"
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), metaPLACE 視覚サーボ / Z サーボ（run_pick_place.py L2229-2946・全量）
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 _build_place_lookup_kwargs(ref_joints, target_ee_y_m, place_j0_ref,
joint_tol_deg=None,
j3_ref=None, j5_ref=None):
"""place_withblock fk_lookup の引数を組む単一の正 (stage8 と Z 再 lookup で共有).
stage_lookup_and_move の place_withblock 分岐と _place_z_servo_relookup の
両方でこれを使う. 将来 stage8 に別の引数 (j3_ref/j5_ref 等) を足すときは
ここへ追加すれば Z 側にも自動反映される.
引数の実値 (ee_y 帯・tol 等) は現行 stage8 呼び出しの現物と一致.
「共通化するのは lookup 引数だけ」— 送角/abort 処理は共通化しない
(Z 側の「ガード落ちで abort しない」要件を壊さないため).
"""
if joint_tol_deg is None:
joint_tol_deg = PLACE_JOINT_TOL_DEG
kwargs = dict(
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,
)
if place_j0_ref is not None:
kwargs["j0_ref"] = float(place_j0_ref)
kwargs["j0_tol_deg"] = PLACE_J0_LOCK_TOL_DEG
return kwargs
def _place_z_servo_relookup(current_target_xz, log_record, dry_run,
ref_joints, target_ee_y_m, place_j0_ref, z_iter,
measured_j0=None,
measured_angles=None):
"""PLACE Z サーボ 1 反復の再 lookup + 送信前ガード + _do_move.
stage8 と同一の place_withblock lookup 引数構成 (ee_y 帯・ref_joints・
place_ee_y_floor・place_j1_min_deg・target_ee_y_m・j4_safe_min/max・
place_j0_ref) をそのまま継承し, target_xz の z だけ差し替えて呼ぶ.
新規の lookup 呼び出しを書かない (stage8 の経路をそのまま再利用).
修正1 (2026-07-12): measured_j0 が来ていれば place_j0_ref の代わりに使う.
X サーボが j0 を修正した直後の実測値を渡し, Z 再 lookup で「stage8 固定 j0」に
巻き戻すのを防ぐ. None ならフォールバックで従来の place_j0_ref を使う.
修正2 (2026-07-12): measured_angles が来ていれば, ガード通過後に同一姿勢判定を行い,
全関節の |Δ| が SAME_POSE_SKIP_DEG 以下なら "same_pose_skip" を返して送角しない.
送信前ガード (_check_guard) で NG または method が out_of_range のときは
**run を ABORT せず**「Z 詰めをやめて last_safe_pose のまま置く」に変える.
ここが本仕様の安全の要 (§C1).
戻り値: (r or None, action_str).
action ∈ {"moved","dry_run","guard_stop:<reason>","lookup_error:<Exc>",
"move_failed","same_pose_skip"}.
"""
label = "stage8_place_zservo_i{}".format(z_iter + 1)
# 修正1: 実測 j0 優先. None なら従来の place_j0_ref にフォールバック.
effective_j0_ref = (measured_j0
if measured_j0 is not None else place_j0_ref)
lookup_kwargs = _build_place_lookup_kwargs(
ref_joints=ref_joints,
target_ee_y_m=target_ee_y_m,
place_j0_ref=effective_j0_ref)
try:
r = fk_lookup(
"place_withblock",
(float(current_target_xz[0]), float(current_target_xz[1])),
**lookup_kwargs,
)
except (ValueError, FileNotFoundError, RuntimeError) as e:
return None, "lookup_error:{}".format(type(e).__name__)
ok_guard, reason = _check_guard(r, label)
method_str = str(r.get("method", ""))
if (not ok_guard) or method_str.endswith("out_of_range"):
# run を止めない. last_safe_pose のまま Z 詰め終了.
return r, "guard_stop:{}".format(reason)
if dry_run:
return r, "dry_run"
# 修正2: 同一姿勢なら送角しない (ガード通過後・_do_move 前).
# 到達 tol (2.5-3°) より小さい SAME_POSE_SKIP_DEG=1.0° 以内なら「動かない送角」なので
# 送らずに break. X サーボ修正の巻き戻し (run 073606) を防ぐ.
if measured_angles is not None:
try:
_tas = [float(v) for v in (r.get("target_angles") or [])]
_cur = [float(v) for v in measured_angles]
except (TypeError, ValueError):
_tas, _cur = [], []
if len(_tas) == 6 and len(_cur) == 6:
_max_dj = max(abs(_tas[i] - _cur[i]) for i in range(6))
if _max_dj <= SAME_POSE_SKIP_DEG:
print("[SERVO] same_pose_skip: max|Δj|={:.3f}° <= {:.1f}° "
"(狙い z を動かしても FK 最近傍が変わらない・送角不要)".format(
_max_dj, SAME_POSE_SKIP_DEG))
return r, "same_pose_skip"
ok_move, _post = _do_move(
r["target_angles"], label, TOL_PICK_PLACE, log_record, dry_run=dry_run)
if not ok_move:
return r, "move_failed"
return r, "moved"
def _run_place_visual_servo(box_xz, log_record, dry_run,
current_target_xz=None,
ref_joints=None,
target_ee_y_m=None,
place_j0_ref=None):
"""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 でも動く実時間制御)
2026-07-11 Z サーボ追加:
既存 X サーボ (_visual_servo_loop_place・1 バイト不変) を外側 Z ループで包む.
各反復:
(A) X サーボ (既存) → (B) 保持ブロック z 実測 →
(C) 収束 (|diff_z|≤PLACE_Z_CONVERGE_THRESHOLD_MM) →
(D) new_target_z (±PLACE_Z_MAX_STEP_MM クランプ) →
(E) stage8 と同一の place_withblock lookup を狙い z だけ差し替えて呼ぶ →
(F) 送信前ガード. J4_SAFE 外 / out_of_range は run を止めず z_guard_stop で
抜ける (last_safe_pose のまま) →
(G) 送角. 送角後のポーズを次 X サーボの j0/base に反映.
PLACE_Z_ENABLE=False で従来 X のみ挙動に即ロールバック.
PLACE_Z_OBSERVE_ONCE=True で 2 反復目以降は lookup せず即 break (送角ゼロ).
"""
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": "",
# Z サーボ観測フィールド (2026-07-11 追加).
"z_enabled": bool(PLACE_Z_ENABLE),
"z_observe_once": bool(PLACE_Z_OBSERVE_ONCE),
"z_iter_count": 0,
"z_history": [],
"z_exit_reason": "",
"z_target_z_m_start": None,
"z_target_z_m_final": None,
# B2: X サーボ wrapper 集計フィールド. dry_run 時も空配列で残す.
"x_runs": [],
# 修正3 (2026-07-12): 学習オフセット観測フィールド.
"release_offset_key": None,
"release_offset_applied_mm": 0.0,
# release_offset_after_mm / release_offset_update_reason は run 完了時の
# update-decision で埋める (この関数の外).
}
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])))
# Z サーボ用: 初期の狙い xz. 呼び出し側から current_target_xz を渡してもらう.
# 未指定なら box_xz を初期狙いとする (measure モード時に stage8 が使った狙いと同じ).
if not (isinstance(current_target_xz, (list, tuple))
and len(current_target_xz) == 2
and current_target_xz[0] is not None
and current_target_xz[1] is not None):
_cur_txz = (float(box_xz[0]), float(box_xz[1]))
else:
_cur_txz = (float(current_target_xz[0]), float(current_target_xz[1]))
servo_rec["z_target_z_m_start"] = round(_cur_txz[1], 5)
servo_rec["z_target_z_m_final"] = round(_cur_txz[1], 5)
# 上カメラ 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
# B2 レビュー対応: X サーボ (中核) を Z ループで複数回呼ぶため, wrapper 側で
# 集計する. x_runs (上の servo_rec 初期化で空配列で用意済み) に各 X 呼び出しの
# 結果を保存し, iter_count は累積, elapsed_sec は stage8.5 全体の経過時間で
# 上書きする. 中核 (_visual_servo_loop_place) は 1 バイトも触らず, 中核が
# servo_rec に書いた値を wrapper が読んで aggregate する.
_wrapper_t_start = time.time()
_total_x_iter_count = 0
# A1 レビュー対応: Z 収束の基準を「箱中心 z」に固定. ループ内で二度と代入しない.
# 狙い z (_cur_txz[1]) は反復で動かすが (=操作量), 残差の測定基準はここで
# 確定した desired_z_mm (=箱中心・不動) を常に使う.
# 補正記憶で stage8 の狙いを z 方向へずらしても, 最終目的は「箱中心へ置く」
# なので基準は raw box_xz[1] で正しい.
#
# 修正3 (2026-07-12): 学習オフセットは desired_z_mm への 1 箇所適用のみ.
# stage8 初回 lookup 狙い・X サーボ target には触らない (二重反映しない).
# ファイルは KIOXIA. 未マウント時は load が {} を返し offset=0.0 で run 継続.
_release_offset_key_str = _release_offset_key(
float(box_xz[0]), float(box_xz[1]))
_release_offset_applied_mm = 0.0
if PLACE_RELEASE_OFFSET_ENABLE:
_rel_mem_read = _release_offset_load()
_release_offset_applied_mm = _release_offset_get_mm(
_rel_mem_read, _release_offset_key_str)
if _release_offset_applied_mm != 0.0:
print("[OFFSET] apply cell={} offset={:+.2f}mm "
"(desired_z = raw + offset)".format(
_release_offset_key_str, _release_offset_applied_mm))
servo_rec["release_offset_key"] = _release_offset_key_str
servo_rec["release_offset_applied_mm"] = float(_release_offset_applied_mm)
desired_z_mm = float(box_xz[1]) * 1000.0 + _release_offset_applied_mm
try:
# warm-up.
for _ in range(VLM_CAPTURE_WARMUP_FRAMES):
cap.read()
# ---- 外側 Z ループ (最大 PLACE_Z_MAX_ITER 回・OFF 時は 1 回) ----
# 各反復: (A) X サーボ (既存・無改変) → (B) block z 実測 →
# (C) 収束判定 (基準は固定 desired_z_mm) → (D) new_target_z →
# (E,F) 再 lookup + ガード → (G) 送角 → 次 X サーボ用に j0/base 更新.
final_j0 = j0_current
converged = False
loop_count = 0
exit_reason = ""
z_iter_count = 0
z_iter_max = max(1, PLACE_Z_MAX_ITER) if PLACE_Z_ENABLE else 1
for z_iter in range(z_iter_max):
# (A) X サーボ (既存・無改変. 内部で j0 だけ更新)
_x_j0_before = float(j0_current)
(final_j0_i, conv_i, loop_i, exit_i) = _visual_servo_loop_place(
cap, j0_current, target_x_mm, servo_base_j1_j5, servo_rec)
final_j0 = float(final_j0_i)
converged = bool(conv_i)
loop_count = int(loop_i)
exit_reason = exit_i
j0_current = final_j0
# B2: この X 呼び出しの結果を x_runs に残す (中核が上書きする elapsed_sec は
# 各 X の値としてここで捕捉する).
servo_rec["x_runs"].append({
"z_iter": z_iter + 1,
"exit_reason": exit_i,
"converged": bool(conv_i),
"iter_count": int(loop_i),
"elapsed_sec": float(servo_rec.get("elapsed_sec", 0.0) or 0.0),
"j0_start": round(_x_j0_before, 3),
"j0_final": round(final_j0, 3),
})
_total_x_iter_count += int(loop_i)
if not PLACE_Z_ENABLE:
servo_rec["z_exit_reason"] = "disabled"
break
# 修正A (2026-07-12・R1): X が converge していないと Z へ進まない.
# detection_failed / timeout / send_angle_error 等の全経路で Z 送角に
# 進めないゲート.
# 修正F (2026-07-12・R1 ⚠️): X observe の例外許可は Z observe と両方 True
# のときだけ. フルループ時 (Z observe=False) は conv_i=True のみが通行証.
# X が observe のまま Z をフル解禁する誤設定を構造的に弾く.
x_ok_for_z = bool(conv_i)
if (PLACE_Z_OBSERVE_ONCE
and PLACE_SERVO_OBSERVE_ONCE
and exit_i == "observe_once"):
x_ok_for_z = True # 両方 observe のときだけ明示許可
if not x_ok_for_z:
servo_rec["z_exit_reason"] = "x_not_converged:{}".format(exit_i)
print("[SERVO] Z-SKIP (X 非収束) iter={} x_exit={} "
"(Z 送角に進まない・run 継続)".format(z_iter + 1, exit_i))
break
# (B) 保持ブロック z を実測.
ret_b, frame_b = cap.read()
block_xz_meas = (_detect_block_from_frame(frame_b)
if ret_b else None)
if (block_xz_meas is None
or not isinstance(block_xz_meas, (list, tuple))
or len(block_xz_meas) < 2
or block_xz_meas[1] is None):
servo_rec["z_history"].append({
"z_iter": z_iter + 1,
"note": "block_detect_fail",
"desired_z_mm": round(desired_z_mm, 2),
"current_target_z_m": round(_cur_txz[1], 5),
})
servo_rec["z_exit_reason"] = "z_detect_fail"
break
block_z_mm = float(block_xz_meas[1]) * 1000.0
# A1: 残差は「固定基準 (箱中心) − 実測」で常に測る.
# current_target_z_mm は狙い (操作量) として観測用に別途残す.
current_target_z_mm = _cur_txz[1] * 1000.0
diff_z_mm = desired_z_mm - block_z_mm
z_hist = {
"z_iter": z_iter + 1,
"block_z_mm": round(block_z_mm, 2),
"desired_z_mm": round(desired_z_mm, 2),
"current_target_z_mm": round(current_target_z_mm, 2),
"diff_z_mm": round(diff_z_mm, 2),
}
# (C) 収束判定 (基準は固定 desired_z_mm).
if abs(diff_z_mm) <= PLACE_Z_CONVERGE_THRESHOLD_MM:
z_hist["converged"] = True
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = "z_converged"
z_iter_count = z_iter + 1
print("[SERVO] Z-CONVERGED iter={} block_z={:+.1f} "
"desired={:+.1f} diff={:+.1f}mm <= {}mm".format(
z_iter + 1, block_z_mm, desired_z_mm, diff_z_mm,
PLACE_Z_CONVERGE_THRESHOLD_MM))
break
# (D) new_target_z (±PLACE_Z_MAX_STEP_MM クランプ).
# 操作量 (_cur_txz[1]) を diff_z ぶん寄せる. 基準 desired_z_mm は動かさない.
step_mm = max(-PLACE_Z_MAX_STEP_MM,
min(PLACE_Z_MAX_STEP_MM, diff_z_mm))
new_target_z_m = _cur_txz[1] + step_mm / 1000.0
new_target_xz = (_cur_txz[0], new_target_z_m)
z_hist["step_mm"] = round(step_mm, 2)
z_hist["new_target_z_mm"] = round(new_target_z_m * 1000.0, 2)
# 修正E (2026-07-12・❌ 対応): Z observe 判定は re-lookup + 送角の前に置く.
# 旧実装は _place_z_servo_relookup 呼び出しの後に判定していたため, 2 反復目
# もフル姿勢を送角してから「送らずログのみ」と表示していた (run 073606 で
# stage8_place_zservo_i2 の SEND が実測された). observe の安全条件を守る.
# 1 反復目 (z_iter==0) までは通過, 2 反復目以降は lookup せず break.
if PLACE_Z_OBSERVE_ONCE and z_iter >= 1:
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = "z_observe_once"
z_iter_count = z_iter
print("[SERVO] Z-OBSERVE-ONCE iter={} 2 反復目以降は "
"re-lookup せず break (送角ゼロ)".format(z_iter + 1))
break
# 修正1 (2026-07-12): Z 再 lookup の j0 基準を「X サーボ完了後の実測 j0」に.
# daemon_get_angles 例外 or 6軸未満のときは j0_current (直近 X の j0_final) を使う.
# stage8 の固定 place_j0_ref に戻すことはしない (巻き戻し欠陥回避).
try:
_cur_angs = daemon_get_angles()
except Exception:
_cur_angs = None
if _cur_angs and len(_cur_angs) == 6:
_j0_now = float(_cur_angs[0])
_measured_angles = [float(v) for v in _cur_angs]
else:
_j0_now = float(j0_current)
_measured_angles = None
z_hist["measured_j0"] = round(_j0_now, 3)
# (E, F) 再 lookup + 送信前ガード. run を絶対に ABORT しない.
r_new, action = _place_z_servo_relookup(
new_target_xz, log_record, dry_run,
ref_joints, target_ee_y_m, place_j0_ref, z_iter,
measured_j0=_j0_now,
measured_angles=_measured_angles)
z_hist["relookup_action"] = action
# D4: guard 結果と姿勢を明示的に残す (guard_stop 経路も含む).
# 3 分岐:
# guard_stop → guard_pass=False (ガードで送信を止めた)
# lookup_error → guard_pass=None (lookup 前に例外・ガード未実行)
# それ以外 → guard_pass=True (ガード通過)
# guard_reason は同じ分岐内で 1 度だけ代入する (二重代入しない).
if action.startswith("guard_stop"):
z_hist["guard_pass"] = False
z_hist["guard_reason"] = action.split(":", 1)[1]
elif action.startswith("lookup_error"):
z_hist["guard_pass"] = None # ガード未実行 (lookup 前に例外)
z_hist["guard_reason"] = action
else:
z_hist["guard_pass"] = True
z_hist["guard_reason"] = None
if r_new is not None:
z_hist["method"] = r_new.get("method")
z_hist["j4_selected_deg"] = r_new.get("j4_selected_deg")
z_hist["pose_id"] = r_new.get("pose_id")
_tangs_h = r_new.get("target_angles")
if isinstance(_tangs_h, (list, tuple)):
try:
z_hist["target_angles"] = [
round(float(v), 3) for v in _tangs_h]
except (TypeError, ValueError):
z_hist["target_angles"] = None
z_hist["x_residual_m"] = r_new.get("x_residual_m")
z_hist["z_residual_m"] = r_new.get("z_residual_m")
z_hist["joint_limit_ok"] = r_new.get("joint_limit_ok")
if action.startswith("guard_stop") or action.startswith("lookup_error"):
# ガード落ち / lookup 例外 → run 継続. Z 詰め終了.
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = action
z_iter_count = z_iter + 1
print("[SERVO] Z-GUARD-STOP iter={} action={} "
"(last_safe_pose のまま Z 詰め終了)".format(
z_iter + 1, action))
break
# 修正2 (2026-07-12): 同一姿勢なら送角ゼロで break.
# ガード通過 (guard_pass=True) だが FK 最近傍が動かず「動かない送角」になる
# ケース. これ以上狙いを動かしても同じ行が返るので Z 詰めを終了する.
if action == "same_pose_skip":
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = "z_same_pose_skip"
z_iter_count = z_iter + 1
print("[SERVO] Z-SAME-POSE-SKIP iter={} 送角姿勢が現在と同一 "
"(max|Δj|<={:.1f}°). 送角スキップして Z 詰め終了".format(
z_iter + 1, SAME_POSE_SKIP_DEG))
break
# (旧 post-send observe 判定は 修正E で送角前へ移動. ここでは削除)
# move_failed / dry_run はガード後の段なので run は継続, ログ残して抜ける.
if action == "move_failed":
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = "z_move_failed"
z_iter_count = z_iter + 1
break
if action == "dry_run":
# dry_run はサーボ全体が上で return してるはず. 保険.
servo_rec["z_history"].append(z_hist)
servo_rec["z_exit_reason"] = "z_dry_run"
z_iter_count = z_iter + 1
break
# (G) 送角完了. 送角した姿勢を次 X サーボの j0 / base に反映.
try:
tangs = [float(v) for v in (r_new.get("target_angles") or [])]
except (TypeError, ValueError):
tangs = []
if len(tangs) == 6:
j0_current = tangs[0]
servo_base_j1_j5 = list(tangs[1:])
_cur_txz = new_target_xz
servo_rec["z_target_z_m_final"] = round(_cur_txz[1], 5)
servo_rec["z_history"].append(z_hist)
z_iter_count = z_iter + 1
servo_rec["z_iter_count"] = z_iter_count
# B2: トップレベルフィールドの意味を wrapper 集計へ更新.
# exit_reason / converged / j0_final は最終 (最後の) X 呼び出しの値.
# iter_count は全 X 呼び出しの累積, elapsed_sec は stage8.5 全体の経過時間.
servo_rec["exit_reason"] = exit_reason
servo_rec["converged"] = bool(converged)
servo_rec["iter_count"] = int(_total_x_iter_count)
servo_rec["elapsed_sec"] = time.time() - _wrapper_t_start
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={} "
"z_iter={} z_exit={} j0 {}→{} elapsed={:.2f}s".format(
servo_rec.get("converged"),
servo_rec.get("iter_count"),
servo_rec.get("exit_reason"),
servo_rec.get("z_iter_count"),
servo_rec.get("z_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 と同じ姿勢非依存判定).06f_top_white_box_detect_v7.py（全ファイル・箱中心の輪郭主/テンプレfallback）
"""
06f_top_white_box_detect_v7.py
06e との差分:
evaluate_frame を 06f 内で自前実装し、テンプレートマッチを
BGR (3 チャネル) → グレースケール (1 チャネル) で実行する。
cv2.matchTemplate の計算量がチャネル数に比例するため、約 3 倍速を期待。
test_white_box_score.py 本体は触らない (PICK 側に影響しない)。
自前 evaluate_frame_gray は元実装と同じインターフェイス。
ROI = マット全体 (変更なし)
"""
import sys
import time
from pathlib import Path
import cv2
import numpy as np
SCRIPTS_ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(SCRIPTS_ROOT))
import visual_servo_pick as vsp
import test_white_box_score as twbs
# テンプレ参照先と角度・スケールを上書き
NEW_TEMPLATES_DIR = Path(__file__).resolve().parents[1] / "templates"
twbs.TEMPLATES_DIR = str(NEW_TEMPLATES_DIR)
twbs.ROTATION_ANGLES_DEG = [0, 15, 30, 45, 60, 75]
twbs.SCALES = [0.85, 1.0, 1.15]
print(f"[06f] TEMPLATES_DIR overridden to: {twbs.TEMPLATES_DIR}")
print(f"[06f] ROTATION_ANGLES_DEG overridden to: {twbs.ROTATION_ANGLES_DEG}")
print(f"[06f] SCALES overridden to: {twbs.SCALES}")
from test_white_box_score import load_templates as _load_white_box_templates
SIDE_CAMERA_INDEX = 1
TOP_CAMERA_INDEX = 2
WARMUP_FRAMES = 30
TEMPLATE_SCORE_THRESHOLD = 0.5
DETECT_AFTER_SEC = 0.5
# 胴体除外ROI（pixel・top カメラ画像座標）。
# HOME系姿勢でアーム胴体が画角中央上に写り込み、白箱と誤検出されるのを防ぐ。
# この矩形内に「検出候補の中心(cx,cy)」が入った候補は overall_best 更新の対象から外す。
# 実測（DAY224）: 胴体中心≈(1005,213) size≈212×197。白箱5点(P1〜P4+左奥)は全て帯の外。
# 注意: これは固定pixel座標。カメラ位置・マット範囲が変わると帯がずれ、胴体取りこぼし or
# 白箱巻き込みの可能性がある。将来マット範囲相対へ移行する余地あり（DAY224時点は固定で可）。
BODY_EXCLUDE_ROI_PX = (850, 1150, 80, 360) # (min_x, max_x, min_y, max_y)
def _is_in_body_exclude_roi(cx, cy):
"""検出候補の中心が胴体除外ROI内なら True（= 採用しない）。"""
min_x, max_x, min_y, max_y = BODY_EXCLUDE_ROI_PX
return (min_x <= cx <= max_x) and (min_y <= cy <= max_y)
# ============================================================
# 白箱 輪郭検出 (2026-07-12・案B 実装)
# preview_place_delta.py の detect_box_by_contour と同一ロジック.
# preview はこの実装を import して使う (二重実装を避ける・単一の正).
# detect_white_box_once は「テンプレでスコア/胴体除外/角度を決める」既存経路を
# 残したまま, 中心の出所だけを輪郭に差し替える (案B). 輪郭 None なら
# テンプレ中心へ fallback して検出継続 (停止しない).
# ============================================================
BOX_CONTOUR_HSV_S_MAX = 45 # 白マスク: 彩度 < これ
BOX_CONTOUR_HSV_V_MIN = 165 # 白マスク: 明度 > これ
BOX_CONTOUR_MORPH_KERNEL = 9 # 楕円 K x K の CLOSE→OPEN
BOX_CONTOUR_AREA_MIN = 8000 # contourArea 下限
BOX_CONTOUR_AREA_MAX = 120000 # contourArea 上限
BOX_CONTOUR_ASPECT_MIN = 1.0 # max(w,h)/min(w,h) of minAreaRect (>=1)
BOX_CONTOUR_ASPECT_MAX = 1.30
BOX_CONTOUR_EXTENT_MIN = 0.75 # contourArea / (rw*rh) of minAreaRect
BOX_CONTOUR_APPROX_EPS_RATIO = 0.04 # approxPolyDP ε = 周長 * これ
BOX_CONTOUR_VTX_MIN = 4
BOX_CONTOUR_VTX_MAX = 6
# R4 (2026-07-12・アーム直上誤検出防御):
# 輪郭中心とテンプレ中心の距離が閾値超なら輪郭を採用せず fallback.
# テンプレは粗位置を当てている前提の sanity check.
# preview 実測で確認された系統ズレは最大 +41mm (画像 pixel 換算約 40 px 前後).
# 60 px は「テンプレ中心から 60 px 以上離れた輪郭中心は箱以外を掴んだ疑い」の閾値.
BOX_CONTOUR_TMPL_SANITY_MAX_PX = 60
def detect_box_center_by_contour(frame_bgr, mat_bbox=None):
"""輪郭ベースで箱中心を1個選ぶ. preview で確定済みのパラメータをそのまま使う.
フロー:
1) HSV 白マスク (S < S_MAX and V > V_MIN)
2) mat_bbox が与えられていればマット範囲外を 0 で潰す (胴体・机の白除外)
3) 楕円 K x K で MORPH_CLOSE → MORPH_OPEN
4) findContours (EXTERNAL) → 各輪郭を minAreaRect で評価:
- area(contourArea) ∈ [AREA_MIN, AREA_MAX]
- aspect = max(rw,rh)/min(rw,rh) ∈ [ASPECT_MIN, ASPECT_MAX] (回転不変)
- extent = area / (rw*rh) > EXTENT_MIN
- approxPolyDP(ε=EPS*peri) の頂点数 ∈ [VTX_MIN, VTX_MAX]
5) 候補が 1 個だけ残ればそれを採用. 0 個 / 複数なら None.
6) minAreaRect の中心と 4 頂点を返す (回転含みの実中心).
引数:
frame_bgr: BGR フレーム
mat_bbox: (min_x, max_x, min_y, max_y) or None. マット範囲外を除外.
戻り値: None または dict {
"cx": int, "cy": int, # 四角中心 (pixel)
"rect_pts": [[x,y], ...] (4 頂点・int),
"area": float, "aspect": float, "extent": float, "n_vertices": int,
}
"""
if frame_bgr is None:
return None
hsv = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2HSV)
s = hsv[:, :, 1]
v = hsv[:, :, 2]
mask = ((s < BOX_CONTOUR_HSV_S_MAX)
& (v > BOX_CONTOUR_HSV_V_MIN)).astype(np.uint8) * 255
# マット範囲外を潰す (胴体・机上の白を輪郭候補にしない).
if mat_bbox is not None:
try:
mx1, mx2, my1, my2 = mat_bbox
roi = np.zeros_like(mask)
h, w = mask.shape[:2]
x1 = max(0, int(mx1)); x2 = min(w, int(mx2))
y1 = max(0, int(my1)); y2 = min(h, int(my2))
if x2 > x1 and y2 > y1:
roi[y1:y2, x1:x2] = 255
mask = cv2.bitwise_and(mask, roi)
except (TypeError, ValueError):
# mat_bbox 形式異常は無視して全域を使う (停止しない).
pass
# R4-1 (2026-07-12・保険レベル / 任意):
# アーム胴体 ROI を白マスクから 0 で引く. 主防御は R4-2 (輪郭中心とテンプレ中心
# の距離 sanity check) と面積上限 (BOX_CONTOUR_AREA_MAX). R4-1 はそれらを
# 通り抜ける事故 (箱＋アーム結合塊で「1 個に絞れた」と誤判定) の追加保険.
# テンプレ経路 (_is_in_body_exclude_roi) と同じ矩形定数 BODY_EXCLUDE_ROI_PX を
# 再利用 (数値の複製なし・実装 1 箇所). ランタイムコストはほぼゼロなので残置.
# 将来 BODY_EXCLUDE_ROI_PX の適用範囲を見直すなら、この行の削除で
# 輪郭側だけを外すことができる (テンプレ側の胴体除外はそのまま維持される).
try:
h_m, w_m = mask.shape[:2]
_min_x, _max_x, _min_y, _max_y = BODY_EXCLUDE_ROI_PX
_bx1 = max(0, int(_min_x)); _bx2 = min(w_m, int(_max_x))
_by1 = max(0, int(_min_y)); _by2 = min(h_m, int(_max_y))
if _bx2 > _bx1 and _by2 > _by1:
mask[_by1:_by2, _bx1:_bx2] = 0
except (TypeError, ValueError):
pass
k = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE,
(BOX_CONTOUR_MORPH_KERNEL, BOX_CONTOUR_MORPH_KERNEL))
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
contours, _ = cv2.findContours(
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return None
candidates = []
for c in contours:
area = cv2.contourArea(c)
if not (BOX_CONTOUR_AREA_MIN <= area <= BOX_CONTOUR_AREA_MAX):
continue
rect = cv2.minAreaRect(c)
(rcx, rcy), (rw, rh), rang = rect
if rw < 1.0 or rh < 1.0:
continue
aspect = max(rw, rh) / min(rw, rh)
if not (BOX_CONTOUR_ASPECT_MIN <= aspect <= BOX_CONTOUR_ASPECT_MAX):
continue
extent = float(area) / float(rw * rh)
if extent < BOX_CONTOUR_EXTENT_MIN:
continue
peri = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, BOX_CONTOUR_APPROX_EPS_RATIO * peri, True)
nv = len(approx)
if not (BOX_CONTOUR_VTX_MIN <= nv <= BOX_CONTOUR_VTX_MAX):
continue
candidates.append({
"contour": c, "area": area,
"aspect": aspect, "extent": extent, "n_vertices": nv,
"rect": rect,
})
# 仕様: 1 個に絞れなければ None (0 個 / 2 個以上 とも).
if len(candidates) != 1:
return None
picked = candidates[0]
rect = picked["rect"]
(cx, cy), _size, _ang = rect
box_pts = cv2.boxPoints(rect).astype(int).tolist()
return {
"cx": int(round(cx)),
"cy": int(round(cy)),
"rect_pts": box_pts,
"area": float(picked["area"]),
"aspect": float(picked["aspect"]),
"extent": float(picked["extent"]),
"n_vertices": int(picked["n_vertices"]),
}
def evaluate_frame_gray(frame, mat_bbox, templates_by_group):
"""グレースケール版 evaluate_frame。
元の test_white_box_score.evaluate_frame と同じインターフェイス。
内部で roi と template を BGR → GRAY に変換してから cv2.matchTemplate を呼ぶ。
cv2.matchTemplate の計算量がチャネル数に比例するため、3 チャネル → 1 チャネルで
約 3 倍速になる想定。
"""
min_x, max_x, min_y, max_y = mat_bbox
roi = frame[min_y:max_y, min_x:max_x]
if roi.size == 0:
print("[06f] ERROR: ROI が空です")
return {"day165": [], "auto": [], "overall_best": None}
# ★ ここでグレースケール化 ★
roi_gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
roi_h, roi_w = roi_gray.shape[:2]
results = {"day165": [], "auto": []}
overall_best = None
excluded_best = None # 胴体除外ROIで弾いた最高スコア候補（診断ログ用）
for group, tmpls in templates_by_group.items():
for t in tmpls:
best_score = -1.0
best_scale = None
best_angle = None
best_size = (0, 0)
best_cxcy = (0, 0)
for rot in t["rotations"]:
angle = rot["angle"]
template_bgr = rot["bgr"]
# ★ テンプレもグレースケール化 ★
template_gray = cv2.cvtColor(template_bgr, cv2.COLOR_BGR2GRAY)
for scale in twbs.SCALES:
sw = int(template_gray.shape[1] * scale)
sh = int(template_gray.shape[0] * scale)
if sw < 30 or sh < 30:
continue
if sw >= roi_w or sh >= roi_h:
continue
scaled = cv2.resize(template_gray, (sw, sh))
res = cv2.matchTemplate(roi_gray, scaled, cv2.TM_CCOEFF_NORMED)
_, max_val, _, max_loc = cv2.minMaxLoc(res)
if max_val > best_score:
best_score = max_val
best_scale = scale
best_angle = angle
best_size = (sw, sh)
mx, my = max_loc
cx = mx + min_x + sw // 2
cy = my + min_y + sh // 2
best_cxcy = (cx, cy)
row = {
"name": t["name"],
"best_score": best_score,
"best_scale": best_scale,
"best_angle": best_angle,
"scaled_w": best_size[0],
"scaled_h": best_size[1],
"cx": best_cxcy[0],
"cy": best_cxcy[1],
}
results[group].append(row)
if best_score > -1.0:
cand = dict(row)
cand["group"] = group
# 胴体除外ROI内に中心が入る候補は overall_best の対象にしない
if _is_in_body_exclude_roi(cand["cx"], cand["cy"]):
cand["excluded_body_roi"] = True
if excluded_best is None or cand["best_score"] > excluded_best["best_score"]:
excluded_best = cand
else:
if overall_best is None or cand["best_score"] > overall_best["best_score"]:
overall_best = cand
results["overall_best"] = overall_best
results["excluded_best"] = excluded_best
return results
def open_camera(index, label):
print(f"[06f] opening {label} camera (index={index}) ...")
cap = cv2.VideoCapture(index)
if not cap.isOpened():
print(f"[06f] ERROR: {label} camera open failed (index={index})")
return None
print(f"[06f] {label} opened, warming up ({WARMUP_FRAMES} reads) ...")
for _ in range(WARMUP_FRAMES):
cap.read()
print(f"[06f] {label} warm-up done")
return cap
def detect_white_box_once(top_cap, templates_by_group):
if vsp._mat_px_range is None:
print("[06f] ERROR: マット bbox 未初期化")
return None
for _ in range(2):
top_cap.read()
ret, frame = top_cap.read()
if not ret or frame is None:
print("[06f] ERROR: top camera read 失敗")
return None
mat_bbox = vsp._mat_px_range
t0 = time.time()
results = evaluate_frame_gray(frame, mat_bbox, templates_by_group)
elapsed_eval = time.time() - t0
best = results.get("overall_best")
if best is None or best.get("best_score") is None:
excluded = results.get("excluded_best")
if excluded is not None:
print(
f"[06f] DETECT: overall_best=None (eval {elapsed_eval:.2f}s) "
f"※胴体ROIで除外した最高スコア候補あり: "
f"score={excluded.get('best_score'):.3f} "
f"center=({excluded.get('cx')},{excluded.get('cy')}) "
f"name={excluded.get('name')}"
)
else:
print(f"[06f] DETECT: overall_best=None (eval {elapsed_eval:.2f}s)")
return None
score = best["best_score"]
tmpl_cx, tmpl_cy = best["cx"], best["cy"]
angle = best.get("best_angle") or 0
scale = best.get("best_scale") or 1.0
scaled_w = best.get("scaled_w") or 200
scaled_h = best.get("scaled_h") or 200
name = best.get("name", "?")
# 2026-07-12 案B: 中心を輪郭ベースに差し替え. テンプレは fallback.
# テンプレマッチ (evaluate_frame_gray) は上流で「箱の粗位置検出・胴体ROI除外・
# score 閾値判定」に使い続ける. 中心の出所だけを contour に差し替える.
# R3 (2026-07-12): 輪郭検出の全例外を握って fallback へ.
# run を絶対止めない.
try:
contour_r = detect_box_center_by_contour(frame, mat_bbox)
except Exception as _e_contour:
contour_r = None
print(f"[06f] WARN: 輪郭検出 例外 → fallback ({type(_e_contour).__name__}: {_e_contour})")
# R4-2 (2026-07-12): sanity check. 輪郭中心とテンプレ中心の距離が
# BOX_CONTOUR_TMPL_SANITY_MAX_PX を超えていたら箱以外を掴んだ疑いで
# 輪郭を捨てて fallback. テンプレは粗位置は当てている前提.
sanity_ok = True
if contour_r is not None:
_dcx = contour_r["cx"] - tmpl_cx
_dcy = contour_r["cy"] - tmpl_cy
_dist = (_dcx * _dcx + _dcy * _dcy) ** 0.5
if _dist > BOX_CONTOUR_TMPL_SANITY_MAX_PX:
print(f"[06f] WARN: 輪郭中心とテンプレ中心の距離が閾値超 "
f"|Δ|={_dist:.1f}px > {BOX_CONTOUR_TMPL_SANITY_MAX_PX}px "
f"→ 輪郭捨てて fallback")
sanity_ok = False
if contour_r is not None and sanity_ok:
cx, cy = contour_r["cx"], contour_r["cy"]
center_source = "contour"
dcx = cx - tmpl_cx
dcy = cy - tmpl_cy
print(f"[06f] DETECT: center=({cx},{cy}) source=contour "
f"Δvs_tmpl=({dcx:+d},{dcy:+d})px "
f"aspect={contour_r['aspect']:.3f} extent={contour_r['extent']:.3f} "
f"score={score:.3f} angle={angle}° scale={scale} "
f"size=({scaled_w}x{scaled_h}) template={name} (eval {elapsed_eval:.2f}s)")
else:
cx, cy = tmpl_cx, tmpl_cy
center_source = "template_fallback"
if contour_r is None:
print(f"[06f] WARN: 輪郭検出失敗 → テンプレ中心に fallback")
print(f"[06f] DETECT: center=({cx},{cy}) source=template_fallback "
f"score={score:.3f} angle={angle}° scale={scale} "
f"size=({scaled_w}x{scaled_h}) template={name} (eval {elapsed_eval:.2f}s)")
below = score < TEMPLATE_SCORE_THRESHOLD
if below:
print(f"[06f] WARN: score < threshold ({TEMPLATE_SCORE_THRESHOLD})")
return {
"center": (cx, cy),
"center_source": center_source,
"center_template": (tmpl_cx, tmpl_cy),
"score": score,
"angle": angle,
"scale": scale,
"scaled_w": scaled_w,
"scaled_h": scaled_h,
"name": name,
"below_threshold": below,
}
def draw_overlay(frame, detection):
if detection is None:
return
cx, cy = detection["center"]
score = detection["score"]
angle = detection["angle"]
scale = detection["scale"]
sw = detection["scaled_w"]
sh = detection["scaled_h"]
below = detection.get("below_threshold", False)
draw_angle = -float(angle)
rect = ((cx, cy), (float(sw), float(sh)), draw_angle)
box_pts = cv2.boxPoints(rect).astype(int)
color = (0, 0, 255)
cv2.polylines(frame, [box_pts], isClosed=True, color=color, thickness=3)
cv2.circle(frame, (cx, cy), 10, color, -1)
tag = " [BELOW THRESHOLD]" if below else ""
cv2.putText(frame,
f"DETECTED: ({cx},{cy}) angle={angle} (draw={draw_angle:.0f}) "
f"scale={scale} size=({sw}x{sh}) score={score:.3f}{tag}",
(20, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
def main():
print("[06f] start")
t_total = time.time()
try:
from camera_resolver import resolve_cameras
cams = resolve_cameras(require_top=True, require_side=False)
top_idx = cams["top"]
side_idx = cams["side"] if cams["side"] is not None else SIDE_CAMERA_INDEX
except Exception as e:
print(f"[06f] resolver 失敗、定数 index にフォールバック: {e}")
top_idx = TOP_CAMERA_INDEX
side_idx = SIDE_CAMERA_INDEX
side_cap = open_camera(side_idx, "side")
top_cap = open_camera(top_idx, "top")
if side_cap is None or top_cap is None:
print("[06f] ABORT: camera open failed")
if side_cap is not None:
side_cap.release()
if top_cap is not None:
top_cap.release()
return
print("[06f] setting up homography and mat range ...")
try:
vsp.setup_homography_and_mat_range(top_cap)
print(f"[06f] mat_px_range = {vsp._mat_px_range}")
except Exception as e:
print(f"[06f] ERROR: homography セットアップ失敗: {e}")
side_cap.release()
top_cap.release()
return
print(f"[06f] loading white box templates from {twbs.TEMPLATES_DIR} ...")
print(f"[06f] (angles {twbs.ROTATION_ANGLES_DEG}, scales {twbs.SCALES})")
try:
templates_by_group = _load_white_box_templates()
except Exception as e:
print(f"[06f] ERROR: テンプレ読込失敗: {e}")
side_cap.release()
top_cap.release()
return
print(f"[06f] ready (setup {time.time() - t_total:.2f}s)")
print(f"[06f] entering preview loop, detection at +{DETECT_AFTER_SEC}s")
print("[06f] press 'q' to quit")
t_start = time.time()
detection = None
detected = False
try:
while True:
ret_s, frame_s = side_cap.read()
ret_t, frame_t = top_cap.read()
elapsed = time.time() - t_start
if not detected and elapsed >= DETECT_AFTER_SEC:
detected = True
print(f"[06f] running detection at t={elapsed:.2f}s")
detection = detect_white_box_once(top_cap, templates_by_group)
t_done = time.time() - t_start
print(f"[06f] detection done at t={t_done:.2f}s")
label = (f"t={elapsed:.1f}s "
f"detected={'YES' if detected else 'pending'} q=quit")
if ret_s and frame_s is not None:
disp_s = frame_s.copy()
cv2.putText(disp_s, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
cv2.putText(disp_s, "SIDE camera", (20, 80),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
cv2.imshow("side", disp_s)
if ret_t and frame_t is not None:
disp_t = frame_t.copy()
cv2.putText(disp_t, label, (20, 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
cv2.putText(disp_t, "TOP camera", (20, 80),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
draw_overlay(disp_t, detection)
cv2.imshow("top", disp_t)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
print("[06f] 'q' pressed, exiting")
break
except KeyboardInterrupt:
print("[06f] Ctrl+C detected")
finally:
side_cap.release()
top_cap.release()
cv2.destroyAllWindows()
print("[06f] done")
if __name__ == "__main__":
main()test_white_box_score.py（全ファイル・load_templates のディレクトリ解決修正含む）
#!/usr/bin/env python3
"""
test_white_box_score.py (DAY209 Step 0)
上カメラ 1 フレームに対し、~/r2r2r-scripts/templates/ の全 PNG で
白箱検出のスコア（cv2.TM_CCOEFF_NORMED）を測る検証スクリプト。
実機（アーム）は一切動かさない。カメラ + テンプレ + マット範囲検出のみ。
DAY165 時点のテンプレ資産が現在の照明条件で機能するかをグループ別に評価する：
- white_box_2026* : DAY165 撮影分（6 枚）
- white_box_auto_* : auto 学習分（6 枚）
"""
import os
import sys
import glob
import time
from datetime import datetime
import cv2
import numpy as np
from mat_corner_detection import (
capture_frames,
detect_corners,
remove_outliers_and_average,
)
# ============================================================
# 定数
# ============================================================
CAMERA_INDEX = 2
WARMUP_FRAMES = 30
TEMPLATES_DIR = os.path.expanduser("~/r2r2r-scripts/templates")
LOGS_DIR = os.path.expanduser("~/r2r2r-scripts/logs")
SCALES = [0.70, 0.85, 1.00, 1.15, 1.30]
# ============================================================
# 回転探索について
# ============================================================
# 本番側 r2r2r_loop.py:488 の find_white_box() は回転探索を実装していない。
# 本スクリプトは PLACE 実装のための要件出しとして、白箱の回転を意図的に検証する。
# 本番側にも同じ 3 角度回転探索を移植する作業は、PLACE フェーズ実装と同時に行う予定。
# 「本番スコアと完全一致」を測定する用途ではここを単一角度に戻すこと。
# ============================================================
ROTATION_ANGLES_DEG = [0, 30, 60] # 白箱は正方形なので 90° 周期。30° 刻みで 3 角度
DETECT_THRESHOLD = 0.5
# マット範囲検出のフレーム取得設定（mat_corner_detection.py のデフォルトに合わせる）
MAT_NUM_FRAMES = 10
MAT_WARMUP = 3
MAT_INTERVAL = 0.1
# 本番 r2r2r_loop.py:251 は 1 枚成功で進むが、検証側はスコア再現性を確保するため
# 意図的に 3 枚以上を要求する（マット bbox がブレるとスコア比較が無意味になるため）。
MIN_SUCCESS_FRAMES = 3
# ============================================================
# テンプレートロード
# ============================================================
def classify_group(filename):
"""ファイル名から所属グループを返す。"""
base = os.path.basename(filename)
if base.startswith("white_box_auto_"):
return "auto"
if base.startswith("white_box_2026"):
return "day165"
return "day165" # それ以外は DAY165 扱い（warning は呼び出し側で）
GROUP_LABEL = {
"day165": "DAY165 撮影分（white_box_2026*）",
"auto": "auto 学習分（white_box_auto_*）",
}
def rotate_image_keep_size(img_bgr, angle_deg):
"""画像を中心で回転。サイズは元のまま保持し、はみ出した部分は四隅平均色で埋める。
四隅 4 ピクセルの平均色を背景として使うことで、matchTemplate の余白が
マット背景に近い色になり、合成境界がスコアを過剰に下げないようにする。
"""
h, w = img_bgr.shape[:2]
center = (w / 2.0, h / 2.0)
corners = [
img_bgr[0, 0], img_bgr[0, w - 1],
img_bgr[h - 1, 0], img_bgr[h - 1, w - 1],
]
bg_color = tuple(int(np.mean([c[i] for c in corners])) for i in range(3))
M = cv2.getRotationMatrix2D(center, angle_deg, 1.0)
rotated = cv2.warpAffine(
img_bgr, M, (w, h),
flags=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
borderValue=bg_color,
)
return rotated
def load_templates(templates_dir=None):
"""templates/ 配下の全 PNG をロードしてグループ別に返す。
2026-07-12 案A: 引数 templates_dir を追加 (default None).
None のときはモジュール変数 TEMPLATES_DIR を **関数実行時に** 参照する.
06f が実行時に `twbs.TEMPLATES_DIR` を上書きしたケースに追従できる.
(Python の名前解決仕様上, 関数内での TEMPLATES_DIR 参照は module globals
から call-time に解決されるので, 引数なしでも上書きは効くが, 明示引数を
渡せば「どのディレクトリを読むか」を呼び出し側の意図として固定できる.)
呼び出し側 (06f 側) は無変更で動く. 明示的に渡したいときだけ使う.
"""
_dir = (templates_dir
if templates_dir is not None
else TEMPLATES_DIR)
_pat = os.path.join(_dir, "*.png")
# 診断ログ: 実行時に見えているディレクトリと glob 結果を stderr へ出す.
# 「6 枚あるはずが 8 枚読まれる」等の切り分けが即座にできるようにする.
print(f"[テンプレ] loading white box templates from: {_dir}",
file=sys.stderr)
paths = sorted(glob.glob(_pat))
print(f"[テンプレ] glob '{_pat}' → {len(paths)} file(s)",
file=sys.stderr)
for p in paths:
print(f"[テンプレ] - {os.path.basename(p)}", file=sys.stderr)
if not paths:
print(f"[ERROR] テンプレートが見つかりません: {_dir}")
sys.exit(1)
by_group = {"day165": [], "auto": []}
total = 0
for p in paths:
img = cv2.imread(p, cv2.IMREAD_COLOR)
if img is None:
print(f"[WARN] 読込失敗: {p}")
continue
base = os.path.basename(p)
group = classify_group(base)
if not base.startswith(("white_box_auto_", "white_box_2026")):
print(f"[WARN] 命名規則外のテンプレ: {base} → day165 扱い")
# 本番 r2r2r_loop.py:497 はカラー(BGR)のまま matchTemplate を呼ぶ。
# スコアを本番と揃えるため、ここでもグレースケール化せず BGR を保持。
# 斜め置き対応のため ROTATION_ANGLES_DEG ぶんを事前生成しておく
# （0° は元画像そのまま、回転コストゼロ）。
rotations = []
for ang in ROTATION_ANGLES_DEG:
if ang == 0:
rotations.append({"angle": 0, "bgr": img})
else:
rotations.append({"angle": ang,
"bgr": rotate_image_keep_size(img, ang)})
by_group[group].append({
"name": base,
"rotations": rotations,
"w": img.shape[1],
"h": img.shape[0],
})
total += 1
if total == 0:
print("[ERROR] テンプレートを 1 枚も読み込めませんでした"
"（PNG は存在するが cv2.imread が全件失敗）")
sys.exit(1)
print(f"[テンプレ] 計 {total} 枚 "
f"(day165={len(by_group['day165'])}, auto={len(by_group['auto'])})")
return by_group
# ============================================================
# マット範囲検出
# ============================================================
def detect_mat_range(cap):
"""カメラから複数フレームを取り、マットの 4 隅と bbox を返す。
戻り値: (corners_4x2_float, (min_x, max_x, min_y, max_y))
失敗時は sys.exit(1)。
"""
print(f"[マット検出] {MAT_NUM_FRAMES} フレーム取得 ...")
frames = capture_frames(cap, MAT_NUM_FRAMES, MAT_WARMUP, MAT_INTERVAL)
if not frames:
print("[ERROR] マット検出用フレームが取得できません")
sys.exit(1)
all_corners = []
for i, f in enumerate(frames):
corners = detect_corners(f, debug=False, frame_idx=i)
if corners is not None:
all_corners.append(corners)
if len(all_corners) < MIN_SUCCESS_FRAMES:
print(f"[ERROR] マット隅検出成功 {len(all_corners)} 枚 < {MIN_SUCCESS_FRAMES}")
sys.exit(1)
avg = remove_outliers_and_average(all_corners) # (4, 2)
xs = avg[:, 0]
ys = avg[:, 1]
# 画像端ギリギリの検出で NumPy 負インデックス（末尾参照）が起きないよう、
# frame サイズで bbox をクリップする
frame_h, frame_w = frames[0].shape[:2]
min_x = max(0, int(np.floor(xs.min())))
max_x = min(frame_w - 1, int(np.ceil(xs.max())))
min_y = max(0, int(np.floor(ys.min())))
max_y = min(frame_h - 1, int(np.ceil(ys.max())))
if max_x <= min_x or max_y <= min_y:
print(f"[ERROR] マット bbox が不正です: "
f"x=[{min_x},{max_x}], y=[{min_y},{max_y}]")
sys.exit(1)
print(f"[マット検出] 成功 {len(all_corners)}/{len(frames)}枚 → "
f"bbox=({min_x},{min_y})-({max_x},{max_y})")
return avg, (min_x, max_x, min_y, max_y)
# ============================================================
# スコア評価
# ============================================================
def evaluate_frame(frame, mat_bbox, templates_by_group):
"""frame に対し全テンプレ × 全スケールで matchTemplate を実行。
戻り値:
{
"day165": [{"name", "best_score", "best_scale", "scaled_w", "scaled_h",
"cx", "cy"}],
"auto": [...],
"overall_best": {同上 + "group"},
}
マット ROI が小さすぎる等で 1 件もスコアが取れなかった場合は overall_best=None。
"""
min_x, max_x, min_y, max_y = mat_bbox
# 本番（r2r2r_loop.py:481）と同じ exclusive な切り出しに統一
roi = frame[min_y:max_y, min_x:max_x]
if roi.size == 0:
print("[ERROR] ROI が空です（マット範囲が画面外？）")
return {"day165": [], "auto": [], "overall_best": None}
# 本番（r2r2r_loop.py:497）に合わせてカラー(BGR)のまま照合
roi_bgr = roi
roi_h, roi_w = roi_bgr.shape[:2]
results = {"day165": [], "auto": []}
overall_best = None
for group, tmpls in templates_by_group.items():
for t in tmpls:
best_score = -1.0
best_scale = None
best_angle = None
best_size = (0, 0)
best_cxcy = (0, 0)
for rot in t["rotations"]:
angle = rot["angle"]
template_bgr = rot["bgr"]
for scale in SCALES:
sw = int(template_bgr.shape[1] * scale)
sh = int(template_bgr.shape[0] * scale)
if sw < 30 or sh < 30:
continue
if sw >= roi_w or sh >= roi_h:
continue
scaled = cv2.resize(template_bgr, (sw, sh))
res = cv2.matchTemplate(roi_bgr, scaled, cv2.TM_CCOEFF_NORMED)
_, max_val, _, max_loc = cv2.minMaxLoc(res)
if max_val > best_score:
best_score = max_val
best_scale = scale
best_angle = angle
best_size = (sw, sh)
mx, my = max_loc
cx = mx + min_x + sw // 2
cy = my + min_y + sh // 2
best_cxcy = (cx, cy)
row = {
"name": t["name"],
"best_score": best_score,
"best_scale": best_scale,
"best_angle": best_angle,
"scaled_w": best_size[0],
"scaled_h": best_size[1],
"cx": best_cxcy[0],
"cy": best_cxcy[1],
}
results[group].append(row)
if best_score > -1.0:
cand = dict(row)
cand["group"] = group
if overall_best is None or cand["best_score"] > overall_best["best_score"]:
overall_best = cand
results["overall_best"] = overall_best
return results
# ============================================================
# 表示
# ============================================================
def print_group_table(group_key, rows):
print(f"\n=== {GROUP_LABEL[group_key]} ===")
if not rows:
print(" （テンプレートなし）")
return
print(f"{'name':<42}{'best_score':>11}{'scale':>8}{'angle':>8}{'size':>11}")
valid_scores = []
for r in rows:
score = r["best_score"]
scale = r["best_scale"]
angle = r.get("best_angle")
if scale is None:
scale_str = " — "
angle_str = " — "
size_str = " — "
else:
scale_str = f"{scale:.2f}"
angle_str = f"{angle}°"
size_str = f"{r['scaled_w']}x{r['scaled_h']}"
valid_scores.append(score)
print(f"{r['name']:<42}{score:>11.3f}{scale_str:>8}{angle_str:>8}{size_str:>11}")
print("─" * 80)
if valid_scores:
print(f"{'平均':<42}{np.mean(valid_scores):>11.3f}")
print(f"{'最高':<42}{max(valid_scores):>11.3f}")
else:
print(" （有効スコアなし）")
def print_overall(overall_best):
print("\n" + "=" * 72)
if overall_best is None:
print("★ 全体最高スコア: なし")
print("=" * 72)
return
score = overall_best["best_score"]
mark = "✓ PASS" if score >= DETECT_THRESHOLD else "✗ FAIL"
print(f"★ 全体最高スコア: {score:.3f}")
print(f" template: {overall_best['name']}")
print(f" group: {GROUP_LABEL[overall_best['group']]}")
print(f" scale: {overall_best['best_scale']}")
print(f" angle: {overall_best.get('best_angle')}°")
print(f" center: pixel=({overall_best['cx']}, {overall_best['cy']})")
print(f" threshold {DETECT_THRESHOLD}: {mark}")
print("=" * 72)
# ============================================================
# Debug 画像保存
# ============================================================
def save_debug_frame(frame, mat_corners, mat_bbox, overall_best):
img = frame.copy()
min_x, max_x, min_y, max_y = mat_bbox
# マット範囲: 黄色矩形
cv2.rectangle(img, (min_x, min_y), (max_x, max_y), (0, 255, 255), 2)
# マット四隅: 緑円
for (x, y) in mat_corners:
cv2.circle(img, (int(x), int(y)), 8, (0, 255, 0), 2)
# 最高スコアテンプレ矩形 + 中心
if overall_best is not None and overall_best["best_scale"] is not None:
cx = overall_best["cx"]
cy = overall_best["cy"]
sw = overall_best["scaled_w"]
sh = overall_best["scaled_h"]
angle = overall_best.get("best_angle") or 0
if angle == 0:
x1 = cx - sw // 2
y1 = cy - sh // 2
x2 = x1 + sw
y2 = y1 + sh
cv2.rectangle(img, (x1, y1), (x2, y2), (0, 0, 255), 3)
else:
# 画像座標は Y 軸が下向き。matchTemplate でテンプレを angle 回した結果が
# 一致したということは、frame 上の白箱はテンプレに対して -angle ぶん
# 回って見えている。boxPoints の angle 符号は実機で要調整。
rect = ((cx, cy), (sw, sh), -angle)
box = cv2.boxPoints(rect).astype(int)
cv2.polylines(img, [box], isClosed=True, color=(0, 0, 255), thickness=3)
cv2.circle(img, (cx, cy), 10, (0, 0, 255), -1)
label = (f"{overall_best['name']} "
f"score={overall_best['best_score']:.3f} angle={angle}°")
label_y = max(20, cy - sh // 2 - 10)
cv2.putText(
img, label, (cx - sw // 2, label_y),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2, cv2.LINE_AA,
)
os.makedirs(LOGS_DIR, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
out_path = os.path.join(LOGS_DIR, f"debug_white_score_DAY209_{ts}.jpg")
cv2.imwrite(out_path, img)
print(f"[Debug 保存] {out_path}")
# ============================================================
# main
# ============================================================
def main():
print("=" * 72)
print("test_white_box_score.py (DAY209 Step 0)")
print("=" * 72)
# (1) テンプレ読み込み
templates_by_group = load_templates()
# (2) カメラオープン + warmup
print(f"\n[カメラ] open index={CAMERA_INDEX}")
cap = cv2.VideoCapture(CAMERA_INDEX)
if not cap.isOpened():
print(f"[ERROR] カメラオープン失敗 (index={CAMERA_INDEX})")
sys.exit(1)
for _ in range(WARMUP_FRAMES):
cap.read()
try:
# (3) マット範囲検出
mat_corners, mat_bbox = detect_mat_range(cap)
# (4) プレビューウィンドウ
win = "DAY209 white-box score test"
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
print("\n[操作] SPACE: スコア測定 / Q: 終了")
while True:
ret, frame = cap.read()
if not ret:
time.sleep(0.05)
continue
preview = frame.copy()
min_x, max_x, min_y, max_y = mat_bbox
cv2.rectangle(preview, (min_x, min_y), (max_x, max_y), (0, 255, 255), 2)
cv2.putText(
preview, "SPACE: measure score | Q: quit",
(20, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (255, 255, 255), 2, cv2.LINE_AA,
)
cv2.imshow(win, preview)
key = cv2.waitKey(1) & 0xFF
if key == ord('q') or key == ord('Q') or key == 27:
print("[終了] Q キー")
break
if key == ord(' '):
# スコア測定
results = evaluate_frame(frame, mat_bbox, templates_by_group)
print_group_table("day165", results["day165"])
print_group_table("auto", results["auto"])
print_overall(results["overall_best"])
save_debug_frame(frame, mat_corners, mat_bbox, results["overall_best"])
print("\n[操作] SPACE: 再測定 / Q: 終了")
finally:
cap.release()
cv2.destroyAllWindows()
print("[DONE]")
if __name__ == "__main__":
main()preview_place_delta.py（全ファイル・新規・箱中心プレビュー）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""preview_place_delta.py — 上カメラ ライブプレビュー + place Δ HUD.
place サーボの Δx / Δz / Δdist をライブ映像上に重畳して目視確認するための
独立ビューワ. 本線 (run_pick_place.py) には一切手を入れない.
検出・homography コードは import のみで再実装しない (=プレビューと本線で
必ず同じ座標が出る保証).
読み取り専用. アーム制御・daemon 接続は絶対にしない.
使い方:
cd ~/r2r2r-scripts/dynamic_tracking_limits/place/place_modules
python3 preview_place_delta.py
ウィンドウで:
q 終了
s 現フレームを KIOXIA (diag/preview_captures/) へ保存 (未マウント時は skip)
表示:
- 白箱中心: シアン十字 + "BOX (x, z)" [mm]
- 赤ブロック中心: 緑十字 + "BLOCK (x, z)" [mm]
- 2 点を結ぶ直線
- HUD: box / block / Δx / Δz / Δdist [mm 小数1桁]
- HUD: 該当セルの release_offset_z_mm (place_release_offset.json を読む)
- HUD: fps / box 検出間隔 / 保存ステータス
技術:
camera_resolver.resolve_cameras(require_top=True) で Continuity 解決
visual_servo_pick.setup_homography_and_mat_range(cap) で homography
visual_servo_pick.detect_red_block(frame) で赤ブロック (毎フレーム)
visual_servo_pick.pixel_to_unity(px, py) で mm 変換
06f_top_white_box_detect_v7._load_white_box_templates() / detect_white_box_once
で白箱 (BOX_DETECT_INTERVAL_SEC ごとに間引き)
"""
from __future__ import print_function
import contextlib
import importlib
import json
import math
import sys
import time
from datetime import datetime
from pathlib import Path
# パス解決: place_modules と r2r2r-scripts ルートを通す
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
import cv2 # noqa: E402
import numpy as np # noqa: E402
# 既存関数を import のみ (実装は複製しない)
from camera_resolver import resolve_cameras # noqa: E402
from visual_servo_pick import ( # noqa: E402
setup_homography_and_mat_range,
pixel_to_unity,
detect_red_block,
)
# ---- 定数 ----
WARMUP_FRAMES = 30
BOX_DETECT_INTERVAL_SEC = 1.5 # 白箱は重い→間引き. block は毎フレーム
WINDOW_NAME = "preview_place_delta (q:quit / s:save)"
FONT = cv2.FONT_HERSHEY_SIMPLEX
# ---- 白箱 輪郭検出 (2026-07-12: 実装は 06f_top_white_box_detect_v7 側・単一の正) ----
# 案B 移植で 06f に detect_box_center_by_contour を配置した後、preview は
# それを import して使う (二重実装を避ける). 定数群も 06f 側に集約.
# 06f module は _load_box_detector() 内で lazy load される. その戻りから
# `detect_box_center_by_contour` を取り出して使う.
# セルキー計算 (run_pick_place の _corr_key と同一格子). 3 行の純計算なので
# 検出/homography 複製ではなく単純な数式として再実装する.
CELL_MM = 20
# release offset (本線 run_pick_place と同じパス. 本線が書いた値を読むだけ)
RELEASE_OFFSET_PATH = Path(
"/Volumes/KIOXIA/r2r2r-data/diag/place_release_offset.json")
SAVE_DIR = Path("/Volumes/KIOXIA/r2r2r-data/diag/preview_captures")
def _corr_key_place(x_m, z_m):
"""(x, z)[m] → 'place_x{cx}_z{cz}'. 20mm 格子で round.
本線 run_pick_place の _corr_key('place', x, z) と同一の格子.
数式のみ (検出/homography ではないため複製ではなく再実装可)."""
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 "place_x{}_z{}".format(cx, cz)
def _read_release_offset_mm(cell_key):
"""place_release_offset.json から offset_z_mm を取る. 失敗時は None.
ファイル無し / 未マウント / パース失敗 / セル未登録 全て None."""
if not RELEASE_OFFSET_PATH.exists():
return None
try:
with open(RELEASE_OFFSET_PATH, "r", encoding="utf-8") as f:
mem = json.load(f)
except Exception:
return None
e = mem.get(cell_key)
if not isinstance(e, dict):
return None
try:
return float(e.get("offset_z_mm", 0.0))
except (TypeError, ValueError):
return None
def _ssd_writable():
"""KIOXIA が書き込み可能か. 保存ガード用."""
try:
SAVE_DIR.parent.exists() # 親 (diag) の存在
except Exception:
return False
return SAVE_DIR.parent.exists() and SAVE_DIR.parent.is_dir()
def _save_frame(frame):
"""現フレームを KIOXIA へ保存. 未マウント時は False で継続."""
if not _ssd_writable():
return False, "KIOXIA 未マウント"
try:
SAVE_DIR.mkdir(parents=True, exist_ok=True)
fn = "preview_" + datetime.now().strftime("%Y%m%d_%H%M%S") + ".jpg"
p = SAVE_DIR / fn
if not cv2.imwrite(str(p), frame):
return False, "imwrite 失敗"
return True, str(p)
except Exception as e:
return False, "{}: {}".format(type(e).__name__, e)
def _load_box_detector():
"""06f を lazy に読み込み. テンプレ欠落等の SystemExit を握って None 返す."""
try:
m06f = importlib.import_module("06f_top_white_box_detect_v7")
templates = m06f._load_white_box_templates()
return m06f, templates
except (Exception, SystemExit) as e:
print("[PREVIEW] 06f 初期化失敗 ({}: {}). box 検出なしで続行".format(
type(e).__name__, e), file=sys.stderr)
return None, None
def _draw_crosshair(img, cx, cy, color, size=14, thick=2):
cv2.line(img, (cx - size, cy), (cx + size, cy), color, thick)
cv2.line(img, (cx, cy - size), (cx, cy + size), color, thick)
def _fmt_mm(v):
return "--" if v is None else "{:+7.1f}".format(v * 1000.0)
def _fmt_mm_scalar(mm):
return "--" if mm is None else "{:+7.1f}".format(mm)
def _draw_hud(frame, box_xz, block_xz, dx_mm, dz_mm, dist_mm,
cell_key, offset_mm, fps, save_msg,
box_contour_xz=None, delta_vs_template_mm=None):
"""左上に半透明パネル + テキスト. 検出失敗は '--'.
box_contour_xz / delta_vs_template_mm は新設 (輪郭検出との比較用)."""
h, w = frame.shape[:2]
panel_h = 252
overlay = frame.copy()
cv2.rectangle(overlay, (0, 0), (max(600, w // 2), panel_h),
(0, 0, 0), -1)
cv2.addWeighted(overlay, 0.55, frame, 0.45, 0, frame)
def put(text, y, color=(255, 255, 255), scale=0.55):
cv2.putText(frame, text, (12, y), FONT, scale, color, 1,
cv2.LINE_AA)
put("box (T) = ({}, {}) mm".format(
_fmt_mm(box_xz[0] if box_xz else None),
_fmt_mm(box_xz[1] if box_xz else None)),
24, (200, 240, 255))
put("box (C) = ({}, {}) mm contour".format(
_fmt_mm(box_contour_xz[0] if box_contour_xz else None),
_fmt_mm(box_contour_xz[1] if box_contour_xz else None)),
46, (0, 255, 255))
put("block = ({}, {}) mm".format(
_fmt_mm(block_xz[0] if block_xz else None),
_fmt_mm(block_xz[1] if block_xz else None)),
66, (200, 255, 200))
put("d(vs T) = ({}, {}) mm (C - T)".format(
_fmt_mm_scalar(delta_vs_template_mm[0]
if delta_vs_template_mm else None),
_fmt_mm_scalar(delta_vs_template_mm[1]
if delta_vs_template_mm else None)),
90, (0, 255, 255))
put("dx = {} mm (block - box_T)".format(_fmt_mm_scalar(dx_mm)), 116)
put("dz = {} mm".format(_fmt_mm_scalar(dz_mm)), 136)
put("ddist = {} mm".format(_fmt_mm_scalar(dist_mm)), 156,
(255, 220, 100), 0.6)
put("cell = {}".format(cell_key or "--"), 184)
put("offset = {} mm (place_release_offset.json)".format(
_fmt_mm_scalar(offset_mm)), 204)
put("fps = {:.1f} box interval = {:.1f} s".format(
fps, BOX_DETECT_INTERVAL_SEC), 224)
put("save = {}".format(save_msg or ""), 244,
(200, 200, 200), 0.5)
# ヘルプ (右上)
cv2.putText(frame, "q:quit s:save frame", (w - 220, 24),
FONT, 0.5, (200, 200, 200), 1, cv2.LINE_AA)
def main():
print("[PREVIEW] camera_resolver で top カメラ解決中...")
try:
# resolver の verbose print を stderr へ逃がす (stdout は空に保つ)
with contextlib.redirect_stdout(sys.stderr):
cams = resolve_cameras(require_top=True, require_side=False,
verbose=True)
except (Exception, SystemExit) as e:
print("[PREVIEW] camera_resolver 例外: {}: {}".format(
type(e).__name__, e), file=sys.stderr)
return 1
top_idx = cams.get("top") if cams else None
if top_idx is None:
print("[PREVIEW] top カメラ未解決. Continuity/USB を確認.",
file=sys.stderr)
return 1
print("[PREVIEW] top index = {}".format(top_idx))
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
print("[PREVIEW] cap.open 失敗 index={}".format(top_idx),
file=sys.stderr)
return 1
print("[PREVIEW] warm-up {} frames...".format(WARMUP_FRAMES))
for _ in range(WARMUP_FRAMES):
cap.read()
print("[PREVIEW] homography セットアップ...")
try:
if not setup_homography_and_mat_range(cap):
print("[PREVIEW] homography 失敗", file=sys.stderr)
cap.release()
return 1
except (Exception, SystemExit) as e:
print("[PREVIEW] homography 例外: {}: {}".format(
type(e).__name__, e), file=sys.stderr)
cap.release()
return 1
print("[PREVIEW] 06f 白箱テンプレを lazy ロード...")
m06f, templates = _load_box_detector()
print("[PREVIEW] ウィンドウ起動. q: 終了 / s: 保存")
last_box_detect_t = 0.0
last_box_result = None # {"cx","cy","score","xz_m"} or None
fps_ewma = 0.0
prev_t = time.time()
save_msg = ""
save_msg_expire = 0.0
try:
while True:
ok, frame = cap.read()
if not ok or frame is None:
print("[PREVIEW] フレーム取得失敗", file=sys.stderr)
time.sleep(0.05)
continue
now = time.time()
dt = max(now - prev_t, 1e-6)
prev_t = now
fps_ewma = 0.9 * fps_ewma + 0.1 * (1.0 / dt)
# ---- 白箱検出 (間引き) ----
if (m06f is not None and templates is not None
and (now - last_box_detect_t) >= BOX_DETECT_INTERVAL_SEC):
last_box_detect_t = now
try:
det = m06f.detect_white_box_once(cap, templates)
except (Exception, SystemExit) as e:
det = None
print("[PREVIEW] box 検出例外: {}".format(e),
file=sys.stderr)
if (det is not None
and not det.get("below_threshold", False)):
try:
bcx, bcy = det["center"]
ux, uz = pixel_to_unity(bcx, bcy)
last_box_result = {
"cx": int(bcx), "cy": int(bcy),
"score": det.get("score"),
"xz_m": (float(ux), float(uz)),
}
except Exception as e:
print("[PREVIEW] box 座標変換例外: {}".format(e),
file=sys.stderr)
last_box_result = None
else:
last_box_result = None
# ---- 白箱 輪郭検出 (毎フレーム・従来テンプレと並列) ----
# 実装は 06f の detect_box_center_by_contour を使う (単一の正).
# 06f が未 load / 関数未定義なら None を返す (preview を落とさない).
# mat_bbox は visual_servo_pick が homography セットアップで設定した
# マット範囲. マット外 (胴体/机) の白を候補にしない.
box_contour_result = None
_bc_fn = getattr(m06f, "detect_box_center_by_contour", None) \
if m06f is not None else None
try:
import visual_servo_pick as _vsp_mod
_mat_bbox = getattr(_vsp_mod, "_mat_px_range", None)
except Exception:
_mat_bbox = None
try:
_bc = (_bc_fn(frame, _mat_bbox)
if _bc_fn is not None else None)
except Exception as e:
_bc = None
print("[PREVIEW] box_contour 例外: {}".format(e),
file=sys.stderr)
if _bc is not None:
try:
ux, uz = pixel_to_unity(_bc["cx"], _bc["cy"])
box_contour_result = {
"cx": _bc["cx"], "cy": _bc["cy"],
"rect_pts": _bc["rect_pts"],
"xz_m": (float(ux), float(uz)),
}
except Exception as e:
print("[PREVIEW] box_contour 座標変換例外: {}".format(e),
file=sys.stderr)
box_contour_result = None
# ---- 赤ブロック検出 (毎フレーム) ----
block_result = None
try:
bpix = detect_red_block(frame)
except Exception as e:
bpix = None
print("[PREVIEW] block 検出例外: {}".format(e),
file=sys.stderr)
if bpix is not None:
try:
bcx, bcy = bpix
ux, uz = pixel_to_unity(bcx, bcy)
block_result = {
"cx": int(bcx), "cy": int(bcy),
"xz_m": (float(ux), float(uz)),
}
except Exception as e:
print("[PREVIEW] block 座標変換例外: {}".format(e),
file=sys.stderr)
block_result = None
# ---- Δ 計算 ----
box_xz = last_box_result["xz_m"] if last_box_result else None
box_c_xz = (box_contour_result["xz_m"]
if box_contour_result else None)
blk_xz = block_result["xz_m"] if block_result else None
dx_mm = dz_mm = dist_mm = None
if box_xz is not None and blk_xz is not None:
dx_mm = (blk_xz[0] - box_xz[0]) * 1000.0
dz_mm = (blk_xz[1] - box_xz[1]) * 1000.0
dist_mm = math.hypot(dx_mm, dz_mm)
# 輪郭中心 vs テンプレ中心 の Δ (mm 単位・(C - T))
delta_vs_template_mm = None
if box_xz is not None and box_c_xz is not None:
delta_vs_template_mm = (
(box_c_xz[0] - box_xz[0]) * 1000.0,
(box_c_xz[1] - box_xz[1]) * 1000.0,
)
# ---- release offset (box が取れているセルのみ) ----
cell_key = None
offset_mm = None
if box_xz is not None:
cell_key = _corr_key_place(box_xz[0], box_xz[1])
offset_mm = _read_release_offset_mm(cell_key)
# ---- 描画 ----
# 従来テンプレ検出 (シアン系・青十字扱い). 新旧比較のため残す.
if last_box_result:
_draw_crosshair(frame, last_box_result["cx"],
last_box_result["cy"],
(255, 220, 100), size=16, thick=2)
cv2.putText(frame,
"BOX-T ({:+.3f},{:+.3f})".format(
last_box_result["xz_m"][0],
last_box_result["xz_m"][1]),
(last_box_result["cx"] + 20,
last_box_result["cy"] - 12),
FONT, 0.5, (255, 220, 100), 1, cv2.LINE_AA)
# 輪郭検出 (黄) の 4 頂点ポリラインと中心黄十字.
if box_contour_result:
pts = np.array(box_contour_result["rect_pts"],
dtype=np.int32).reshape(-1, 1, 2)
cv2.polylines(frame, [pts], isClosed=True,
color=(0, 255, 255), thickness=2,
lineType=cv2.LINE_AA)
_draw_crosshair(frame, box_contour_result["cx"],
box_contour_result["cy"],
(0, 255, 255), size=18, thick=2)
cv2.putText(frame,
"BOX-C ({:+.3f},{:+.3f})".format(
box_contour_result["xz_m"][0],
box_contour_result["xz_m"][1]),
(box_contour_result["cx"] + 22,
box_contour_result["cy"] + 24),
FONT, 0.5, (0, 255, 255), 1, cv2.LINE_AA)
if block_result:
_draw_crosshair(frame, block_result["cx"],
block_result["cy"],
(100, 255, 100), size=14, thick=2)
cv2.putText(frame,
"BLOCK ({:+.3f},{:+.3f})".format(
block_result["xz_m"][0],
block_result["xz_m"][1]),
(block_result["cx"] + 18,
block_result["cy"] + 22),
FONT, 0.5, (100, 255, 100), 1, cv2.LINE_AA)
if last_box_result and block_result:
cv2.line(frame,
(last_box_result["cx"], last_box_result["cy"]),
(block_result["cx"], block_result["cy"]),
(200, 200, 255), 1, cv2.LINE_AA)
# save メッセージの寿命
if save_msg and now > save_msg_expire:
save_msg = ""
_draw_hud(frame, box_xz, blk_xz, dx_mm, dz_mm, dist_mm,
cell_key, offset_mm, fps_ewma, save_msg,
box_contour_xz=box_c_xz,
delta_vs_template_mm=delta_vs_template_mm)
cv2.imshow(WINDOW_NAME, frame)
k = cv2.waitKey(1) & 0xFF
if k == ord("q"):
break
elif k == ord("s"):
ok_s, msg = _save_frame(frame)
save_msg = ("saved: " + msg) if ok_s else ("skip: " + msg)
save_msg_expire = now + 3.0
print("[PREVIEW] " + save_msg)
except KeyboardInterrupt:
print("[PREVIEW] Ctrl-C 検出")
finally:
try:
cap.release()
except Exception:
pass
cv2.destroyAllWindows()
return 0
if __name__ == "__main__":
sys.exit(main())diag_place_z_move.py（全ファイル・新規・Z 単発診断）
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""diag_place_z_move.py — PLACE Z 動作の単発診断（本線から完全独立）
目的:
「アームを PLACE 姿勢に置いて、上カメラで保持ブロックの Z を実測 →
箱中心との Δz を出す → 狙い z をそのぶん一発で動かして再降下 → もう一度実測」
の 1 サイクルを、サーボの刻み・反復・学習・observe・emit を全部排除して裸で回す。
期待: 狙い z を -51mm 動かしたら block_z が -50mm 前後動く。
失敗: 動かない場合 (block_z 変化 <10mm) は「サーボの刻み以前に Z が動かない壁」。
流用（コード複製なし・import のみ）:
- run_pick_place: _check_guard / _build_place_lookup_kwargs /
_detect_block_from_frame / _get_white_box_detector /
定数 (BOX_XZ, PLACE_EE_Y_*, J4_SAFE_*, TOL_PICK_PLACE, SPEED,
SEND_WAIT_SEC, X_RESID_THRESH, PLACE_JOINT_TOL_DEG,
PLACE_EE_Y_FLOOR, PLACE_J1_MIN_DEG)
- fk_lookup: fk_lookup
- visual_servo_pick: daemon_send_angles / daemon_get_angles /
setup_homography_and_mat_range / pixel_to_unity /
detect_red_block
- camera_resolver: resolve_cameras
- 06f_top_white_box_detect_v7: _get_white_box_detector 経由で lazy 取得
絶対禁止:
- release_all_servos() / send_coords() は呼ばない
- グリッパー操作しない（掴んだまま位置だけ動かす）
- 本線 (run_pick_place / observe / emit) の状態を書き換えない
- J4_SAFE ガードを緩めない（_check_guard を本線同一で通す）
- ループしない（測定 A → 一発送角 → 測定 B の 1 サイクルのみ）
CLI:
python3 diag_place_z_move.py \
--target-x -0.180 --target-z 0.200 \
[--start-pose "j0,j1,j2,j3,j4,j5"] \
[--target-ee-y 0.100] [--place-j0-ref 36.22] \
[--repeat 3] [--dry-run] [--auto-confirm]
出力の要点:
狙い z 変化 = -XX.X mm vs block_z 変化 = -YY.Y mm
(XX≈YY なら「動いた」・|差|<12mm なら実質収束・|差|>25mm なら「動かない壁」)
"""
from __future__ import print_function
import argparse
import sys
import time
from pathlib import Path
# ---- パス解決（run_pick_place と同じ流儀） ---------------------------------
_THIS = Path(__file__).resolve()
_PLACE_MODULES_DIR = _THIS.parent
_SCRIPTS_ROOT = _THIS.parents[3]
sys.path.insert(0, str(_SCRIPTS_ROOT))
sys.path.insert(0, str(_PLACE_MODULES_DIR))
# ---- 本線からの流用 --------------------------------------------------------
import cv2 # noqa: E402
import numpy as np # noqa: E402
from fk_lookup import fk_lookup # noqa: E402
from camera_resolver import resolve_cameras # noqa: E402
from visual_servo_pick import ( # noqa: E402
daemon_send_angles,
daemon_get_angles,
setup_homography_and_mat_range,
)
# run_pick_place: 送信前ガード / lookup 引数 / 検出ヘルパ / 定数を **一切書き換えず** 借りる
import run_pick_place as rpp # noqa: E402
# ---- ヘルパ ---------------------------------------------------------------
def _median_pair(pairs):
"""[(x, z), ...] の各軸独立中央値. None を含むエントリはスキップ."""
xs = [p[0] for p in pairs if p is not None and p[0] is not None]
zs = [p[1] for p in pairs if p is not None and p[1] is not None]
if not xs or not zs:
return None
return (float(np.median(xs)), float(np.median(zs)))
def _detect_box_from_frame(frame):
"""1 BGR フレームから 06f の輪郭ベース検出で白箱中心 → Unity 座標.
preview_place_delta と同じ `detect_box_center_by_contour(frame_bgr, mat_bbox)` を
BGR そのまま (cvtColor しない) で叩く. gray に変換すると 06f 側の
`cv2.cvtColor(roi, COLOR_BGR2GRAY)` が 2ch 入力で例外を投げる.
失敗は None (制御を止めない).
"""
if frame is None:
return None
try:
m06f, _templates = rpp._get_white_box_detector()
if m06f is None:
return None
import visual_servo_pick as vsp
if vsp._mat_px_range is None:
return None
_bc_fn = getattr(m06f, "detect_box_center_by_contour", None)
if _bc_fn is None:
return None
contour_r = _bc_fn(frame, vsp._mat_px_range) # BGR 直渡し
if contour_r is None:
return None
cx, cy = contour_r["cx"], contour_r["cy"]
ux, uz = vsp.pixel_to_unity(cx, cy)
return [float(ux), float(uz)]
except Exception as e:
print(" [WARN] box 検出例外: {}".format(e))
return None
def _measure_once(cap, tag, repeat):
"""--repeat 回だけキャプチャして block_xz と box_xz を中央値集約.
戻り値: dict {block_xz_m, box_xz_m, block_samples, box_samples}
"""
block_samples = []
box_samples = []
for i in range(max(1, repeat)):
ret, frame = cap.read()
if not ret:
print(" [{}][{}/{}] cap.read 失敗".format(tag, i + 1, repeat))
continue
blk = rpp._detect_block_from_frame(frame)
box = _detect_box_from_frame(frame)
block_samples.append(blk)
box_samples.append(box)
blk_s = ("({:+.4f},{:+.4f})".format(blk[0], blk[1]) if blk else "None")
box_s = ("({:+.4f},{:+.4f})".format(box[0], box[1]) if box else "None")
print(" [{}][{}/{}] block={} box={}".format(tag, i + 1, repeat, blk_s, box_s))
if i < repeat - 1:
time.sleep(0.15)
return {
"block_xz_m": _median_pair(block_samples),
"box_xz_m": _median_pair(box_samples),
"block_samples": block_samples,
"box_samples": box_samples,
}
def _confirm(prompt, auto_confirm):
if auto_confirm:
print(" [AUTO] {} (--auto-confirm)".format(prompt))
return True
try:
ans = input(" {} [y/N]: ".format(prompt)).strip().lower()
except (EOFError, KeyboardInterrupt):
return False
return ans in ("y", "yes")
def _print_angles(label, angles):
if angles is None:
print(" {}: None".format(label))
return
def _fmt(a):
try:
return "{:+8.3f}".format(float(a))
except (TypeError, ValueError):
return " None "
print(" {}: [{}]".format(label, ", ".join(_fmt(a) for a in angles)))
def _send_pose(angles, label, auto_confirm, dry_run):
"""1 姿勢送信. 送信前に 6 軸を print し, 対話確認 or --auto-confirm."""
print("[SEND ] {} (SPEED={}, SEND_WAIT_SEC={})".format(label, rpp.SPEED, rpp.SEND_WAIT_SEC))
_print_angles("planned 6-axis", angles)
if dry_run:
print(" [DRY-RUN] 送角スキップ")
return None
if not _confirm("この 6 軸を送りますか?", auto_confirm):
print(" [ABORT] ユーザー確認 NO・送角スキップ")
return None
daemon_send_angles(angles, speed=rpp.SPEED, wait=rpp.SEND_WAIT_SEC)
time.sleep(0.2) # 応答直後の get_angles ノイズ回避（実験用）
post = daemon_get_angles()
_print_angles("post_actual", post)
return post
# ---- メイン ---------------------------------------------------------------
def main():
ap = argparse.ArgumentParser(description=__doc__.split("\n")[0],
formatter_class=argparse.RawDescriptionHelpFormatter)
ap.add_argument("--target-x", type=float, required=True,
help="現在の狙い x [m] (箱中心近傍・BOX_XZ 相当)")
ap.add_argument("--target-z", type=float, required=True,
help="現在の狙い z [m] (Z サーボ開始前と同じ値)")
ap.add_argument("--target-ee-y", type=float, default=0.100,
help="fk_lookup の target_ee_y_m (default 0.100)")
ap.add_argument("--start-pose", type=str, default=None,
help="開始姿勢 6 軸 (カンマ区切り). 省略時は現在姿勢に触らない")
ap.add_argument("--place-j0-ref", type=float, default=None,
help="fk_lookup の j0_ref [deg]. 省略時は測定 j0 を使う")
ap.add_argument("--repeat", type=int, default=3,
help="測定 A/B での median 用フレーム数 (default 3)")
ap.add_argument("--dry-run", action="store_true",
help="送角しない (lookup + guard 判定 + 予測姿勢の print のみ)")
ap.add_argument("--auto-confirm", action="store_true",
help="送角前の対話確認を全て y として進む")
args = ap.parse_args()
print("=" * 78)
print("diag_place_z_move — PLACE Z 単発診断")
print("=" * 78)
print("target_x_m = {:+.4f}".format(args.target_x))
print("target_z_m = {:+.4f}".format(args.target_z))
print("target_ee_y_m = {:+.4f}".format(args.target_ee_y))
print("place_j0_ref = {}".format(args.place_j0_ref))
print("start_pose = {}".format(args.start_pose))
print("repeat = {}".format(args.repeat))
print("dry_run = {}".format(args.dry_run))
print("auto_confirm = {}".format(args.auto_confirm))
print("SPEED / SEND_WAIT= {} / {}".format(rpp.SPEED, rpp.SEND_WAIT_SEC))
print("J4_SAFE = [{}, {}]".format(rpp.J4_SAFE_MIN, rpp.J4_SAFE_MAX))
print("PLACE_EE_Y_MIN/MAX/FLOOR = {}, {}, {}".format(
rpp.PLACE_EE_Y_MIN, rpp.PLACE_EE_Y_MAX, rpp.PLACE_EE_Y_FLOOR))
print("X_RESID_THRESH = {}".format(rpp.X_RESID_THRESH))
print("TOL_PICK_PLACE = {}".format(rpp.TOL_PICK_PLACE))
print("=" * 78)
# ---- 0. 現在姿勢を取得 -------------------------------------------------
print("[STEP 0] 現在姿勢を取得")
cur_angles = daemon_get_angles()
_print_angles("current 6-axis", cur_angles)
if cur_angles is None or len(cur_angles) != 6:
print("[ERR] daemon_get_angles 失敗. 中断.")
sys.exit(1)
# ---- 1. --start-pose が来ていれば移動 ---------------------------------
# ref_pose_for_lookup: STEP 5 の fk_lookup ref_joints に渡す基準.
# --start-pose 指定時 = その CLI 値 (実測 post ではなく指令値)
# 未指定時 = STEP 0 で取った現在姿勢
if args.start_pose is not None:
try:
parts = [float(x) for x in args.start_pose.split(",")]
except ValueError:
print("[ERR] --start-pose のパース失敗: {!r}".format(args.start_pose))
sys.exit(1)
if len(parts) != 6:
print("[ERR] --start-pose は 6 個必要 (got {}): {!r}".format(len(parts), args.start_pose))
sys.exit(1)
print("[STEP 1] --start-pose に移動")
post = _send_pose(parts, "start_pose", args.auto_confirm, args.dry_run)
if post is not None:
cur_angles = post
ref_pose_for_lookup = list(parts)
ref_pose_source = "--start-pose"
else:
print("[STEP 1] --start-pose なし → 現在姿勢をそのまま基準として使う")
ref_pose_for_lookup = list(cur_angles)
ref_pose_source = "current 6-axis (STEP 0 実測)"
# ---- 2. カメラセットアップ ---------------------------------------------
print("[STEP 2] カメラ setup (top)")
cams = resolve_cameras(require_top=True, require_side=False, verbose=True)
top_idx = cams.get("top") if cams else None
if top_idx is None:
print("[ERR] top カメラ未解決. 中断.")
sys.exit(1)
print(" resolved top index = {}".format(top_idx))
cap = cv2.VideoCapture(top_idx)
if not cap.isOpened():
print("[ERR] cv2.VideoCapture open 失敗 (index={}). 中断.".format(top_idx))
sys.exit(1)
try:
for _ in range(30):
cap.read() # warm-up
if not setup_homography_and_mat_range(cap):
print("[ERR] setup_homography_and_mat_range 失敗. 中断.")
sys.exit(1)
# ---- 3. 測定 A ------------------------------------------------------
print("[STEP 3] 測定 A (送角前)")
meas_a = _measure_once(cap, "A", args.repeat)
if meas_a["block_xz_m"] is None or meas_a["box_xz_m"] is None:
print("[ERR] 測定 A で block or box を取れず. 中断.")
sys.exit(1)
block_x_a, block_z_a = meas_a["block_xz_m"]
box_x_a, box_z_a = meas_a["box_xz_m"]
dx_a_mm = (box_x_a - block_x_a) * 1000.0
dz_a_mm = (box_z_a - block_z_a) * 1000.0
print(" block_A = ({:+.4f}, {:+.4f}) m".format(block_x_a, block_z_a))
print(" box_A = ({:+.4f}, {:+.4f}) m".format(box_x_a, box_z_a))
print(" Δx_A = {:+.2f} mm".format(dx_a_mm))
print(" Δz_A = {:+.2f} mm (期待: block が箱より +Z 側 = 負値)".format(dz_a_mm))
# ---- 4. 新狙い z を一発で計算 (クランプなし) ------------------------
# 本線 Z サーボと同じ符号: diff_z_mm = desired - block; step_mm = diff_z_mm;
# new_target_z_m = current_target_z_m + step_mm/1000
step_mm = dz_a_mm
new_target_z_m = args.target_z + step_mm / 1000.0
aim_delta_mm = (new_target_z_m - args.target_z) * 1000.0
print("[STEP 4] 新狙い z 計算 (クランプなし・一発)")
print(" current_target_z_m = {:+.4f}".format(args.target_z))
print(" step_mm = {:+.2f} mm (= Δz_A)".format(step_mm))
print(" new_target_z_m = {:+.4f}".format(new_target_z_m))
print(" aim_delta_mm = {:+.2f} mm".format(aim_delta_mm))
# ---- 5. fk_lookup ---------------------------------------------------
print("[STEP 5] fk_lookup(place_withblock, ...) 呼び出し")
j0_ref = args.place_j0_ref if args.place_j0_ref is not None else float(cur_angles[0])
lookup_kwargs = rpp._build_place_lookup_kwargs(
ref_joints=list(ref_pose_for_lookup),
target_ee_y_m=args.target_ee_y,
place_j0_ref=j0_ref,
)
print(" ref_joints_src = {}".format(ref_pose_source))
_print_angles("ref_joints", ref_pose_for_lookup)
print(" target_ee_y_m = {:+.4f}".format(args.target_ee_y))
print(" place_j0_ref = {:+.3f} deg".format(j0_ref))
try:
r = fk_lookup(
"place_withblock",
(float(args.target_x), float(new_target_z_m)),
**lookup_kwargs,
)
except (ValueError, FileNotFoundError, RuntimeError) as e:
print("[ERR] fk_lookup 例外: {}: {}".format(type(e).__name__, e))
sys.exit(1)
_method_str = str(r.get("method", ""))
print(" method = {}".format(_method_str))
print(" x_residual_m = {}".format(r.get("x_residual_m")))
print(" z_residual_m = {}".format(r.get("z_residual_m")))
# out_of_range の場合 target_angles は None or [None, ...] で来る.
# 送角には進まない (STEP 6 の GUARD-STOP で止まる) ので, ここで
# print しても情報にならず, float(None) で例外を出させない.
if _method_str.endswith("out_of_range"):
print(" lookup target_angles: (skip: method=out_of_range・送角しない)")
else:
_print_angles("lookup target_angles", r.get("target_angles"))
# ---- 6. 送信前ガード (本線同一・緩めない) -------------------------
print("[STEP 6] _check_guard (本線同一)")
ok_guard, reason = rpp._check_guard(r, "diag_place_z_move_step6")
method_str = str(r.get("method", ""))
if (not ok_guard) or method_str.endswith("out_of_range"):
print(" [GUARD-STOP] {} / method={}".format(reason, method_str))
print(" → 送角せず終了 (安全側). run は壊れていない.")
sys.exit(0)
print(" [GUARD-PASS]")
# ---- 7. 再降下 (一発送角・ループなし) ------------------------------
print("[STEP 7] 再降下 (一発)")
planned = list(r["target_angles"])
post = _send_pose(planned, "diag_relookup", args.auto_confirm, args.dry_run)
if args.dry_run:
print(" --dry-run のため測定 B はスキップして終了.")
sys.exit(0)
if post is None:
print(" 送角スキップされた (ユーザー NO 等). 測定 B はしない.")
sys.exit(0)
# ---- 8. 測定 B ------------------------------------------------------
print("[STEP 8] 測定 B (送角後)")
# 動作収まり待ち
time.sleep(0.5)
# warm-up: 送角直後は像が動いているので数フレーム捨てる
for _ in range(10):
cap.read()
meas_b = _measure_once(cap, "B", args.repeat)
if meas_b["block_xz_m"] is None:
print("[ERR] 測定 B で block を取れず (box は任意). 中断.")
sys.exit(1)
block_x_b, block_z_b = meas_b["block_xz_m"]
if meas_b["box_xz_m"] is not None:
box_x_b, box_z_b = meas_b["box_xz_m"]
dx_b_mm = (box_x_b - block_x_b) * 1000.0
dz_b_mm = (box_z_b - block_z_b) * 1000.0
print(" block_B = ({:+.4f}, {:+.4f}) m".format(block_x_b, block_z_b))
print(" box_B = ({:+.4f}, {:+.4f}) m".format(box_x_b, box_z_b))
print(" Δx_B = {:+.2f} mm".format(dx_b_mm))
print(" Δz_B = {:+.2f} mm".format(dz_b_mm))
else:
box_x_b = box_z_b = None
dx_b_mm = dz_b_mm = None
print(" block_B = ({:+.4f}, {:+.4f}) m".format(block_x_b, block_z_b))
print(" box_B = 未取得 (Δx_B/Δz_B skip)")
# ---- 9. サマリ ------------------------------------------------------
block_z_change_mm = (block_z_b - block_z_a) * 1000.0
block_x_change_mm = (block_x_b - block_x_a) * 1000.0
print("=" * 78)
print("SUMMARY")
print("=" * 78)
print(" aim_z 変化 = {:+.2f} mm (狙いを動かした量)".format(aim_delta_mm))
print(" block_z 変化 (実測) = {:+.2f} mm (ブロックが実際に動いた量)".format(block_z_change_mm))
print(" block_x 変化 (参考) = {:+.2f} mm".format(block_x_change_mm))
if dz_b_mm is not None:
print(" Δz_A → Δz_B = {:+.2f} → {:+.2f} mm".format(dz_a_mm, dz_b_mm))
# 判定ヒント
diff_abs = abs(block_z_change_mm - aim_delta_mm)
if diff_abs < 15.0:
verdict = "動いた (aim と block 変化が近い)"
elif abs(block_z_change_mm) < 10.0 and abs(aim_delta_mm) > 30.0:
verdict = "動かない壁 (狙い大きく動かしても block ほぼ不動)"
else:
verdict = "部分的に動いた (aim vs block 変化 |差|={:.1f}mm)".format(diff_abs)
print(" verdict = {}".format(verdict))
print("=" * 78)
finally:
try:
cap.release()
except Exception:
pass
if __name__ == "__main__":
main()
