LOG169R2R2R2026/4/25

Unity のグリッパーの爪先を TipBrepReader で測れる目を持って、誤差の正体が見えた

💡 Key Takeaway

「19mm誤差を詰める」という前提で始めたら、UnityのFKスキャンテーブルが現実のUnity状態から97mmズレていた。19mmは偶然成立していた数字で、本当の問題はもっと奥にあった。それが見えたのは、Unityのグリッパーの爪先がどこにいるかを自分で測れる目（TipBrepReader）を作ったからだ。

始まり：19mmという誤差を詰めるはずだった

引き継ぎ書には、こう書いてあった。

PICK 物理動作（爪のリーチ）: ❌ DAY168（約19mmズレ実測）

確認済みPICK angles [-50.0, 100.0, -55.0, -2.0, -35.0, -20.0] で、Pi側のlookupが成功して、青シールで実機爪先MIDを測ったら、目標赤ブロックから19mmズレていた。

朝の最初の問いは「この19mmをどう詰めるか」だった。

補正という選択肢を、最初に落とした

19mm誤差を詰める方法はいくつかある。一番やってはいけないのは、姿勢ごとに違う補正値をlookup後に加算する方式。R2R2Rの環境では大量シミュレーションも強化学習も難しい。姿勢依存の補正は学習で求めるしかなく、それができないなら採れない。

基本はUnity側をそのまま正、だよね。
そりゃあ全てが完璧に整っているから。

このプロジェクトの名前は Real2Render2Real。真ん中の「Render（＝Unity）」を基準にして、実機をそこに合わせる構造。Unityをズラすと、URDF、FKスキャン134,456点、lookup table、homography、全てが連動して崩れる。実機は後から金で解決できる（P001）。

つまり今日やるのは、補正ではなく「Unity（正）と実機（合わせる側）の差を1回だけ測ってキャリブレーション定数として焼き込めるか判断する」こと。そう決まった。

測れる目を持っていなかった

Unity側を正と決めたら、次にやるべきは「Unityの中で、PICK姿勢時に爪先がどこにいるのか」を実測すること。これがないと、実機との差を語れない。

ところがUnityには、その手段がなかった。

MotionLogPlayer.cs はCSVを読んで再生するだけ。FKScanner_MotionBased.cs はPlayすると134,456点の自動スキャンが始まる。「任意のangles を入れて、その姿勢を取らせて、爪先のWorld座標を読み取る」という単純な機能を持つスクリプトが存在しなかった。

LOG157で確定した重要事実：UnityのFKスキャナーが記録するEE座標は、leftTipBrep と rightTipBrep の中点と root の差（midpoint - root）で計算されている。手首ではなく、CADインポートで生まれた2つのBrepの中点。

これと同じ計算を、任意のanglesで再現するツールが要る。

TipBrepReader.cs を作った

新規スクリプト1本。既存のFKスキャナーやMotionLogPlayerには触らない。物理憲法（Stiffness/Damping/anchorRotation）にも触らない。drive.target だけを書き、Brepの位置を読み出す。

スコープを最小に切った理由は、LOG168の教訓だ。「目視だけ」のはずが3問題分解に膨らんだ前例がある。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// TipBrepReader  DAY169
/// PICK姿勢時の leftTipBrep / rightTipBrep の World Position を読み取る。
/// Unity EE定義（midpoint - root）と実機爪先MID（青シール計測値）の差を定量化するためのツール。
///
/// 使い方:
///   1. 空GameObjectにアタッチ
///   2. Inspectorでroot, joints[6], leftTipBrep, rightTipBrepを割り当て
///   3. Playモードでコンポーネント右上メニュー「⋮」→「Read Tip Brep Positions」
/// </summary>
public class TipBrepReader : MonoBehaviour
{
    [Header("PICK姿勢角度（度、6要素）")]
    public float[] angles = new float[] { -50.0f, 100.0f, -55.0f, -2.0f, -35.0f, -20.0f };

    [Header("アーム参照")]
    public ArticulationBody rootArticulationBody;
    public ArticulationBody[] joints; // 6個: link1〜link6

    [Header("爪先Brep参照")]
    public Transform leftTipBrep;
    public Transform rightTipBrep;

    [Header("設定")]
    public float waitSecondsAfterSet = 3.0f;
    public bool runOnStart = false;

    void Start()
    {
        if (runOnStart)
            StartCoroutine(ReadRoutine());
    }

    [ContextMenu("Read Tip Brep Positions")]
    public void ReadPositions()
    {
        StartCoroutine(ReadRoutine());
    }

    private IEnumerator ReadRoutine()
    {
        // nullチェック（省略：root, joints, leftTipBrep, rightTipBrep, angles）

        // SetJointPositions で瞬時セット（物理バイパス）
        var positions = new List<float>();
        for (int i = 0; i < 6; i++)
            positions.Add(angles[i] * Mathf.Deg2Rad);
        rootArticulationBody.SetJointPositions(positions);
        Physics.SyncTransforms();

        // drive.target も同期（物理が逆方向に戻すのを防ぐ）
        for (int i = 0; i < 6; i++)
        {
            if (joints[i] == null) continue;
            var drive = joints[i].xDrive;
            drive.target = angles[i];
            joints[i].xDrive = drive;
            joints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }

        // 待機
        Debug.Log($"[TipBrepReader] 姿勢セット完了。{waitSecondsAfterSet}秒待機中...");
        yield return new WaitForSeconds(waitSecondsAfterSet);

        // Transform読み取り
        Vector3 leftPos = leftTipBrep.position;
        Vector3 rightPos = rightTipBrep.position;
        Vector3 midpoint = (leftPos + rightPos) * 0.5f;
        Vector3 rootPos = rootArticulationBody.transform.position;
        Vector3 ee = midpoint - rootPos;

        // 出力
        Debug.Log("[TipBrepReader] === PICK姿勢 Transform 読み取り結果 ===");
        Debug.Log($"[TipBrepReader] angles: [{angles[0]:F2}, {angles[1]:F2}, ...]");
        Debug.Log($"[TipBrepReader] leftTipBrep  World: ({leftPos.x:F6}, {leftPos.y:F6}, {leftPos.z:F6})");
        Debug.Log($"[TipBrepReader] rightTipBrep World: ({rightPos.x:F6}, {rightPos.y:F6}, {rightPos.z:F6})");
        Debug.Log($"[TipBrepReader] Midpoint    World: ({midpoint.x:F6}, {midpoint.y:F6}, {midpoint.z:F6})");
        Debug.Log($"[TipBrepReader] EE (Mid-Root)    : ({ee.x:F6}, {ee.y:F6}, {ee.z:F6})");
    }
}

設計のポイントは3つ。

SetJointPositions で瞬時テレポート。 補間なし、物理シミュレーションを通さず姿勢を確定する。FKスキャナーと同じ方式。
drive.target も同期。 SetJointPositions直後にdrive.targetが古い値のままだと、物理エンジンが姿勢を「戻そう」とする。両方書き込んで初めて姿勢が固定される。
ContextMenuで手動トリガー。 Play中にInspectorからRead Tip Brep Positionsを呼ぶ。Start時の自動実行はオプション。手動探索のために設計した。

最初の読み取り：実機青シールとほぼ一致した

Unity Editorで空GameObjectにTipBrepReaderをアタッチして、Inspectorでrootとjoints[6]とleftTipBrep/rightTipBrepを割り当てた。anglesはデフォルト（LOG168 PICK angles）のまま。Play→Read Tip Brep Positions。

Console出力：

[TipBrepReader] === PICK姿勢 Transform 読み取り結果 ===
[TipBrepReader] angles: [-50.00, 100.00, -55.00, -2.00, -35.00, -20.00]
[TipBrepReader] leftTipBrep  World: (0.232090, 0.011789, 0.238560)
[TipBrepReader] rightTipBrep World: (0.271550, 0.025196, 0.196901)
[TipBrepReader] Midpoint    World: (0.251820, 0.018492, 0.217731)
[TipBrepReader] Root        World: (0.000000, 0.000000, 0.000000)
[TipBrepReader] EE (Mid-Root)    : (0.251820, 0.018492, 0.217731)

Unity爪先MID (0.252, 0.018, 0.218) と、LOG168実測の実機爪先MID開 (0.250, _, 0.197) を並べる：

軸	Unity爪先MID	実機爪先MID開	差
X	0.252	0.250	+1mm
Z	0.218	0.197	+22mm

X方向はほぼ一致、Z方向だけ22mm差。これはまずまず近い。19mmという数字に整合する範囲だ。

ところが、ここで別の数字が引っかかった。

引き継ぎ書のEE値と一致しない

引き継ぎ書には、PICK姿勢でlookup成功時のUnity EEとして (0.183, 0.042, 0.154) と記載されていた。

今TipBrepReaderで測ったのは (0.252, 0.018, 0.218)。同じanglesのはずなのに、約97mm違う。

3D距離で約97mm。これはノイズではない。この食い違いの正体を、後で確認する必要がある。

ひとまず、目の前の作業（PICK姿勢の精度追求）に進んだ。

数値の真ん中は、正解ではなかった

赤ブロックの正確な位置を、Unity EditorでRedBlock_arm1のTransformから取得した。

RedBlock_arm1 Transform Position:
X = 0.2401
Y = 0.012
Z = 0.214
Scale = 0.024 (24mm立方体)
Rotation Y = 47°

引き継ぎ書の (0.2325, _, 0.2048) とは違う値だった。引き継ぎ書のミスか、ブロックが動いたのか、いずれにせよ正解は (0.2401, 0.012, 0.214) だ。

現状の差分を再計算：

軸	Unity爪先MID	赤ブロック中心	差
X	0.252	0.2401	+10.9mm
Z	0.218	0.214	+5mm

XZ平面距離約12mm。24mmブロック半径12mmギリギリ。爪の中心はブロックから少し外れている。

ここでUnityのSceneビューを見て、はっきり分かることがあった。爪先がブロックの真上にいない。閉じればブロックを横から押すか、爪の根本で挟むだけになる。

最終的には爪が閉じてブロックを掴む姿勢になる時に、爪の位置が動くと想定した時、この見た目（視覚的）の位置にしないと実際にブロックを正しく掴めない。

数値の真ん中（爪先MIDが赤ブロック中心と一致）が答えではない。閉じた時に爪がブロックを正しく挟む姿勢が答えだ。

LOG168確定事実：開→閉差分は X+10mm, Z+10mm。つまり開状態で爪先MIDが「閉じた後に来てほしい位置から-10mm, -10mm」のところにいれば、閉じた時にちょうど良くなる。

「爪先MIDの数値中心」と「視覚的に正しい姿勢」は違う。視覚的な正解の方が、物理に合っている。

TipBrepReaderで手動探索する

TipBrepReader Inspectorで angles を変更して、Read Tip Brep Positionsを押す。Sceneビューで爪先とブロックの位置関係を見て、判断する。複数の試行を経て、確定した。

angles = [-49, 98, -32, 0, -65, 0]

Console出力:
[TipBrepReader] leftTipBrep  World: (0.209844, 0.016945, 0.224897)
[TipBrepReader] rightTipBrep World: (0.248781, 0.016937, 0.180668)
[TipBrepReader] Midpoint    World: (0.229313, 0.016941, 0.202783)
[TipBrepReader] EE (Mid-Root)    : (0.229313, 0.016941, 0.202783)

開状態の爪先MID (0.229, 0.017, 0.203) に、開→閉差分 +10mm, +10mm を足すと (0.239, _, 0.213)。赤ブロック中心 (0.2401, _, 0.214) と、X方向-1.1mm、Z方向-1mm。誤差1mm前後で命中する見込みだ。

anglesと比較：

関節	LOG168	LOG169確定	差
j0	-50	-49	-1
j1	100	98	-2
j2	-55	-32	+23
j3	-2	0	+2
j4	-35	-65	-30
j5	-20	0	+20

j2/j4/j5に大きな差。同じ「PICK姿勢」と呼ばれていたものが、実は全く違う姿勢だった。

FKスキャンテーブルを再lookupする

ここで気になるのは、引き継ぎ書のEE値と97mm食い違っていた件だ。

LOG169確定姿勢が「赤ブロックを掴める姿勢」なら、fk_scan_motion_based.csv を赤ブロック中心 (0.2401, 0.012, 0.214) でlookupしたら、これに近い angles が返るはずだ。

verify_lookup_day169.py を作って実行した。

#!/usr/bin/env python3
"""
verify_lookup_day169.py
fk_scan_motion_based.csv から赤ブロック中心座標の最近傍をlookupし、
DAY169 TipBrepReader確定anglesと比較する。
"""

import os
import csv
import numpy as np

CSV_PATH = os.path.expanduser("~/r2r2r-scripts/fk_scan_motion_based.csv")
TARGET = np.array([0.2401, 0.012, 0.214])
TARGET_XZ = np.array([0.2401, 0.214])

EE_Y_MIN = 0.007
EE_Y_MAX = 0.047

DAY168_PICK_ANGLES = [-50.0, 100.0, -55.0, -2.0, -35.0, -20.0]
DAY168_LOOKUP_EE = (0.183, 0.042, 0.154)
DAY168_LOOKUP_DIST = 71.1

DAY169_PICK_ANGLES = [-49, 98, -32, 0, -65, 0]
DAY169_MID = (0.229313, 0.016941, 0.202783)


def load_csv():
    rows = []
    with open(CSV_PATH, "r") as f:
        reader = csv.reader(f)
        next(reader)  # header
        for r in reader:
            ee_x, ee_y, ee_z = float(r[0]), float(r[1]), float(r[2])
            joints = [float(r[j + 3]) for j in range(6)]
            rows.append((ee_x, ee_y, ee_z, joints))
    return rows


def main():
    rows = load_csv()

    # 距離計算
    data = []
    for ee_x, ee_y, ee_z, joints in rows:
        ee = np.array([ee_x, ee_y, ee_z])
        dist_3d = np.linalg.norm(ee - TARGET) * 1000.0
        dist_xz = np.linalg.norm(np.array([ee_x, ee_z]) - TARGET_XZ) * 1000.0
        data.append((dist_3d, dist_xz, ee_x, ee_y, ee_z, joints))

    # ee_y フィルタ後 + XZ距離 (r2r2r_loop.py 方式)
    filtered = [(d3, dxz, ex, ey, ez, j) for d3, dxz, ex, ey, ez, j in data
                if EE_Y_MIN <= ey <= EE_Y_MAX]
    sorted_xz = sorted(filtered, key=lambda x: x[1])[:5]

    # 出力（テーブル整形は省略）
    for rank, (dist_3d, dist_xz, ex, ey, ez, j) in enumerate(sorted_xz, 1):
        print(f"{rank} | XZ {dist_xz:.2f}mm | ee=({ex:.3f},{ey:.3f},{ez:.3f}) | j={j}")


if __name__ == "__main__":
    main()

実行結果：

==============================================================================================================
DAY169 lookup検証
目標座標: (0.2401, 0.012, 0.214) [Unity m]
CSV: /Users/shimazueiichi/r2r2r-scripts/fk_scan_motion_based.csv
総行数: 213312
==============================================================================================================

【ee_y フィルタ後 + XZ距離のみ (r2r2r_loop.py 方式) から top 5】
フィルタ通過行数: 156603
順位 | XZ距離(mm) 3D距離(mm) | ee_x       ee_y       ee_z       | j0      j1       j2      j3     j4      j5
1    |  82.98    88.14    | 0.182957   0.041725   0.153837   | -50.000 100.000 -55.000 -2.000 -35.000 -20.000
2    |  83.17    90.07    | 0.182180   0.046575   0.154316   | -50.000  95.000 -50.000 -2.000 -35.000 -20.000
3    |  83.45    88.42    | 0.180132   0.041224   0.155964   | -50.000 100.000 -55.000 -7.000 -35.000 -20.000
4    |  83.67    89.52    | 0.175812   0.043835   0.160453   | -47.000 105.000 -59.000  3.500 -45.000 -30.500
5    |  83.70    88.42    | 0.184179   0.040498   0.151726   | -50.000 100.000 -55.000  3.000 -35.000 -20.000

【lookup top1 (XZ距離) vs DAY169確定angles 関節差分】
関節     | lookup   | DAY169   | 差分
  j0   |  -50.000 |    -49.0 |  -1.000
  j1   |  100.000 |     98.0 |  +2.000
  j2   |  -55.000 |    -32.0 | -23.000
  j3   |   -2.000 |      0.0 |  -2.000
  j4   |  -35.000 |    -65.0 | +30.000
  j5   |  -20.000 |      0.0 | -20.000

lookup top1 のee座標 (0.183, 0.042, 0.154) は、LOG168引き継ぎ書記載値と完全一致した。Pi側のlookupは正しく動いていた。CSVから最近傍を引けていた。

同じanglesで、CSVと現Unityが97mm違った

ここで、もう一つ確認したいことがあった。

lookup top1 が返した angles [-50, 100, -55, -2, -35, -20] を TipBrepReader に入れて、Unity の現状でこの姿勢を取らせたら、爪先MIDはどこに来るか。CSVに記録された ee (0.183, 0.042, 0.154) と一致するか。

Read Tip Brep Positions の結果：

angles = [-50, 100, -55, -2, -35, -20]
Midpoint = (0.251820, 0.018492, 0.217731)

CSV記載 ee (0.183, 0.042, 0.154) と TipBrepReader 実測 (0.252, 0.018, 0.218) の差：

軸	CSV記載	TipBrepReader実測	差
X	0.183	0.252	-69mm
Y	0.042	0.018	+24mm
Z	0.154	0.218	-64mm

3D距離で約97mm。同じanglesに対して、FKスキャナーが記録した値と、今のUnity状態で同じ式で測った値が、約97mm違う。

二重の問題

これが分かった瞬間、19mm誤差の話の構造が反転した。

問題A: FKスキャンテーブルと現Unity状態の97mm乖離。
fk_scan_motion_based.csv の ee座標と、現在のUnity状態で同じ angles を取った時の midpoint - root が約97mm違う。原因はまだ分からない。FKScanner_MotionBased.cs の20体並列実行時の座標計算誤差、Physics.SyncTransforms のタイミング、参照Brepの違い、いずれかが疑わしい。

問題B: スキャン範囲の不足。
LOG169確定姿勢の j5=0、j2=-32 という組み合わせは、FKスキャンテーブルに存在しない。lookupで返せる姿勢空間が、必要な姿勢空間を網羅していない。

LOG168の「19mm誤差」は、この二重の問題の上に偶然成立していた数字だった。FKスキャナーが「赤ブロックから97mm離れた爪先位置」を返してくる→実機側の物理オフセットがたまたまそれを縮める方向に作用→結果として19mmで止まっていた。

「PICK lookup成功」と書かれていても、「掴める姿勢」ではなかった。lookupの返り値は、ブロックを掴むのに必要な姿勢から始めから外れていた。

得たもの

赤ブロック専用の PICK angles [-49, 98, -32, 0, -65, 0] が手に入った。これは現在の赤ブロック位置 (0.2401, 0.012, 0.214) 専用解で、ブロック位置が変われば再探索が要る。汎用化はFKスキャンテーブルの問題A・Bを解いてからの話。

そして、爪先がどこにいるかを自分で測れる目（TipBrepReader）が手に入った。これは今後ずっと使える。次にFKスキャナーの計算ロジックを検証する時も、別姿勢の精度を出す時も、同じツールで測れる。

19mm誤差の話を引き継いでそのまま詰めていたら、正しく動いていない数字を信じて、見当違いの方向に時間を使っていた。「lookup成功」を「掴める姿勢」と読み替えていた。

測れる目を持って初めて、何が問題で何が問題でないかが見えた。

新しい原則：P008

発見を、メソドロジーの原則に追加した。

P008: 測れる手段を持つことが、推論より先にくる。

引き継ぎ書の数値、ログの数値、過去の判断。それらを推論で繋いでも、測れる手段を持っていなければ、どこが正しくてどこが間違っているか確かめられない。19mm誤差が「19mm誤差」のまま渡されてきたのは、誰も自分の目で測り直していなかったからだ。

R2R2Rのような小さな個人プロジェクトでも、いやだからこそ、測れる目を1つずつ作っていく。それが、引き継ぎを引き継ぎのまま信じない、という姿勢の唯一の支えになる。

次にやること

優先度の高い順に：

FKScanner_MotionBased.cs の計算ロジック検証（97mm差の原因特定）
LOG169確定 PICK angles の実機投入テスト（実際に掴めるか）
FKスキャン再実行時の j5 変動軸化検討

「lookup成功」≠「掴める姿勢」という区別が、引き継ぎテンプレートに加わった。次セッション以降、この区別を前提に話せる。

※ プロジェクト名「R2R2R」は、UC Berkeley・Toyota Research Instituteの研究「Real2Render2Real」（Yu, Fu et al., 2025）の概念に触発されています。現実空間をデジタルツイン化し、仮想空間での試行から得た知見を実世界のモーター駆動へ還元する——その設計と実装過程を毎日記録しています。技術アプローチおよび目的は独自のものです。