LOG243R2R2R2026/6/19

ロボットアームのテレメトリダッシュボードを作る Log#4 | 青シールで測る爪先とブロックのズレ。掴めても15mm離れていた

Log#1 で制御ループの状態をターミナルからブラウザへ逃がす観測層を作り、Log#2 で VLM の判定理由と座標を画面と台帳に残し、Log#3 で観測層を「一試行の成否を確定して残す道具」に変えた。ダッシュボードは pick が成功したか、place が箱に入ったかを yes / no で残せるようになっていた。

だがその二値の手前に、ずっと見えていない層があった。pick が yes で返るとき、爪は本当にブロックの中心を掴んでいるのか。掴めてはいても、爪先はブロックからどれだけ離れた場所を噛んでいるのか。成否だけを見ていると、この「掴めていても残っているズレ」が記録に残らない。

この回は、その連続量を観測に足す。爪先（グリッパーに貼った青いシール）と赤いブロックの位置差分を、掴む直前の同じ一枚のカメラ画像から測り、台帳と画面に残す。あわせて、これまで散らばっていた出力（画像・JSON・台帳）を一試行一フォルダに集約し、画面と台帳の両方からファイルに辿れるようにした。

二値の手前にある連続量

pick の成否は VLM が判定している。持ち上げた後の写真を撮り、ブロックが爪と一緒に浮いていれば yes、床に残っていれば no。これは「掴めたか」を答えるには十分だが、「どう掴めたか」は答えない。

爪が物理的にどこを噛んだかを知るには、掴む瞬間の爪先の位置と、ブロックの位置を、別々に測って引き算するしかない。爪先の位置は、グリッパーの左右の爪先と手首根元に貼った青いシールから取れる。ブロックの位置は、これまでも赤の HSV 検出で取っている。両方を同じ一枚のフレームから検出すれば、その差分が「爪先とブロックのズレ」になる。

設計で最初に潰したのは座標変換の経路だった。青シールの検出ロジックは過去に作った独立スクリプト（LOG168）にあるが、そのスクリプトは自前の homography を持っている。爪先を青シール側の homography で、ブロックを本線の homography で変換すると、差分は「検出のズレ」ではなく「homography の違いによる人工物」になる。これは過去に FK テーブルの二重配置で 97mm の乖離が出たのと同じ構造の罠だった。

だから検出ロジック（HSV マスクから重心まで）だけを本線にローカル移植し、座標変換は本線が既に import している pixel_to_unity 一本に統一した。青シールも赤ブロックも同じ関数で Unity 座標へ変換する。これで差分から homography の誤差が相殺される。

爪先の振り分けを姿勢非依存にする

LOG168 の青シール検出は、爪先2点を「画像の Y 座標が下方の2点」で選んでいた。これは横から見たカメラを前提にした選び方で、上から見るカメラ（top-down）かつ pick の j0 がブロックの X 座標で変わる本線では成立しない。アームの向きが変われば、画面上での3点の並びも変わる。

代わりに、姿勢に依存しない選び方に置き換えた。検出した青シール上位3点のうち、互いに最も近い2点を爪先、残りの1点を手首とする。爪先2点の間隔はグリッパーの開き幅、手首から爪先までの間隔はグリッパー本体の長さで、本体長より開き幅が大きくなることはない。だからアームが何度回っても、最も近い2点が爪先になる。左右の区別は要らない。欲しいのは爪先2点の中点だけだからだ。

実際の pick 到達フレームで確認すると、爪先2点の間隔はおよそ43〜44px、手首までの距離はおよそ237〜238px、比率はおよそ0.18。グリッパーが開いた状態でも、最近2点ペアの判定が崩れる余地は十分に無かった。

検出を受け持つローカル関数。青の HSV 閾値は LOG168 から流用し、座標変換はここでは一切しない。pixel 座標のまま面積上位3点を返す。

def _detect_blue_tips_px(frame):
    """フレームから青シールを検出し、面積上位3点のpixel重心 [(cx,cy),...] を返す.
    変換はしない（pixel座標のみ）。3点未満なら None。全例外を握って None.
    """
    try:
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        lower = np.array(TIP_BLUE_LOWER)
        upper = np.array(TIP_BLUE_UPPER)
        mask = cv2.inRange(hsv, lower, upper)
        k = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, k)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k)
        cnts, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        pts = []
        for c in cnts:
            a = cv2.contourArea(c)
            if a < TIP_MIN_AREA or a > TIP_MAX_AREA:
                continue
            m = cv2.moments(c)
            if m["m00"] == 0:
                continue
            cx = m["m10"] / m["m00"]
            cy = m["m01"] / m["m00"]
            pts.append((cx, cy, a))
        if len(pts) < 3:
            return None
        pts.sort(key=lambda t: t[2], reverse=True)   # 面積降順
        top3 = [(p[0], p[1]) for p in pts[:3]]        # 上位3点（ノイズ除去）
        return top3
    except Exception:
        return None

差分計算の本体。3点から最近ペアを爪先に決め、中点とブロックを同じ pixel_to_unity で Unity 座標へ変換し、mm 差分を返す。全例外を握り、検出に失敗しても None を返して制御を止めない。爪先間距離が手首までの距離を超えるという前提破綻が起きた場合も None を返してそのまま grip へ進む。測定が動作に干渉しない設計を最優先にした。

def _measure_tip_vs_block(image_path):
    """pick到達フレームから 爪先中点 と 赤block を同一frame・同一 pixel_to_unity
    で検出し差分を返す. 失敗時 None（制御を絶対止めない）.
    戻り値 dict: tip_x, tip_z, block_x, block_z（Unity m）, dx_mm, dz_mm, dist_mm

    姿勢非依存の爪先判定: 上位3点のうち「互いに最も近い2点＝爪先」、
    残り1点＝手首. これは横/上カメラやアーム向きに依存しない（DAY168 の
    「Y下方2点」は姿勢依存なので採用しない）.
    ガード: 爪先間距離 < 手首までの距離 が崩れたら（開きすぎ等で前提破綻）
    差分を None 記録してそのまま grip へ進む（測定が動作に干渉しない）.
    """
    if image_path is None:
        return None
    try:
        frame = cv2.imread(str(image_path))
        if frame is None:
            return None

        # --- 爪先（青シール上位3点 → 最近2点の中点）---
        top3 = _detect_blue_tips_px(frame)
        if top3 is None:
            return None
        import itertools
        pairs = list(itertools.combinations(range(3), 2))
        def _d(p, q):
            return math.hypot(p[0] - q[0], p[1] - q[1])
        # 最近ペア＝爪先
        i, j = min(pairs, key=lambda ij: _d(top3[ij[0]], top3[ij[1]]))
        tip1, tip2 = top3[i], top3[j]
        wrist = top3[3 - i - j]   # 0+1+2=3 → 残り1点
        mid_px = ((tip1[0] + tip2[0]) / 2.0, (tip1[1] + tip2[1]) / 2.0)
        tip_pair_d = _d(tip1, tip2)
        wrist_d = _d(mid_px, wrist)
        if not (tip_pair_d < wrist_d):   # 前提破綻ガード
            return None

        # --- block（同フレーム・同 pixel_to_unity で再検出）---
        red_px = detect_red_block(frame)
        if red_px is None:
            return None

        tip_ux, tip_uz = pixel_to_unity(mid_px[0], mid_px[1])
        blk_ux, blk_uz = pixel_to_unity(red_px[0], red_px[1])
        dx_mm = (tip_ux - blk_ux) * 1000.0
        dz_mm = (tip_uz - blk_uz) * 1000.0
        dist_mm = math.hypot(dx_mm, dz_mm)
        return {
            "tip_x": float(tip_ux), "tip_z": float(tip_uz),
            "block_x": float(blk_ux), "block_z": float(blk_uz),
            "dx_mm": float(dx_mm), "dz_mm": float(dz_mm),
            "dist_mm": float(dist_mm),
        }
    except Exception:
        return None

HSV 閾値と面積フィルタの定数は次の値を使った。青シールの検出パラメータは LOG168 のものをそのまま流用している。

TIP_BLUE_LOWER = (100, 100, 50)
TIP_BLUE_UPPER = (130, 255, 255)
TIP_MIN_AREA   = 80
TIP_MAX_AREA   = 8000

測定をどこに差すか

測定は関節を一切動かさない。pick が到達した姿勢のまま、上カメラで一枚だけ撮り、その一枚から爪先とブロックを検出する。だから差し込む場所は、pick 降下が到達した直後、グリッパーを閉じる直前になる。本線のシーケンスでは、この位置に「最終 TIP 測定（GRIP 前、評価用）」という設計上の空きが元々あったが、コードには実装が無かった。stage4 の pick 到達と stage5 のグリッパー閉が直行していた。その隙間に測定を一段差した。

撮影には既存の VLM 撮影と同じ関数を使う。新しいカメラを開く処理は書かず、撮った一枚を保存してパスを返す既存の仕組みをそのまま使った。撮影で grip が約2秒遅れるが、その間アームは pick 姿勢を保持しているだけで、関節へは何も送らない。

            # 段4 PICK
            _observe_phase("stage4_pick" + suf)
            ok, r_pick = stage_lookup_and_move(
                "pick", cur_block_xz, "stage4_pick" + suf,
                TOL_PICK_PLACE, log_record, dry_run, 4)
            if not ok:
                return False, log_path

            # 段4.5 爪先(青シール) vs block 差分測定（GRIP前・測定専用・動作非干渉）
            tip_img = _capture_for_vlm(
                "stage4_pick_tip" + suf, frames_dir, log_record,
                dry_run, run_stem=run_stem)
            tip_diff = _measure_tip_vs_block(tip_img)
            _OBSERVE_STATE["tip_pick"] = tip_diff
            log_record.setdefault("tip_measurements", []).append(
                {"attempt": attempt, "image": (str(tip_img) if tip_img else None),
                 "diff": tip_diff})
            if tip_diff is not None:
                print("[TIP   ] attempt={} dist={:.1f}mm "
                      "(dx={:.1f} dz={:.1f})".format(
                          attempt, tip_diff["dist_mm"],
                          tip_diff["dx_mm"], tip_diff["dz_mm"]))
            else:
                print("[TIP   ] attempt={} 測定スキップ（検出不足/前提破綻）"
                      .format(attempt))

            # 段5 グリッパー閉

差し込みは regrasp（掴み直し）のループの中に入っている。pick が失敗して掴み直すたびに測定が再発火し、各 attempt の差分が記録される。台帳には最後の attempt の値が残り、各 attempt の詳細は JSON の tip_measurements に全量残る。

この変更は関節制御のシーケンスに割り込むため、外部レビュー（codex）を通した。レビューでは run の冒頭で前回の測定値を消すリセットに tip_pick が含まれていない点を指摘された。単発の実行では初期値が None なので表面化しないが、同じプロセスで連続して実行し、二回目が測定の手前で中断すると、前回の値が台帳に混ざる可能性がある。リセットに一行を足して解消した。

掴めても15mm離れていた

最初の実機 run で測定値が出た。

[TIP   ] attempt=1 dist=15.5mm (dx=-3.5 dz=-15.1)

この run は pick が yes（confidence 0.85）、place も yes（confidence 0.97）、in_box も True で、掴んで運んで箱に入れて判定まですべて成功している。つまり「15.5mm ずれていても掴めてはいる」。ズレは失敗の理由ではなく、成功してもなお残っている差として記録された。

そして内訳を見ると、ズレはほぼ全部が奥行き方向（Z）に乗っていた。横方向（X）は -3.5mm でほぼ合っている。奥行き方向が -15.1mm。差分が等方ではなく、特定の軸に偏っている。これは手動でクリックして測っていた頃には見えなかった構造だった。

この差分は、左右の爪先2点の中点と、赤ブロック検出の中心との距離だ。接触面そのもののズレではなく、上カメラで見た平面上の中点と中心の差にあたる。一枚だけでは外れ値か系統的な偏りか分からない。同じ条件で何度か回した。pick の成否を含めて、測定された差分は次の通り。

run	attempt	dist (mm)	dx (mm)	dz (mm)	pick
run_0001_20260619_074617	1	15.5	-3.5	-15.1	yes
run_0001_20260619_081805	1	5.8	+1.1	-5.7	yes
run_0001_20260619_082406	1	8.2	-1.3	-8.1	yes
run_0001_20260619_151135	1	7.7	+2.2	-7.3	no
run_0001_20260619_151135	2	17.0	-12.6	-11.4	yes
run_0001_20260619_153315	1	3.9	+3.9	-0.1	yes

6 run、7 attempt。dz は全ての attempt で負だった。X の振れは ±1〜4mm で小さく、ズレの大半が Z に乗っている回が多い。ここまでは「奥行き方向に系統的な偏りがある」という読みを支持していた。

ただし最後の run_0001_20260619_153315 で、その読みが単純ではないことが出た。dz が -0.1mm、dist が 3.9mm。奥行きの偏りがほぼ消えている。これまで -5 から -15mm に偏っていた dz が、この回だけゼロに近い。掴めた写真を見ると、爪はブロックの中心をきれいに噛んでいた。

つまり奥行きの偏りは常にあるわけではなく、出る run と出ない run がある。homography に固定で乗っている定数オフセットなら毎回同じだけ出るはずで、出ない回があるということは、原因は固定オフセットではない。ブロックの位置や、毎回計算し直しているマット四隅の検出のばらつきに依存している可能性がある。

もう一つ、151135 の attempt 1 と 2 の対比も残しておく。attempt 1 は pick が no で、dz は -7.3mm。attempt 2 は yes で、dz は -11.4mm。dz の値そのものを見ても成否は割れていない。失敗した attempt の dz の方が、成功した attempt より小さい。奥行きのズレは常にあるが、それ単独では掴めるかどうかを決めていない。失敗の理由は dz の偏在ではなく、別のところにある。

成否の二値だけを見ていたら、これらはどれも「pick=yes」「pick=no」の一行で終わっていた。連続量を測ったことで、成功の中身がばらついていること、失敗と成功で奥行きのズレが反転すらしうることが見えた。成功を一つの yes に潰している限り、15.5mm の成功と 3.9mm の成功は同じ行に並ぶ。この測定で、その二つを別の成功として台帳に残せるようになった。

出力を一試行一フォルダに集約する

測定を足すのと並行して、出力先の散らばりを直した。これまで JSON と画像は Mac のローカル、台帳だけが外部 SSD（KIOXIA）にあり、画像は frames というサブフォルダ、JSON はその一つ上、と階層も揃っていなかった。機能を足すたびに出力先を継ぎ足してきた結果で、設計した配置ではなかった。

一試行のデータ（JSON・画像）を一つのフォルダにまとめ、出力先をすべて KIOXIA に向けた。アームを一回動かすと、その run のフォルダが一つでき、中に画像も JSON も揃う。台帳だけは全 run を横断するので runs 直下に置く。

/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/
  run_0001_YYYYMMDD_HHMMSS/
    run_..._stage4_pick_tip.jpg
    run_..._pick.jpg
    run_..._place.jpg
    run_..._.json
  pick_place_ledger_v2.csv

出力の起点になっていた定数を KIOXIA に向け、run フォルダを作ってその中に JSON を置くようにした。画像も同じフォルダ直下に置き、frames サブ階層は廃止した。

LOG_ROOT = Path("/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place")


def _next_log_path():
    LOG_ROOT.mkdir(parents=True, exist_ok=True)
    ts = datetime.now().strftime("%Y%m%d_%H%M%S")
    for n in range(1, 10000):
        stem = "run_{:04d}_{}".format(n, ts)
        run_dir = LOG_ROOT / stem
        if not run_dir.exists():
            run_dir.mkdir(parents=True, exist_ok=True)
            return run_dir / (stem + ".json")
    raise RuntimeError("log seq 9999 超過")

外部 SSD への出力には macOS 特有の挙動がある。SSD がマウントされていないとき、/Volumes/KIOXIA がローカルに自動生成され、内蔵ディスクに書き込まれてしまう。台帳は既にマウント確認のガードを持っていたが、JSON と画像は無条件に書いていた。run の冒頭にガードを置き、マウントされていなければ何も作る前に中断するようにした。

    # KIOXIA 未マウント時に Mac 内蔵ディスクへ誤書き込みするのを防ぐ。
    # dry_run は撮影/保存しないので素通し。実機 run のみガード。
    if not dry_run and not _ensure_ssd_mounted():
        print("[ABORT ] KIOXIA SSD 未マウント。run データの出力先が無いため中断します。"
              "（{} を確認）".format(LEDGER_DIR))
        return False, None
    log_path = _next_log_path()
    run_stem = log_path.stem

ガードは実機で両側を確認した。マウント状態では run フォルダに画像3枚と JSON が揃い、台帳が runs 直下に追記される。SSD を外して実行すると、カメラもアームも触る前に [ABORT] KIOXIA SSD 未マウント で止まり、内蔵ディスクに偽のフォルダは作られなかった。

画面と台帳の両方から辿れるようにする

集約でフォルダは一つにまとまったが、画面と台帳から実ファイルへ辿る線がまだ無かった。run の主キー（run_stem）はあるのに、そのキーから画像や JSON の場所に行けない。台帳を見て「この失敗した試行の写真が見たい」と思っても、画面からも台帳からも辿れなかった。

そこで run フォルダのパスと、撮った3枚（爪先測定・pick 判定・place 判定）のパスを、観測状態に保持し、画面の emit と台帳の両方に流すようにした。

    "run_dir": None,
    "cap_tip": None,
    "cap_pick": None,
    "cap_place": None,

各撮影の直後に、対応するキーへパスを保存する。新しい撮影はせず、既存の戻り値を保持するだけだ。

            _OBSERVE_STATE["cap_tip"] = (str(tip_img) if tip_img else None)

画面側（observe/static/index.html）では、GRIPPER パネルの in_box の下に、爪先とブロックのズレ、run フォルダのパス、3枚のファイル名を出す行を足した。run フォルダはフルパスで折り返して表示し、画像はファイル名だけ出す。フルパスは run dir で分かるからだ。

    var tp = dg.tip_pick;
    if(tp && typeof tp.dist_mm === "number"){
      els.gTipDist.textContent = tp.dist_mm.toFixed(1) + " mm";
      els.gTipDx.textContent   = (typeof tp.dx_mm==="number") ? tp.dx_mm.toFixed(1) : "—";
      els.gTipDz.textContent   = (typeof tp.dz_mm==="number") ? tp.dz_mm.toFixed(1) : "—";
    }else{
      els.gTipDist.textContent = "—";
      els.gTipDx.textContent   = "—";
      els.gTipDz.textContent   = "—";
    }
    function baseName(p){
      if(typeof p!=="string" || !p) return "—";
      var i = p.lastIndexOf("/");
      return (i>=0) ? p.slice(i+1) : p;
    }
    els.gRunDir.textContent = (typeof dg.run_dir==="string" && dg.run_dir) ? dg.run_dir : "—";
    els.gCapTip.textContent   = baseName(dg.cap_tip);
    els.gCapPick.textContent  = baseName(dg.cap_pick);
    els.gCapPlace.textContent = baseName(dg.cap_place);

ここで一つ、run dir が画面でも台帳でも空のまま出るバグに当たった。run フォルダのパスを観測状態にセットした直後、すぐ下にある「前回残置を消すリセット」が同じキーを None で上書きしていた。セットした値を9行後に潰していたことになる。リセットからその一行を削除して直した。run は毎回先頭から実行されるので、セットが各 run で値を入れ直し、残置問題も起きない。

台帳は列を足した。爪先とブロックの差分（爪先中点の座標、dx、dz、dist）に加えて、run フォルダと、爪先測定・pick の画像パスを記録する列を増やした。place の画像パスは既に列があった。

LEDGER_COLS = [
    "timestamp", "run_stem", "trial_no",
    "pick_verdict", "pick_reason", "pick_confidence",
    "place_verdict", "place_reason", "place_confidence",
    "block_pick_x", "block_pick_z",
    "block_place_x", "block_place_z",
    "box_x", "box_z",
    "in_box_geom", "in_box_dist_mm",
    "final_j1", "final_j2", "final_j3", "final_j4", "final_j5", "final_j6",
    "gripper", "place_capture_path",
    "tip_pick_x", "tip_pick_z",
    "tip_block_dx_mm", "tip_block_dz_mm", "tip_block_dist_mm",
    "run_dir",
    "tip_capture_path", "pick_capture_path",
]

列を増やすと既存の台帳とヘッダの列数が合わなくなる。同じファイルに古い列数の行と新しい列数の行が混ざると、後で集計するときに崩れる。だから列を変えるたびに、既存の台帳を退避してから新しい列数で作り直した。退避は外部 SSD のルールに従い、コピーで複製を確保してから元を消す順で行う。今日の測定値を含む台帳は、退避ファイルとして残っている。

集約後の最後の run では、台帳に run フォルダと3枚の画像パスがすべて記録された。

run_dir  : '/Volumes/KIOXIA/r2r2r-data/runs/run_pick_place/run_0001_20260619_153315'
tip_jpg  : '/Volumes/KIOXIA/.../run_0001_20260619_153315_stage4_pick_tip.jpg'
pick_jpg : '/Volumes/KIOXIA/.../run_0001_20260619_153315_pick.jpg'
place_jpg: '/Volumes/KIOXIA/.../run_0001_20260619_153315_place.jpg'

画面の GRIPPER パネルにも、同じ run の爪先ズレ（dist 3.9mm、dx 3.9、dz -0.1）と、run フォルダ、3枚のファイル名が出た。画面と台帳が同じ値で揃い、どちらからでも実ファイルに辿れるようになった。

残っていること

place の後に元の姿勢へ戻る stage10 は、この回でも全ての run で到達に失敗して中断した。置けた記録は中断より前に確定して残るので run の価値は失われていないが、最後まで完走はしていない。これは退避のトレランスを専用化したのと同じ手法で解消できるが、この回の範囲の外なので触っていない。

赤ブロックの検出は HSV のため信頼度スコアを持たず、白箱のテンプレートマッチと違って block_score が null のままになっている。爪先とブロックの奥行きのズレが、検出側のものか到達側のものかの切り分けも、まだ結論には至っていない。6 run でズレが一定でないことは見えたが、何に依存して出たり消えたりするのかは、次に追う。

観測層に足したのは、成否の手前にある一つの連続量だった。掴めたか掴めないかだけを見ていると、掴めた試行の中で爪がどこを噛んでいたかは記録に残らない。失敗から立て直すループを設計するなら、二値の成否だけでなく、成功の中身も測れている必要がある。今回はその一本目を観測に通した。