「これが箱だよ」と教える 。テンプレートマッチングと自動学習ループ
朝5時46分
Macを開いた瞬間に Console の最後のログが目に飛び込んできた。
[00:35:32] [LowerLeftFK] Robot[18] 完了 (17715点) [20/20] elapsed=8907s
[00:35:33] [LowerLeftFK] === スキャン完了 ===
[00:35:33] [LowerLeftFK] 総点数: 354294 / 生成: 354294
[00:35:33] [LowerLeftFK] 所要時間: 8907s (148.5min)
[00:35:33] [LowerLeftFK] 出力: /Users/shimazueiichi/Robotics_UX/Robotics_UX_New/Assets/Data/fk_scan_lowerleft_dense.csv前夜22時07分にPlayを押して、20体のロボットアームを並列で動かしながら取得した順運動学（Forward Kinematics）データ。約2.5時間、完走している。マットの左下区画に偏ったカバレッジを補うための追加スキャンが、無事に終わっていた。
なぜscan拡張が必要だったか
LOG153で9点試し引き検証（test_lookup_9points.py）を走らせていた。マットの9点（4隅 + 4辺中点 + 中心）を既存テーブル（134,456点）に対してlookupして、距離と関節角度の妥当性を検証するものだ。
=== Summary ===
Total points : 9
DIST_OK : 1 ← P4 (LR corner) = 0.12mm のみ
DIST_WARN : 1 ← P6 (bottom mid) = 16.97mm
DIST_FAIL : 7 ← 30〜273mm
ANGLE_OK : 9
ee_y range : [0.0213, 0.1028]
ee_y stddev : 0.03079点中、lookup距離が10mm以内（DIST_OK）だったのは1点だけ。7点は30mm以上離れていて実用にならない。マットの左下区画、特にRobot Y負側のカバレッジが決定的に足りなかった。
これを埋めるために、LOG154の前夜からUnityで追加scanを走らせていた。
Unity側では LowerLeftFKScanner.cs を新規作成し、20体のロボットに6つの起点姿勢を割り当て、各起点からj0〜j4の5軸に対して4°刻みで±16°のオフセットグリッドを展開する構成にした。6 起点 × 9^5 = 354,294点、filter後の目標50万点には届かないが、左下区画の必要領域はカバーできる見積もり。
verify で確認した改善
新規scanの出力を verify_lowerleft_scan.py で検証する。マット左下区画のPICK高さ帯（ee_x ∈ [-0.20, 0.00], ee_y ∈ [0.007, 0.047], ee_z ∈ [0.00, 0.14]）でfilterして、LOG153失敗2点の近傍候補数を数える。
=== fk_scan_lowerleft_dense.csv 検証 ===
生データ: 354294点
filter通過: 52997点 (15.0%)
ee_x: [-0.2000, -0.0004] mean=-0.1543 std=0.0319
ee_y: [ 0.0078, 0.0470] mean= 0.0264 std=0.0068
ee_z: [ 0.0022, 0.1400] mean= 0.1058 std=0.0260
j0 分布（5°ビン）:
[ -5, +0): 8
[ +0, +5): 13
[ +5, +10): 24
[+10, +15): 43
[+15, +20): 84
[+20, +25): 146
[+25, +30): 431
[+30, +35): 943
[+35, +40): 967
[+40, +45): 3102
[+45, +50): 8825
[+50, +55): 8184
[+55, +60): 9538
[+60, +65): 12549 ← 最頻
[+65, +70): 3070
[+70, +75): 2693
[+75, +80): 2377
10mm格子 被覆率:
カバー: 187/280 セル = 66.8%
DAY153失敗点の近傍候補:
robot(124.4, -155.3) → unity(-0.1553, 0.1244)
20mm以内の候補: 12867点
最近傍: 0.3mm, angles=[53.0, 89.9, -18.1, 3.9, -58.3, -29.4]
robot(134.5, -105.2) → unity(-0.1052, 0.1345)
20mm以内の候補: 3527点
最近傍: 0.2mm, angles=[42.0, 110.7, -31.3, 11.6, -76.1, -30.0]前日 dist=170mm でfallback（lookup失敗で固定動作に切り替わる）した位置に、サブミリ精度の候補が大量にある。これでPiを動かせば、今度こそアームは動的に追従するはず。
既存CSV（134,456点）に新規52,997点を追記して、187,453点の統合テーブルが完成した。
cp fk_scan_motion_based.csv fk_scan_motion_based.csv.bak_DAY154
tail -n +2 fk_scan_lowerleft_dense.csv >> fk_scan_motion_based.csv
wc -l fk_scan_motion_based.csv
# 187454 fk_scan_motion_based.csv動いた、でも逆方向
実機テスト。赤ブロックをマットの画像右下（Y負側）に置いて r2r2r_loop.py を走らせる。
[Mac] ✓ 赤ブロック検出: pixel=(575, 880) → robot=(139.9, -98.8)
[Mac] Unity座標: (-0.0988, 0.1399)
[Mac] ✓ lookup成功: ee=(-0.0993, 0.0216, 0.1395) dist=0.7mm
[Mac] angles=[31.0, 97.9, -14.1, -20.1, -46.3, -29.4]dist=0.7mm。前日 123.8mm でfallbackした位置とほぼ同じ場所で、約180倍の精度向上。
これは勝った、と思った瞬間。
カメラ画像で実機の動きを見ると、アームは赤ブロックの方向に向かわず、完全に反対側に動いた。
なんでだよ。
コ・パイロットのClaude（Anthropic製の対話型AI、戦略パートナーとして起用）と一緒に頭を抱える。lookup の数値は完璧。座標変換コードも素直（符号反転なし）。
def robot_to_unity(robot_x, robot_y):
"""Robot XY (mm) → Unity EE座標 (m)"""
unity_x = robot_y * 0.001
unity_z = robot_x * 0.001
return unity_x, unity_zシンプル。符号反転していない。なのに動きが逆。
カメラの座標、反転してみる？
ふと聞いた。
カメラで取得している座標データのXの＋とーを反転する？
Claudeの返答が早かった。反転を入れられる場所は4箇所あると整理してくれた。カメラ座標で反転、送信時にj0だけ反転、CSV内のj0列を全部書き換え、Unity シーンの link1 軸を反転。
そしてClaudeは実機検証で確かめろと言った。
ssh ubuntu@192.168.2.1
python3 ~/verify_y_direction.pyPLACE小鉢を基準にして、j0を-30°動かしたときアームがどっちに動いたかを判定する。前日のテストで「左に動いた」と判定していたのを、もう一度慎重に確認した。
結果：EEは画像右に動いた。PLACE小鉢に近づいた。
つまり「j0負 = Robot Y正側」。前日の判定と逆。ホモグラフィキャリブレーションのY軸符号が、最初から逆向きで設定されていた。
修正は一行。
robot_y = float(robot_xy[1])
robot_y = -robot_y # ホモグラフィY軸符号の校正CSVも、scanデータも、Unity sceneも触らずに、座標系全体が整合した。
実機を動かす。アームは赤ブロックの方向に正しく伸びていった。lookup動的追従の実機実証、完了。LOG154 の本来のゴールは、ここで達成された。
PLACE側もlookupにしたい
ここで終わってもよかった。でも勢いがついていた。
次はカメラで白い箱を認識させよう。こっちも位置が変わっても動的にアームが追従してPLACE位置へ行くように
PICKは赤ブロックをカメラが検出して動的にlookupできるのに、PLACE先の白箱は固定座標で決め打ちしていた。実物を置いた場所にPLACEしに行く、というR2R2Rの本来の姿からすると、片肺飛行の状態だった。
Claudeに指示書を作らせて、Claude Codeに実装を依頼することにした。
試行1 — HSV色ベース検出
まずは素朴なアプローチ。白箱は白いのだから、HSV変換して「明度が高く、彩度が低い」ピクセルだけを抽出すれば、白いかたまりが取れるはず。
# HSV変換
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# 白マスク：H全域、S低、V高
lower_white = np.array([0, 0, 180])
upper_white = np.array([180, 50, 255])
white_mask = cv2.inRange(hsv, lower_white, upper_white)
# モルフォロジーでノイズ除去
kernel = np.ones((5, 5), np.uint8)
white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel)
white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel)
# 最大輪郭の中心を白箱とする
contours, _ = cv2.findContours(white_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
largest = max(contours, key=cv2.contourArea)実機テストを走らせる。デバッグウィンドウに青い点が出た。位置：画像右端、マットの外、机の白い余白の上。
[Mac] ✓ 白箱検出: pixel=(1789, 516) → robot=(47.8, -258.8)Robot Y = -258.8mm。マットの縁は約 Y=-200mm だから、これは完全にマットの外。
画面全体で一番大きい「白い領域」を探すと、机自体の白い余白が勝ってしまっていた。白箱本体は見事に無視されている。
机の白い場所を選んでるね。四角い箱の形を認識させた方がいいね
色だけでは足りない。次。
試行2 — 色 + 形状フィルタ
マットの外に行かないよう、ホモグラフィ4点の凸包でROIマスクを切る。さらに輪郭を cv2.approxPolyDP() で近似して、4〜6頂点・アスペクト比 0.7〜1.4・面積 5,000〜50,000 px の条件に絞る。「マット内にある、だいたい正方形の白いかたまり」だけを拾う仕組み。
候補が複数残った場合は「正方形度」で一番高いものを採用する。正方形度 = 1 - (max辺長 - min辺長) / max辺長。
実装して、実機テスト。デバッグ画面を見ると、マットROIの緑線が描画されている。机の白い余白はもう拾わない。マットの中を探している。
だが、採用された青矩形はロボット本体の真上に出ていた。
候補0: area=1076 → 面積範囲外で除外
候補1: area=53 → 面積範囲外で除外
候補2: area=96 → 面積範囲外で除外
候補3: area=12480, vertices=4, aspect=2.88 → アスペクト比NGで除外
候補4: area=174172, vertices=6, aspect=1.74, squareness=0.574 → ✓採用候補
採用: squareness=0.574, area=174172
[Mac] ✓ 白箱検出: pixel=(933, 525) → robot=(32.5, -11.7)
robot(32.5, -11.7)はマットのほぼ中央。画像で確認するとそこにはロボット本体が鎮座している。
あーロボットアームも白だから、そっちにいっちゃったね笑
ここで笑うしかなかった。
改めて画面を見渡す。マットの生地は黒っぽく見えるが、細かく見ると茶色のグリッドが入って全体に淡い色。ロボットは白。机は白。白箱も白。そして周囲の壁も白。
マット、ロボット、机、白箱、全部白い世界だった。
色と形。これまでの古典的な画像認識で「物を見分ける」ためによく使われる2つの手がかりが、この世界では両方足りない。色は似ている。そして「白いマットの中にある、だいたい正方形の白いかたまり」という条件は、そのままロボット本体にも当てはまってしまう。
形状フィルタを緩めればロボットが勝つ。厳しくすれば白箱が除外される。色と形で識別できる問題設定ではなかった。
少し黙って、僕が言った。
一旦画像から箱をおれが四角く囲ったりして、箱はこれだよと学ばせた方がいいんじゃない？
29年UIUXデザイナーをやっていると、人間がどうやって物を見分けているかを考える癖がつく。形でも色でもなく、ものには「これがそれだ」という固有のパターンがある。それを機械に与える方が早い。
Claudeが反応する。「完璧に賢い。これが正解」。
試行3 — テンプレートマッチング
参照画像を作る手順は驚くほど素朴だった。
Iriun Webcamの画面をスクリーンショット撮って、プレビュー.appで白箱部分だけを矩形選択して切り抜き、templates/white_box.png として保存。230×224ピクセル、50KB。「これが箱だよ」と指差して教える、ただそれだけ。
ClaudeがClaude Codeに指示書を渡す。実装方針はシンプル：
cv2.matchTemplateで参照画像を画面内で滑らせながら相関を計算0.7 / 0.85 / 1.0 / 1.15 / 1.3 の5スケールで検索（カメラとの距離変動を吸収）
マットROI内のみ探索（マット外を黒塗り）
cv2.TM_CCOEFF_NORMED（正規化相関係数）を使用（照明変動に強い）スコア閾値 0.5 以上で検出成功
実装は5分で完了。色マスクもモルフォロジーも形状フィルタも、一切なし。ただ「このパターンに似ている場所」を探すだけ。
実機テスト。白箱を一旦マットの内側に動かして、走らせた。
デバッグウィンドウ。白箱の上に、ピタッと青い矩形がロックオン。
[Mac] ✓ 白箱検出: pixel=(1006, 605) → robot=(118.3, -182.5)
score=0.847, scale=1.15, matched_template=white_box.pngスコア0.847。閾値0.5の遥か上。ロボット本体には反応していない。机の白い余白にも反応していない。
白箱の特徴的な配色（白い縁＋黒い内側）が、テンプレートマッチングで一意に識別された。色と形では分離できなかったものが、「パターン」になった瞬間に一発で識別できる対象に変わった。
白い世界のパラダイスから、ようやく抜け出した。
あちこち動かして自動で学ばせない？
これ箱を検出する時に検出した箱の写真を自動で撮って学ばせることはできる？（箱をあちこち移動する）
Claudeの反応が速かった。ただし警告も出した。フル自動学習には危険性がある。一度誤検出したものを高スコアで保存すると、その誤検出が次から固定化される。
Claudeが5つの安全装置を設計した。
# | 安全装置 |
|---|---|
1 | 超高スコア閾値（0.8以上のみ自動追加） |
2 | テンプレート上限10枚（古いものから削除、ただし手動配置の white_box.png は保護） |
3 | 類似度フィルタ（既存と95%以上似ているなら追加しない） |
4 | 緊急停止スイッチ（ |
5 | 学習履歴ログ（全イベントを |
Claude Codeへ指示書を投げる。実装が返ってきたとき、Claude Code は安全装置を6つに拡張していた（white_box.png削除保護を独立項目として追加）。
send_coords(): 0件、物理憲法（過去に決めたコード上の制約）を全部守ったまま、自動学習ループが動き出した。
まだ届かない、でも向きは合っている
実機を動かすと、PICKもPLACEも位置が少しズレる現象が出ていた。アームは赤ブロックや白箱の方向に正しく動くが、その手前で止まったり、追い越したりする。
位置がズレてるけど一旦いいか。リアルタイムで画像見ながら位置補正できないもんかね？先にシミュレーションで動作データログ大量生成した方がいい？
Claudeが答える。
今やるべきは後者。理由は「精度より体験の成立を優先する」哲学に沿うから。そして R2R2R（Real to Render to Real）の本来の設計は、シミュレーションを「データを生む場所」として使うこと。シミュレーションを飛ばしてリアルタイム制御に行くと、R2R2Rではなくなる。
Claudeが続けた。
朝5時から3時間半作業。LOG154の本来目標は達成済み。 ここで方針判断ミスすると、次の数日が無駄になる。 今やるべきは「決断」じゃなく「整理」。
記事化を選んだ。
得たもの
LOG154の本来のメインタスクは「lookup動的追従の実機実証」だった。それは達成した。さらに：
Unity scan拡張（マット左下区画・52,997点追加、統合テーブル187,453点）
lookup動的追従の実機実証（PICK、dist=0.7mm）
ホモグラフィY軸校正（1行修正で全座標系が整合）
PLACE側のlookup動的化
白箱検出のテンプレートマッチング化
テンプレート自動学習ループ実装（6つの安全装置付き）
予定外のボーナスタスクが3つも積み上がった。
コ・パイロット作業で見えてきたこと
朝5時から8時半まで、AIツールとのコ・パイロット作業は3時間半続いた。
Claudeは何度か方向を間違えた。「j0 負 = Y 正側」と推測したが、実機は逆だった。「scan データの符号が逆」と仮説を立てたが、実際はホモグラフィ側が逆だった。「形状フィルタで白箱が検出できる」と提案したが、ロボット本体に反応した。
人間は何度か直感で正解を出した。「カメラの座標データのXの＋とーを反転する？」「箱を四角く囲って、箱はこれだよと学ばせた方がいい」「箱の写真を自動で撮って学ばせる」。
AIの知識と、29年の現場経験から来る直感。この二つがコクピットで噛み合うとき、想定の3倍速で進む。
逆に言うと、人間が直感を出さず、AIに完全に委ねると、いつか方向を間違える。
ホモグラフィY軸の符号が逆だった、という発見。これは人間が「カメラの座標反転してみる？」と聞かなければ、たどり着かなかった。AIは座標変換のコードを見て「符号反転なし、正しい」と判断していた。実機の真実を見ようとしなかった。
ロボティクスは、シミュレーションと実機の間で生まれる。
実機が真実を教えてくれる。
次のために
LOG155は、Sim-Real Calibration Loop の設計から始まる。シミュレーション内のEE位置と、実機のEE位置の差分を全位置で記録して、オフセット表で補正する。これができれば、Unity scanデータをそのまま使いながら、追い越し問題を解消できる。
そしてその先に、Phase 3：LLMによる動作デバッグの言語化が見えている。「アームがブロックの手前で止まった」をLLMが解析して「リーチ不足です」と言語化する。「アームが追い越した」を「J2の角度が大きすぎます」と言語化する。
そういう未来を作るためには、今日のような「人間の直感」と「AIの実装力」のコ・パイロット作業のログ自体が貴重なデータになる。
だから記事に残す。
技術資料：完全アーカイブ
ホモグラフィキャリブレーション（変更禁止）
upper-left: pixel(216, 82) → robot(-89.0, -202.6)
upper-right: pixel(1547, 56) → robot(-112.5, 204.6)
lower-left: pixel(330, 873) → robot(138.1, -172.9)
lower-right: pixel(1459, 847) → robot(128.6, 182.9)物理憲法（コード上の絶対制約）
Stiffness=20,000 / Damping=2,000：変更禁止
drive.target のみ更新
anchorRotation 書き込み禁止
Motion: Limited 禁止
git clean 絶対禁止
git add . 禁止（個別ファイル指定）
send_coords() 使用禁止（DAY152で確定、RZ値が位置で±167°〜−140°で暴れる）
homography.npy 値変更禁止
PLACE ROI (1571, 607, 249, 211) 変更禁止
time.sleep() の削除・短縮禁止
既存 fk_scan_motion_based.csv の上書き禁止（追記も禁止、別ファイル必須）
Unity sim と実機 mechArm では j0 軸の回転方向が反転している（DAY154発見）C. 座標変換
def robot_to_unity(robot_x, robot_y):
"""Robot XY (mm) → Unity EE座標 (m)"""
unity_x = robot_y * 0.001
unity_z = robot_x * 0.001
return unity_x, unity_z
# ブロック検出直後（DAY154で追加された1行）
robot_y = -robot_y # ホモグラフィY軸符号の校正事前検証スクリプト（test_lookup_9points.py 全文）
#!/usr/bin/env python3
"""
nearest-neighbor lookup 9点試し引き検証スクリプト
fk_scan_motion_based.csv から9点を検索し、距離・関節角度の妥当性を検証する。
実機通信なし、CSV検索のみ。
"""
import csv
import os
import math
from datetime import datetime
CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
LOG_DIR = os.path.expanduser("~/r2r2r-scripts/logs")
# mechArm 270-Pi 可動域（公式スペック）
JOINT_LIMITS = [
(-165.0, 165.0), # j0
(-165.0, 165.0), # j1
(-165.0, 165.0), # j2
(-165.0, 165.0), # j3
(-165.0, 165.0), # j4
(-175.0, 175.0), # j5
]
ANGLE_WARN_MARGIN = 5.0 # 可動域端から±5°以内でWARN
# マット4隅（確定済み・変更禁止）
UL = (-89.0, -202.6)
UR = (-112.5, 204.6)
LL = (138.1, -172.9)
LR = (128.6, 182.9)
# 9点生成
def midpoint(*points):
n = len(points)
return (sum(p[0] for p in points) / n, sum(p[1] for p in points) / n)
POINTS = [
("P1 (UL corner)", UL),
("P2 (UR corner)", UR),
("P3 (LL corner)", LL),
("P4 (LR corner)", LR),
("P5 (top mid)", midpoint(UL, UR)),
("P6 (bottom mid)", midpoint(LL, LR)),
("P7 (left mid)", midpoint(UL, LL)),
("P8 (right mid)", midpoint(UR, LR)),
("P9 (center)", midpoint(UL, UR, LL, LR)),
]
def robot_to_unity(robot_x, robot_y):
"""Robot XY → Unity EE座標変換（確定済み）"""
unity_x = robot_y * 0.001
unity_z = robot_x * 0.001
return unity_x, unity_z
def load_csv(path):
"""CSVを読み込んでリストで返す"""
rows = []
with open(path, "r") as f:
reader = csv.reader(f)
next(reader) # ヘッダスキップ
for row in reader:
ee_x = float(row[0])
ee_y = float(row[1])
ee_z = float(row[2])
joints = [float(row[j + 3]) for j in range(6)]
rows.append((ee_x, ee_y, ee_z, joints))
return rows
def nearest_neighbor(rows, query_x, query_z):
"""(ee_x, ee_z)の2次元ユークリッド距離で最近傍を検索"""
best = None
best_dist = float("inf")
for ee_x, ee_y, ee_z, joints in rows:
dist = math.sqrt((ee_x - query_x) ** 2 + (ee_z - query_z) ** 2)
if dist < best_dist:
best_dist = dist
best = (ee_x, ee_y, ee_z, joints)
return best, best_dist
def check_dist(dist_mm):
if dist_mm <= 10:
return "DIST_OK"
elif dist_mm <= 20:
return "DIST_WARN"
else:
return "DIST_FAIL"
def check_angles(joints):
for j, angle in enumerate(joints):
lo, hi = JOINT_LIMITS[j]
if angle <= lo + ANGLE_WARN_MARGIN or angle >= hi - ANGLE_WARN_MARGIN:
return "ANGLE_WARN"
return "ANGLE_OK"
def main():
print("=== nearest-neighbor lookup 9点検証 ===")
print(f"CSV: {CSV_PATH}")
rows = load_csv(CSV_PATH)
print(f"読み込み完了: {len(rows)}点\n")
timestamp = datetime.now().strftime("%Y%m%d_%H%M")
log_path = os.path.join(LOG_DIR, f"lookup_test_{timestamp}.txt")
lines = []
dist_flags = {"DIST_OK": 0, "DIST_WARN": 0, "DIST_FAIL": 0}
angle_flags = {"ANGLE_OK": 0, "ANGLE_WARN": 0}
ee_ys = []
for name, (robot_x, robot_y) in POINTS:
unity_x, unity_z = robot_to_unity(robot_x, robot_y)
match, dist = nearest_neighbor(rows, unity_x, unity_z)
ee_x, ee_y, ee_z, joints = match
dist_mm = dist * 1000 # m → mm
df = check_dist(dist_mm)
af = check_angles(joints)
dist_flags[df] += 1
angle_flags[af] += 1
ee_ys.append(ee_y)
block = []
block.append(f"=== Point {name} ===")
block.append(f"Query : robot({robot_x}, {robot_y}) → unity({unity_x:.4f}, {unity_z:.4f})")
block.append(f"Match : ee=({ee_x:.4f}, {ee_y:.4f}, {ee_z:.4f})")
joint_str = ", ".join(f"{v:.1f}" for v in joints)
block.append(f" j0={joints[0]:.1f}, j1={joints[1]:.1f}, j2={joints[2]:.1f}, j3={joints[3]:.1f}, j4={joints[4]:.1f}, j5={joints[5]:.1f}")
block.append(f"Dist : {dist_mm:.2f} mm")
block.append(f"ee_y : {ee_y:.4f}")
block.append(f"Flags : {df}, {af}")
# ANGLE_WARNの詳細
if af == "ANGLE_WARN":
for j, angle in enumerate(joints):
lo, hi = JOINT_LIMITS[j]
if angle <= lo + ANGLE_WARN_MARGIN:
block.append(f" ⚠ j{j}={angle:.1f}° (下限{lo:.0f}°まで{angle - lo:.1f}°)")
elif angle >= hi - ANGLE_WARN_MARGIN:
block.append(f" ⚠ j{j}={angle:.1f}° (上限{hi:.0f}°まで{hi - angle:.1f}°)")
block.append("")
text = "\n".join(block)
print(text)
lines.append(text)
# サマリ
ee_y_min = min(ee_ys)
ee_y_max = max(ee_ys)
ee_y_mean = sum(ee_ys) / len(ee_ys)
ee_y_stddev = math.sqrt(sum((y - ee_y_mean) ** 2 for y in ee_ys) / len(ee_ys))
summary = []
summary.append("=== Summary ===")
summary.append(f"Total points : 9")
summary.append(f"DIST_OK : {dist_flags['DIST_OK']}")
summary.append(f"DIST_WARN : {dist_flags['DIST_WARN']}")
summary.append(f"DIST_FAIL : {dist_flags['DIST_FAIL']}")
summary.append(f"ANGLE_OK : {angle_flags['ANGLE_OK']}")
summary.append(f"ANGLE_WARN : {angle_flags['ANGLE_WARN']}")
summary.append(f"ee_y range : [{ee_y_min:.4f}, {ee_y_max:.4f}]")
summary.append(f"ee_y stddev : {ee_y_stddev:.4f}")
summary.append("")
summary_text = "\n".join(summary)
print(summary_text)
lines.append(summary_text)
# ログ書き出し
with open(log_path, "w") as f:
f.write("\n".join(lines))
print(f"ログ保存: {log_path}")
if __name__ == "__main__":
main()scan検証スクリプト（verify_lowerleft_scan.py 全文）
#!/usr/bin/env python3
"""
fk_scan_lowerleft_dense.csv の検証スクリプト
- post-filter（マット左下区画 + PICK高さ帯）
- 統計出力
- 10mm格子の被覆率
- DAY153失敗2点の近傍候補数
"""
import csv
import os
import math
from collections import defaultdict
INPUT_CSV = os.path.expanduser("~/Robotics_UX/Robotics_UX_New/Assets/Data/fk_scan_lowerleft_dense.csv")
OUTPUT_CSV = os.path.expanduser("~/r2r2r-scripts/fk_scan_lowerleft_dense.csv")
LOG_PATH = os.path.expanduser("~/r2r2r-scripts/logs/DAY154_scan_verify.md")
# ターゲット領域
EE_X_MIN, EE_X_MAX = -0.200, 0.000
EE_Z_MIN, EE_Z_MAX = 0.000, 0.140
EE_Y_MIN, EE_Y_MAX = 0.007, 0.047
# DAY153失敗点（Robot座標 → Unity変換）
FAIL_POINTS = [
("robot(124.4, -155.3)", 124.4, -155.3),
("robot(134.5, -105.2)", 134.5, -105.2),
]
def robot_to_unity(robot_x, robot_y):
return robot_y * 0.001, robot_x * 0.001
def main():
print(f"=== fk_scan_lowerleft_dense.csv 検証 ===")
# CSV読み込み
all_rows = []
with open(INPUT_CSV, "r") as f:
reader = csv.reader(f)
next(reader)
for row in reader:
ee_x, ee_y, ee_z = float(row[0]), float(row[1]), float(row[2])
joints = [float(row[j + 3]) for j in range(6)]
all_rows.append((ee_x, ee_y, ee_z, joints))
print(f"生データ: {len(all_rows)}点")
# post-filter
filtered = []
for ee_x, ee_y, ee_z, joints in all_rows:
if (EE_X_MIN <= ee_x <= EE_X_MAX and
EE_Z_MIN <= ee_z <= EE_Z_MAX and
EE_Y_MIN <= ee_y <= EE_Y_MAX):
filtered.append((ee_x, ee_y, ee_z, joints))
print(f"filter通過: {len(filtered)}点 ({100 * len(filtered) / max(1, len(all_rows)):.1f}%)")
# フィルタ済みCSVを出力
with open(OUTPUT_CSV, "w", newline="") as f:
writer = csv.writer(f)
writer.writerow(["ee_x", "ee_y", "ee_z", "j0", "j1", "j2", "j3", "j4", "j5"])
for ee_x, ee_y, ee_z, joints in filtered:
writer.writerow([f"{ee_x:.6f}", f"{ee_y:.6f}", f"{ee_z:.6f}"] +
[f"{j:.3f}" for j in joints])
print(f"フィルタ済みCSV出力: {OUTPUT_CSV}")
# 統計
if filtered:
xs = [r[0] for r in filtered]
ys = [r[1] for r in filtered]
zs = [r[2] for r in filtered]
j0s = [r[3][0] for r in filtered]
def stats(vals, name):
mn, mx = min(vals), max(vals)
mean = sum(vals) / len(vals)
std = math.sqrt(sum((v - mean) ** 2 for v in vals) / len(vals))
return f"{name}: [{mn:.4f}, {mx:.4f}] mean={mean:.4f} std={std:.4f}"
print(f"\n{stats(xs, 'ee_x')}")
print(stats(ys, 'ee_y'))
print(stats(zs, 'ee_z'))
# j0ヒストグラム（5°ビン）
print("\nj0 分布（5°ビン）:")
bins = defaultdict(int)
for j in j0s:
b = int(j // 5) * 5
bins[b] += 1
for b in sorted(bins.keys()):
bar = "#" * min(50, bins[b] // max(1, len(filtered) // 500))
print(f" [{b:+4d}, {b + 5:+4d}): {bins[b]:6d} {bar}")
# 10mm格子の被覆率
print("\n10mm格子 被覆率:")
grid_cells = set()
for ee_x, ee_y, ee_z, joints in filtered:
gx = int(ee_x * 1000 // 10) * 10 # 10mm刻み
gz = int(ee_z * 1000 // 10) * 10
grid_cells.add((gx, gz))
# ターゲット領域の全格子セル数
total_cells = 0
for gx in range(int(EE_X_MIN * 100), int(EE_X_MAX * 100), 1):
for gz in range(int(EE_Z_MIN * 100), int(EE_Z_MAX * 100), 1):
total_cells += 1
covered = len(grid_cells)
coverage = 100 * covered / max(1, total_cells)
print(f" カバー: {covered}/{total_cells} セル = {coverage:.1f}%")
# DAY153失敗2点の近傍候補数
print("\nDAY153失敗点の近傍候補:")
for name, rx, ry in FAIL_POINTS:
ux, uz = robot_to_unity(rx, ry)
candidates = []
for ee_x, ee_y, ee_z, joints in filtered:
dist = math.sqrt((ee_x - ux) ** 2 + (ee_z - uz) ** 2)
if dist <= 0.020: # 20mm以内
candidates.append((dist * 1000, joints))
print(f" {name} → unity({ux:.4f}, {uz:.4f})")
print(f" 20mm以内の候補: {len(candidates)}点")
if candidates:
candidates.sort()
best_dist, best_j = candidates[0]
print(f" 最近傍: {best_dist:.1f}mm, angles={[round(j, 1) for j in best_j]}")
# サマリログ書き出し（略）
if __name__ == "__main__":
main()Unity scan 設計パラメータ（LowerLeftFKScanner.cs 相当）
起点姿勢（6個、j0+15°〜+65°に分散）:
Pose A: [+15, 102.74, -43.33, 3.6, -60.11, -30]
Pose B: [+30, 102.74, -43.33, 3.6, -60.11, -30]
Pose C: [+45, 102.74, -43.33, 3.6, -60.11, -30]
Pose D: [+60, 102.74, -43.33, 3.6, -60.11, -30]
Pose E: [+15, 81.91, -30.14, -8.08, -50.27, -29.44] ← APPROACH_PICK系
Pose F: [+45, 81.91, -30.14, -8.08, -50.27, -29.44]
オフセットグリッド:
j0〜j4 の5軸に対して [-16, -12, -8, -4, 0, +4, +8, +12, +16]° (9値) を展開
j5 は固定
1起点あたり: 9^5 = 59,049点
6起点合計: 6 × 59,049 = 354,294点
Post-filter（ターゲット領域）:
ee_x ∈ [-0.200, 0.000] m
ee_y ∈ [ 0.007, 0.047] m (PICK高さ 27mm ± 20mm)
ee_z ∈ [ 0.000, 0.140] m
filter通過率: 15.0% (52,997点)
並列実行:
20体 × 17,715点/体 (平均)
所要時間: 8,907秒 (148.5分)自動Scanner無効化コード（C# 抜粋）
// LowerLeftFKScanner.cs 内
void DisableOtherScanners(List<GameObject> arms)
{
var conflictingTypes = new System.Type[]
{
System.Type.GetType("VariationMotionPlayer"),
System.Type.GetType("MotionLogPlayer"),
System.Type.GetType("GridAngleScanner"),
System.Type.GetType("ParallelFKScanner")
};
foreach (var arm in arms)
{
foreach (var t in conflictingTypes)
{
if (t == null) continue;
var comp = arm.GetComponent(t) as MonoBehaviour;
if (comp != null)
{
comp.enabled = false;
Debug.Log($"[LowerLeftFK] Disabled {t.Name} on {arm.name}");
}
}
}
}
void Start()
{
// ... ロボット20体を foundRobots に検出 ...
DisableOtherScanners(foundRobots);
// scan開始
}白箱テンプレートマッチング（find_white_box 全容）
import os
import cv2
import numpy as np
from datetime import datetime
TEMPLATE_DIR = os.path.expanduser("~/r2r2r-scripts/templates")
LEARNING_LOG = os.path.expanduser("~/r2r2r-scripts/logs/template_learning.log")
PROTECTED_TEMPLATE = "white_box.png" # 削除保護
MAX_TEMPLATES = 10
AUTO_ADD_THRESHOLD = 0.8 # 自動追加の最低スコア
SIMILARITY_THRESHOLD = 0.95 # 類似度フィルタ
DETECT_THRESHOLD = 0.5 # 検出成功の最低スコア
SCALES = [0.7, 0.85, 1.0, 1.15, 1.3]
def load_all_templates():
templates = {}
if not os.path.isdir(TEMPLATE_DIR):
return templates
for filename in sorted(os.listdir(TEMPLATE_DIR)):
if not filename.endswith(".png"):
continue
if filename.startswith("."):
continue
path = os.path.join(TEMPLATE_DIR, filename)
img = cv2.imread(path, cv2.IMREAD_COLOR)
if img is not None:
templates[filename] = img
return templates
ALL_TEMPLATES = load_all_templates()
def log_event(message):
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
os.makedirs(os.path.dirname(LEARNING_LOG), exist_ok=True)
with open(LEARNING_LOG, "a") as f:
f.write(f"[{timestamp}] {message}\n")
def find_white_box(frame, mat_corners_px, debug_log=None):
"""
テンプレートマッチングで白箱を検出。
マットROI内のみ、複数スケール検索、複数テンプレート対応、
高スコア時の自動学習付き。
"""
if not ALL_TEMPLATES:
return None
# マットROIマスク
mat_mask = np.zeros(frame.shape[:2], dtype=np.uint8)
cv2.fillConvexPoly(mat_mask, mat_corners_px, 255)
masked_frame = cv2.bitwise_and(frame, frame, mask=mat_mask)
best_score = -1.0
best_loc = None
best_size = None
best_template_name = None
for tmpl_name, tmpl in ALL_TEMPLATES.items():
TH, TW = tmpl.shape[:2]
for scale in SCALES:
new_w = int(TW * scale)
new_h = int(TH * scale)
if new_w < 30 or new_h < 30:
continue
if new_w >= masked_frame.shape[1] or new_h >= masked_frame.shape[0]:
continue
scaled = cv2.resize(tmpl, (new_w, new_h))
result = cv2.matchTemplate(masked_frame, scaled,
cv2.TM_CCOEFF_NORMED)
_, max_val, _, max_loc = cv2.minMaxLoc(result)
if max_val > best_score:
best_score = max_val
best_loc = max_loc
best_size = (new_w, new_h)
best_template_name = tmpl_name
if best_score < DETECT_THRESHOLD:
if debug_log is not None:
debug_log.append(f"白箱検出失敗: best_score={best_score:.3f}")
return None
x, y = best_loc
w, h = best_size
cx, cy = x + w // 2, y + h // 2
if debug_log is not None:
debug_log.append(
f"白箱検出成功: center=({cx},{cy}), score={best_score:.3f}, "
f"matched={best_template_name}, scale={w/ALL_TEMPLATES[best_template_name].shape[1]:.2f}"
)
# 自動学習（高スコアのみ）
if best_score >= AUTO_ADD_THRESHOLD:
try_auto_add_template(frame, x, y, w, h, best_score)
return (cx, cy)
def try_auto_add_template(frame, x, y, w, h, score):
# 緊急停止スイッチ
lock_path = os.path.join(TEMPLATE_DIR, ".lock")
if os.path.exists(lock_path):
return
cropped = frame[y:y+h, x:x+w].copy()
# 類似度フィルタ
for tmpl_name, tmpl in ALL_TEMPLATES.items():
if tmpl.shape != cropped.shape:
tmpl_resized = cv2.resize(tmpl, (cropped.shape[1], cropped.shape[0]))
else:
tmpl_resized = tmpl
result = cv2.matchTemplate(cropped, tmpl_resized, cv2.TM_CCOEFF_NORMED)
_, sim, _, _ = cv2.minMaxLoc(result)
if sim >= SIMILARITY_THRESHOLD:
log_event(f"SKIP score={score:.3f} reason='similar to {tmpl_name} (sim={sim:.3f})'")
return
# 上限数チェック（white_box.png は保護）
existing = [f for f in os.listdir(TEMPLATE_DIR)
if f.endswith(".png") and not f.startswith(".")
and f != PROTECTED_TEMPLATE]
if len(existing) + 1 >= MAX_TEMPLATES:
existing_with_time = [(f, os.path.getmtime(os.path.join(TEMPLATE_DIR, f)))
for f in existing]
oldest = min(existing_with_time, key=lambda x: x[1])
os.remove(os.path.join(TEMPLATE_DIR, oldest[0]))
if oldest[0] in ALL_TEMPLATES:
del ALL_TEMPLATES[oldest[0]]
log_event(f"REMOVE {oldest[0]} reason='upper limit, oldest'")
# 追加
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
new_name = f"white_box_auto_{timestamp}.png"
new_path = os.path.join(TEMPLATE_DIR, new_name)
cv2.imwrite(new_path, cropped)
ALL_TEMPLATES[new_name] = cropped
log_event(f"ADD {new_name} score={score:.3f} size={w}x{h}")LOG153 vs LOG154 対比
項目 | LOG153 | LOG154 |
|---|---|---|
テーブル規模 | 134,456点 | 187,453点（+52,997点） |
ee_x カバー範囲 | [+0.012, +0.230] | [-0.200, +0.230] |
j0 カバー範囲 | [-65, -15] | [-65, -15] ∪ [+15, +75] |
10mm格子被覆率 | 未計測 | 66.8%（マット左下区画） |
robot(124.4, -155.3) lookup | dist=170.1mm → fallback | dist=0.7mm → lookup成功 |
robot(134.5, -105.2) lookup | dist=123.8mm → fallback | dist=0.2mm → lookup成功 |
実機動作 | HOME固定復帰 | ブロック方向に動的追従 |
白箱検出 | 未実装 | テンプレートマッチング + 自動学習 |
PLACE方式 | 固定角度 | 動的lookup |
実機テスト主要ログ
Y軸反転前（動きが逆）:
[Mac] ✓ 赤ブロック検出: pixel=(575, 880) → robot=(139.9, -98.8)
[Mac] Unity座標: (-0.0988, 0.1399)
[Mac] ✓ lookup成功: ee=(-0.0993, 0.0216, 0.1395) dist=0.7mm
[Mac] angles=[31.0, 97.9, -14.1, -20.1, -46.3, -29.4]
→ 実機はブロックと逆方向に動いたY軸反転後（正しい方向に追従）:
[Mac] ✓ 赤ブロック検出: pixel=(1454, 866) → robot=(134.5, -181.4)
[Mac] Unity座標: (-0.1814, 0.1345)
[Mac] ✓ lookup成功: ee=(-0.1812, 0.0266, 0.1346) dist=0.2mm
[Mac] angles=[53.0, 102.0, -48.0, 1.0, -52.0, -32.0]
→ 実機がブロック方向に伸びる白箱テンプレートマッチング成功:
[Mac] ✓ 白箱検出: pixel=(1006, 605) → robot=(118.3, -182.5)
score=0.847, scale=1.15, matched_template=white_box.png
