mechArm 270 Pi 動的追従の限界を探る Log#28 | 「できた」が場所を変えると崩れる。
デザイナーがロボットを触ると、見えてくるものがある。LOG224で見えたのはこれ。
拡充したテーブルで、接近が通った
前回、掴んだ赤ブロックを白箱の上で離す動作（PLACE）の途中で詰まっていた。落下点が白箱の中心からずれるのを打ち消す補正を入れたところ、降下が止まった。原因は補正でも符号でもなく、その狙いへ降りる姿勢が姿勢テーブル（FK テーブル）に無いという、別の問題だった。テーブルを生成元の Unity まで遡って作り直し、512,087 点から 710,704 点へ拡充した。検算では狙い 3 点すべてが通り、回帰の悪化もなかった。
検算が通ることと、実機が降りられるかは別だ。拡充したテーブルで実際に接近・降下できるかを確かめた。
白箱を左奥（box ≒ -0.177, +0.218）に固定し、赤ブロックを爪に挟んで接近を実行した。実機ログを全量載せる。
[INIT] 上カメラ index=2 横カメラ index=1
[INIT] daemon 接続中...
[Pi] fresh_mode=1
[INIT] fresh_mode=1
[PRE] 現在姿勢: [0.17, 1.23, 0.96, 0.00, 0.17, -29.70]
[PRE] HOME ズレ max_err=1.23° (許容 5.0°)
[BOX] pixel 中心=(1338, 716) score=0.636 angle=0°
[BOX] Unity 座標: (unity_x=-0.177, unity_z=+0.218) [単位 m]
[BOX] 補正前 box xz = (-0.177, +0.218) m
[APPROACH] 把持補正後の狙い xz = (-0.152, +0.193) m (offset = +0.025, -0.025)
[LOOKUP] 目標 (ee_x=-0.152, ee_y=+0.120, ee_z=+0.193)
[LOOKUP] CSV スキャン中: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based_v2.csv
[LOOKUP] 完了: 710704 点 カラム不一致 0 件 到達不可で除外 487739 件
[LOOKUP] 最近傍: ee=(-0.158, +0.126, +0.217) 3D距離=25.9mm
[LOOKUP] 最近傍 関節角度: [13.00, 72.00, -5.00, -42.00, -96.00, -30.00]
[LOOKUP] しきい値判定: OK (25.9mm <= 100mm)
[STAGE1] 到達
[STAGE2] 到達接近は中断せず、白箱上空の APPROACH 姿勢（STAGE2）まで到達した。前回、同じ左奥で詰まっていたのと同じ座標だ。前回はこの狙いへ降りる姿勢がテーブルに無くて選べなかった。それが今回は 710,704 点から 3D 距離 25.9mm で引け、関節制限も通って、実機も到達した。テーブルの穴は埋まったと、実機で確認できた。
降下は、1mm 足りずに止まっていた
接近の次は降下（DESCEND）だ。白箱の上空 120mm から、離す高さ 110mm へ降ろす。ここで、前回まで繰り返し止まっていた現象が、また出た。
[DESCEND[0] ee_y=0.110m] target_xyz = (-0.152, +0.110, +0.193)
[DESCEND[0] ee_y=0.110m] LOOKUP 710704 点 カラム不一致 0 件 到達不可で除外 487739 件
[DESCEND[0] ee_y=0.110m] 最近傍 dist=23.9mm ee_y_n=+0.104m joints=[13.00, 77.00, -5.00, -42.00, -96.00, -30.00]
[DESCEND[0] ee_y=0.110m] [ABORT] 最近傍の高さが低すぎます: ee_y_n=+0.104 < +0.105 (目標 +0.110 の下振れ 5mm 超)目標 110mm に対して、テーブルから引けた一番近い姿勢の高さが 104mm。下振れ 6mm。安全装置は「目標より 5mm を超えて下に外れる姿勢は送らない」と設定してあったので、6mm の下振れが 1mm だけ閾値を超えて、送る前に止まった。
止まり方が前回までと違うことに気づいた。以前あった破綻 ── 距離だけで一番近い点を引いた結果、肘や手首が大きく捻れた、まったく別の腕の形が選ばれて、それが低すぎて止まる ── ではない。引けた姿勢 [13, 77, -5, -42, -96, -30] は、接近姿勢 [13, 72, -5, -42, -96, -30] から j1 が 5°動いただけの、素直な降下だ。姿勢の質は正常で、高さがちょうど 1mm 足りないだけだった。
この同じ 104mm が、白箱を動かさずに繰り返した 4 回すべてで、寸分違わず引かれた。
ee_y_n=+0.104m dist≈23.9-24.1mm joints=[13, 77, -5, -42, -96, -30]4 回とも同じ点。これはばらつきではなく、このテーブルがこの白箱位置に対して持っている「最も狙いに近い降下姿勢」が 104mm で固定されているということだ。試行を重ねて偶然 110mm 近辺を引き当てる、という当てにいく手は消えた。104mm 以外の選択肢が、そもそもテーブルに無い。
その一方で、4 回とも、降下を中断したあと「だいたいの位置で離す」工程へ進めば、ブロックは白箱に入った。落下点を見ると、左奥のこの 1 点では狙いの中心近くに落ちている。
白箱(-0.177, +0.218) / 赤ブロック(-0.174, +0.242) ΔX=+3mm ΔZ=+24mm → y
白箱(-0.177, +0.218) / 赤ブロック(-0.178, +0.246) ΔX=-1mm ΔZ=+28mm → y
白箱(-0.176, +0.218) / 赤ブロック(-0.185, +0.253) ΔX=-9mm ΔZ=+35mm → y
白箱(-0.176, +0.218) / 赤ブロック(-0.179, +0.247) ΔX=-3mm ΔZ=+29mm → y横（ΔX）は -9 〜 +3mm でほぼ中心。前回、補正前は -50mm だったのが、半量の補正でぴたりと寄った。奥行き（ΔZ）は +24 〜 +35mm 残るが、80mm 角の白箱には収まる。降下は ABORT しているが、離す工程が拾って 4 回とも入った。
安全閾値を 1mm 緩めて、降下が最後まで通った
104mm が固定で、姿勢の質も正常なら、安全閾値を 1mm だけ広げて、この 104mm を送れるようにすればいい。降下の高さ下振れ許容 EE_Y_MIN_TOL を 5mm から 6mm へ変える。変更はこの 1 箇所だけだ。
EE_Y_MIN_TOL = 0.006 # 降下用。最近傍が目標より下に外れてよい上限（m）= 6mm。
# 110mm 化の安全意図に合わせ 15→5mm に縮小（codex R2）。
# DAY224: 左奥領域でテーブル最近傍が固定的に 104mm（目標110に対し
# 下振れ6mm）で、姿勢自体は素直な降下と4回再現で確認。5→6mm に
# 1mm だけ広げ、この 104mm 姿勢を送って descend を通す。
# 104mm は旧 release 接触高さ(100mm)の +4mm 直近のため、初回は目視で
# 爪・把持ブロックと白箱フチのクリアランスを確認すること。
# 目標110mmに対し 104mm 未満の FK 姿勢は引き続き送らず ABORT。判定式は既存のまま if ee_y_n < ee_y_stage - EE_Y_MIN_TOL: で、110mm 段に対する閾値は 0.110 - 0.006 = 0.104。104mm ちょうどは通し、103mm 以下は止める。
この変更は降下に直結するので、外部レビュー（codex）に静的レビューを依頼した。レビューは、テーブルの真値を CSV から引いて確認していた。
提示座標でCSV確認した最近傍は
ee_y=0.104469mで、下限0.104000mより約0.47mm上でした。
104mm 表示の真値は 104.47mm で、閾値の 104.0mm より 0.47mm 上にいた。境界ぴったりではなく、わずかに通る側に余裕がある。同時に、物理接触リスクについての注意も返ってきた。
104.469mmは旧接触100mmから約4.5mmしか余裕がなく、同姿勢近傍のCSVでは j1 5°差で
ee_yが約21mm動くため、1°程度の到達ズレでも余裕を食います。初回目視前提なら許容範囲ですが、「FK高さチェックだけで安全」とは言い切らない方がいいです。
テーブル上は 104.47mm で安全でも、実機が送信角ぴったりに止まる保証はなく、到達誤差 1°が高さ 4mm に化ける敏感な領域だという指摘だ。紙の上の 4.5mm 余裕は、実機では数 mm まで縮みうる。FK の高さチェックだけでは担保しきれない。だから初回は目視で見ることを条件に、実機へ進めた。
降下の本体（run_descend）の全文を載せる。安全ガード ── テーブルに姿勢があるか、3D 距離が離れすぎていないか、高さが低すぎないか、関節制限内か ── はすべて送信の手前に置いてある。
def descend_step(target_angles, label, proceed_on_timeout=False):
"""1段の降下送信。
手順:
1) send_angles(target, speed=SPEED, wait=0) を1回送る
2) Mac 側で sleep + get_angles で到達確認（REACH_TIMEOUT_SEC まで）
3) 未到達なら同一 target を「1回だけ」再送（14 の 3 回再送は持ち込まない）
4) 再送後も未到達なら追加送信せず未到達で返す（張り付き・押し付け回避）
proceed_on_timeout=True（HOLD 廃止経路）の場合：
2) の待機後、tight reach 未達でも 3)4) を行わず settle 姿勢で
reach_ok=True を返す。再送・ABORT しない（17 の RELEASE 経路用）。
JOINT_LIMITS チェックと最初の send + 待機は通常どおり実施する。
戻り値: (reach_ok: bool, resent: bool, last_measured: list or None)
"""
violations = check_joint_limits(target_angles)
if violations:
for i, v, lo, hi in violations:
print(f"[{label}] [LIMITS] NG: j{i}={v:.2f} 範囲 [{lo}, {hi}] 外")
return False, False, None
print(f"[{label}] send_angles {fmt(target_angles)}")
daemon_send_angles(target_angles, speed=SPEED, wait=0)
ok, err, measured = _wait_for_reach(target_angles, label)
if ok:
return True, False, measured
if proceed_on_timeout:
err_str = f"{err:.2f}" if err is not None else "None"
print(
f"[{label}] tight reach 未達 max_err={err_str}°だが "
"proceed_on_timeout=True のため settle 姿勢で次へ進む（再送しない）"
)
return True, False, measured
err_str = f"{err:.2f}" if err is not None else "None"
print(f"[{label}] 未到達 max_err={err_str}°。1回だけ再送する")
daemon_send_angles(target_angles, speed=SPEED, wait=0)
ok2, err2, measured2 = _wait_for_reach(target_angles, label)
if ok2:
return True, True, measured2
err2_str = f"{err2:.2f}" if err2 is not None else "None"
print(f"[{label}] 再送後も未到達 max_err={err2_str}°。これ以上送信しない")
return False, True, measured2 if measured2 is not None else measured
def _make_log_path():
LOG_DIR.mkdir(parents=True, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d_%H%M%S")
return LOG_DIR / f"descend_log_{ts}.csv"
def run_descend(X_app, Z_app, approach_measured, proceed_on_timeout=False):
"""X_app, Z_app, approach_measured を引数で受ける降下ランナー。
引数:
X_app, Z_app: float (m)。APPROACH 探索に使った Unity 座標 X・Z。
降下中は固定したまま ee_y のみを下げる。
approach_measured: list[float] 6 要素。APPROACH 到達時の実測関節角（度）。
proceed_on_timeout: bool。デフォルト False＝従来挙動（tight reach 未達なら
1回再送→なお未達なら ABORT）。True の場合、送信後 settle
したら tight reach 未達でも「降下完了」として次へ進む
（HOLD 廃止経路用。17 RELEASE モードから呼ばれる）。
降下前の安全ガード（FK 最近傍 / 3D 距離 / EE_Y_MIN_TOL /
JOINT_LIMITS）は値に関わらず常に有効。
戻り値: bool（True=最終段到達 / False=中断）
前提:
daemon 接続・fresh_mode=1 設定は caller 側で済ませてあること。
本関数は daemon 接続・切断・fresh_mode を一切触らない。
"""
# 降下の狙い点を把持オフセット分だけ逆向きに補正する。
# 補正は FK 最近傍探索に渡る狙い xz のみ。記録（place_trial_log）には
# 補正前の生 box（X_app, Z_app）を渡し、CSV の box は実際の白箱位置に保つ。
X_desc = X_app + PLACE_X_OFFSET_M
Z_desc = Z_app + PLACE_Z_OFFSET_M
print("[INIT] DESCEND 起点")
print(f"[INIT] approach_measured = {fmt(approach_measured)}")
print(
f"[INIT] 補正前 box xz = ({X_app:+.3f}, {Z_app:+.3f}) m "
f"ee_y={EE_Y_APPROACH:+.3f}m (APPROACH)"
)
print(
f"[DESCEND] 把持補正後の狙い xz = ({X_desc:+.3f}, {Z_desc:+.3f}) m "
f"(offset = {PLACE_X_OFFSET_M:+.3f}, {PLACE_Z_OFFSET_M:+.3f})"
)
print(f"[INIT] 降下段 ee_y = {EE_Y_STAGES} m")
log_path = _make_log_path()
print(f"[LOG] {log_path}")
aborted = False
with open(log_path, "w", newline="") as f:
writer = csv.writer(f)
writer.writerow([
"stage", "ee_y_target", "target_xyz",
"nearest_dist_mm", "sent_angles", "measured",
"resent", "reach_ok", "aborted",
])
f.flush()
for stage_idx, ee_y_stage in enumerate(EE_Y_STAGES):
label = f"DESCEND[{stage_idx}] ee_y={ee_y_stage:.3f}m"
place_preview.update(phase=f"DESCEND {ee_y_stage*1000:.0f}mm")
target_xyz = (X_desc, ee_y_stage, Z_desc)
print(
f"\n[{label}] target_xyz = "
f"({X_desc:+.3f}, {ee_y_stage:+.3f}, {Z_desc:+.3f})"
)
best_row, best_dist_sq, data_rows, skipped, skipped_unreach = scan_csv_for_nearest(
CSV_PATH, X_desc, ee_y_stage, Z_desc
)
print(
f"[{label}] LOOKUP {data_rows} 点 "
f"カラム不一致 {skipped} 件 到達不可で除外 {skipped_unreach} 件"
)
if best_row is None:
print(
f"[{label}] [ABORT] 到達可能な FK 姿勢が見つかりません"
"（JOINT_LIMITS 内の有効姿勢が CSV に1件もありません）"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
"", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason="FK姿勢なし",
)
aborted = True
break
dist_mm = math.sqrt(best_dist_sq) * 1000.0
ee_y_n = best_row[1]
joints = list(best_row[3:9])
print(
f"[{label}] 最近傍 dist={dist_mm:.1f}mm ee_y_n={ee_y_n:+.3f}m "
f"joints={fmt(joints)}"
)
if dist_mm > DIST_THRESHOLD_MM:
print(
f"[{label}] [ABORT] 最近傍 3D 距離 {dist_mm:.1f}mm > "
f"{DIST_THRESHOLD_MM:.0f}mm。妥当な姿勢が見つからないため降下を続けません"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason=f"3D距離超過 {dist_mm:.1f}mm>{DIST_THRESHOLD_MM:.0f}mm",
)
aborted = True
break
if ee_y_n < ee_y_stage - EE_Y_MIN_TOL:
print(
f"[{label}] [ABORT] 最近傍の高さが低すぎます: "
f"ee_y_n={ee_y_n:+.3f} < {ee_y_stage - EE_Y_MIN_TOL:+.3f} "
f"(目標 {ee_y_stage:+.3f} の下振れ {EE_Y_MIN_TOL*1000:.0f}mm 超)"
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", "", "", False, False, True,
])
f.flush()
place_trial_log.log_trial(
"descend_abort", X_app, Z_app,
stage="descend",
abort_reason=f"高さ下振れ ee_y_n={ee_y_n:+.3f}<{ee_y_stage - EE_Y_MIN_TOL:+.3f}",
)
aborted = True
break
sent_angles = list(joints)
sent_angles[5] = J5_FIX
print(f"[{label}] j5 を {J5_FIX} に固定 → sent_angles={fmt(sent_angles)}")
reach_ok, resent, measured = descend_step(
sent_angles, label, proceed_on_timeout=proceed_on_timeout
)
writer.writerow([
stage_idx, ee_y_stage, list(target_xyz),
f"{dist_mm:.2f}", fmt(sent_angles), fmt(measured),
resent, reach_ok, not reach_ok,
])
f.flush()
if not reach_ok:
print(
f"[{label}] [ABORT] 未到達のため降下中断。"
"追加送信なしで停止します"
)
aborted = True
break
print(f"[{label}] 到達")
if aborted:
print(
"\n[ABORT] 降下中断。"
"グリッパーは閉じたまま、追加送信なしで終了。"
"HOME 復帰は実行しません。エイイチの目視判断に渡します"
)
return False
print(
f"\n[HOLD] 最終段 ee_y={EE_Y_STAGES[-1]:.3f}m に到達。"
"グリッパー閉じたまま保持。目視確認に渡します。"
"（RELEASE / RETREAT / HOME 復帰は別ステップ）"
)
return True実機で動かすと、降下は最後まで通った。
[DESCEND[0] ee_y=0.110m] 最近傍 dist=23.9mm ee_y_n=+0.104m joints=[13.00, 77.00, -5.00, -42.00, -96.00, -30.00]
[DESCEND[0] ee_y=0.110m] j5 を -30 に固定 → sent_angles=[13.00, 77.00, -5.00, -42.00, -96.00, -30.00]
[DESCEND[0] ee_y=0.110m] send_angles [13.00, 77.00, -5.00, -42.00, -96.00, -30.00]
[DESCEND[0] ee_y=0.110m] [WAIT] t=0.6s max_err=1.14°
[DESCEND[0] ee_y=0.110m] 到達
[HOLD] 最終段 ee_y=0.110m に到達。グリッパー閉じたまま保持。目視確認に渡します。これまで 4 回送る前に止まっていた 104mm が、今回は送られて、実機が max_err 1.14°で一発到達した。レビューが警告した「1°ズレで 4mm 動く敏感な領域」に対して、実機は 1.14°で止まったので、高さのブレは数 mm 以内に収まった。爪と白箱フチの接触もなく降り切った。離すと、白箱に入った。接近・降下・離すの一連が、初めて繋がった。
アームが、自分を白箱と誤検出した
次に、同じ左奥でもう数回回して落下点の安定性を見ようとした。ところが、白箱検出がおかしな場所を指した。
[06f] DETECT: center=(1013,214) score=0.631 angle=75° scale=1.0 size=(212x197) template=white_box_20260520_163929_013.png
[BOX] pixel 中心=(1013, 214) score=0.631 angle=75°
[BOX] Unity 座標: (unity_x=-0.017, unity_z=-0.031) [単位 m]
[LOOKUP] 最近傍: ee=(-0.175, +0.016, +0.017) 3D距離=222.8mm
[ABORT] しきい値超過: 222.8mm > 100mm検出中心が、それまで安定していた (1335, 716) 付近から (1013, 214) ── 画面中央の上のほう ── へ飛んだ。角度も 0°から 75°、選ばれたテンプレートも別物に変わった。スコアは 0.631 で、白箱本物のときとほぼ同じ。導かれた狙い unity(-0.017, -0.031) はアームのほぼ真ん前で、そこへの最近傍は 222.8mm 離れていた。距離しきい値が捕まえて、送信前に止まった。誤検出 → ありえない狙い → 距離しきい値が捕まえて停止、という安全の連鎖は、設計どおり働いた。
同じコマンドを繰り返しても、3 回とも同じ (1005, 213) 付近・角度 75°でロックした。ランダムなら毎回違う場所に飛ぶ。同じ場所に固定でロックするのは、何か特定のものを安定して拾っているということだ。
上カメラの保存画像を見て、原因が分かった。白箱は右下にちゃんと写っている。なのに検出が拾った中心は、画面中央上 ── アーム胴体の白〜グレーの四角い塊だった。テンプレートマッチングは「登録した白箱に形と明るさが似た場所」を探す。真上から見ると、白箱もアーム胴体も、白〜グレーの四角だ。検出器は、自分の胴体と白箱を区別する手段を持っていない。
これは前にも一度起きていた問題だった。テンプレを白箱に最適化することで「白箱のほうが胴体より強くマッチする」状態に持ち込み、いったんは消えていた。だが「マット内にアームが映り込む」構造そのものは残っていた。接近前に検出が走るときのアーム姿勢が、回によって胴体を画面中央上に大きく横たえる形になると、その胴体が白箱を上回るスコアを出す。今回その条件を踏んだ。
緩める方向では直らない。白箱が検出されないのではなく、胴体が白箱と同じスコアで検出されてしまうのが問題だ。しきい値を緩めれば胴体をますます拾う。見る範囲を絞るしかない。
胴体が写る帯を、検出から外した
白箱がこれまで置かれた全ての位置の、画像上の座標が記録に残っていた。胴体の誤検出位置と並べると、きれいに分離していた。
白箱（正しい検出） P1(1390,594) P2(652,716) P3(584,385) P4(1383,355) 左奥(1335,715)
胴体（誤検出）3回 (1013,214) (1012,215) (1001,211) size≈212×197胴体は画面の中央上（X≒1005, Y≒213）に集中し、白箱 5 点は誰もその帯に入らない。白箱の X は 584, 652, 1335, 1383, 1390 で、胴体の X 帯（900〜1110）には一つも入らない。だから「中央上の矩形だけ」を検出対象から外せば、白箱の置き場所を狭めずに胴体だけを落とせる。
除外帯を X:850〜1150 / Y:80〜360 に決めた。検出候補の中心がこの矩形に入ったら、最良候補（overall_best）の選定対象から外す。マッチング計算は変えず、結果の採否だけを中心座標で判定する。06f の検出本体（evaluate_frame_gray）に、この除外を足した。変更箇所を含む全文を載せる。
# 胴体除外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)
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変更は、定数とヘルパ関数の追加、overall_best 更新箇所のフィルタ、そして全候補が除外されたときに分かるログ ── この 3 箇所だけだ。マッチング計算、全候補の保持、検出結果の戻り値のキーは変えていない。全候補が除外されると overall_best は None になり、呼び出し側は検出失敗として送信前に止まる。
この変更も降下と同様、外部レビュー（codex）を通した。実体を読んで、報告と実装が一致していること、None 経路が送信前 ABORT に繋がること、実測 8 点（胴体 3・白箱 5）が正しく分離することを、行番号付きで確認できた。固定ピクセル座標であることのリスクについては、こう整理された。
胴体取りこぼしは旧誤検出が復活して危険寄り、白箱巻き込みは送信前ABORTなので安全側の停止です。
壊れ方が非対称で、危険な側（取りこぼし）に倒れても、その先の距離しきい値が二重に捕まえる。実機が示したとおりだ。
実機で動かすと、検出は白箱に戻った。
[06f] DETECT: center=(1355,737) score=0.634 angle=0° scale=0.85 size=(158x158) template=white_box_20260520_163923_012.png
[BOX] pixel 中心=(1355, 737) score=0.634 angle=0°
[BOX] Unity 座標: (unity_x=-0.197, unity_z=+0.228) [単位 m]
[LOOKUP] 最近傍: ee=(-0.177, +0.118, +0.205) 3D距離=6.1mm誤検出の (1005, 213)・角度 75°が消え、白箱本来の右下・角度 0°・3D 距離 6.1mm に戻った。胴体除外は効いた。
白箱を動かしたら、全く入らなくなった
ここまでで、接近が通り、降下も通り、誤検出も消えた。白箱を別の場所へ動かして、同じことが起きるかを確かめた。右側（box ≒ +0.20, +0.22）と中央寄り（box ≒ +0.13, +0.24）に置いて回した。
白箱(+0.199, +0.223) / 赤ブロック(+0.276, +0.203) ΔX=+77mm ΔZ=-20mm → n
白箱(+0.133, +0.237) / 赤ブロック(+0.237, +0.249) ΔX=+104mm ΔZ=+12mm → n
白箱(+0.201, +0.228) / 赤ブロック(+0.281, +0.206) ΔX=+80mm ΔZ=-22mm → n横のずれ（ΔX）が +77 〜 +104mm。左奥では -9 〜 +3mm だったのが、白箱を右へ動かしただけで 8 〜 10cm ずれて、全く入らなくなった。
最初は検出か、テーブルか、と疑った。だが右側の 3 回のうち 2 回は、接近も降下も低い 3D 距離で正しく引けていた。
[LOOKUP] 最近傍: ee=(+0.234, +0.117, +0.205) 3D距離=8.4mm
[DESCEND[0] ee_y=0.110m] 最近傍 dist=10.2mm ee_y_n=+0.108m接近 8.4mm、降下 10.2mm。テーブルはこの座標に十分な姿勢を持っていて、アームは狙った場所に正しく到達している。それでもブロックは白箱から 8cm ずれて落ちた。lookup は合っている。アームは行くべき場所へ行っている。なのに外れる。ずれの原因は、テーブルでも検出でもなく、その後ろにあった。
1 点で入っていたのは、3 つのずれの偶然の相殺だった
ずれの正体は、把持を打ち消す補正のかけ方にあった。落下点を中心へ寄せる補正（PLACE_X_OFFSET / PLACE_Z_OFFSET）は、Unity 座標の X・Z へ固定の値で足している。だが掴んだブロックとグリッパー先端の位置関係は、グリッパーの向き ── アームの j0 の回転角 ── と一緒に回る。補正は向きに追随せず、固定のまま足される。だから、ある向きでしか合わない。
白箱の置き場所が変われば、そこへ届くアームの向き（j0）が変わる。左奥では j0 が +13 〜 +18°、右側では -30 〜 -40°。アームの向きが 70°近く違う。同じ固定補正でも、グリッパーがこれだけ向きを変えれば、補正が打ち消すべきずれとの関係が崩れる。
左奥 ×6 j0=+13〜+18° ΔX=-9〜+3mm → 主に入る
右側 ×3 j0=-30〜-40° ΔX=+77〜+104mm → 全部外れる右側の 3 回のうち 2 回は、接近も降下も低い 3D 距離で正しく引けていた（接近 8.4mm、降下 10.2mm）。lookup は合っていて、アームは狙った場所へ行っている。それでもブロックは 8cm ずれた。ずれは lookup より後ろ、固定補正とアームの向きの食い違いから出ている。
左奥では、検出のずれ・テーブル最近傍のずれ・固定補正と向きの食い違い、その 3 つがたまたま小さく相殺し合って、中心近くに落ちていた。右側ではその 3 つが揃って大きく出て、8cm 外れた。
「白箱を 1 点に固定すれば入る」は、その 1 点でだけ成り立つ偶然だった。場所を変えると、成立は崩れた。
白箱検出にも、まだ精度の問題が残っている。スコアが 0.63 〜 0.68 と低い（以前の安定した検出では 0.80 前後だった）。スケールが回ごとに 0.85 と 1.0 で揺れる。検出枠が白箱の縁の一部に乗り、中心に来ない。テンプレートが、白箱全体ではなく白い縁の一部に部分マッチしている。検出が返す中心は、物体の中心ではなく縁寄りに出ていて、そのずれ方が白箱の置き場所で変わる。これも固定補正で消せないずれの一つだ。
次にやること
次は、白箱検出のテンプレートを今の環境で撮り直す。今のテンプレートは数週間前に撮ったもので、その後の照明・白箱の向き・カメラ角の変化と乖離し、白箱全体ではなく縁に部分マッチするようになった。スコアの低下（0.80 → 0.65）が、その乖離の数値的な現れだ。白色マスクで白箱本体を自動トリムして撮り直し、スコアと中心が戻るかを測る。それで戻れば、検出の中心ずれは経年乖離が原因だったと確定する。戻らなければ、テンプレートマッチングを離れ、白い領域の輪郭から幾何中心を直接計算する方法へ切り替える。
固定補正と向きの食い違いも、別に詰める。補正を Unity 座標へ固定で足すのをやめ、グリッパーの向き（j0）に合わせて回すか、向きごとに測り直す。
接近が通り、降下も通り、誤検出も消えた。積み上げた先で、白箱を動かした瞬間に全部外れた。1 点で入っていたのは、3 つのずれがそこで偶然打ち消し合っていただけだった。場所を変えて初めて、その下に隠れていたずれが見えた。
