UnityのFKをコードで再現しようとして、Unityが既にやっていることに気づいた

mechArm 270のグリッド角度テーブル生成に着手した。Unityシミュレーション上でCCD IKを動かし、マット全域のRobot座標→関節角度の対応表をCSVで出力する計画だ。物理IKの発散、FK行列計算の座標系混乱を経て、最終的に「UnityはFKを既に計算している」という当たり前の事実にたどり着いた。

💡 Key Takeaway

Robot XY→Unity座標の変換が確定した（Unity X = Robot Y × 0.001、Unity Z = Robot X × 0.001）。EndEffectorのWorld座標は endEffector.transform.position で直接読める。FKをコードで書く必要はなかった。

最初の実装：物理CCD IK

出発点はUnityのArticulationBodyを直接動かすCCD IKだった。各ジョイントに対して手先からベース方向に向かって回転量を計算し、drive.target に加算する。

private IEnumerator RunCCDIKCoroutine(Vector3 targetPos, int iterations)
{
    float[] lowerLimits = { -160f, -75f, -175f, -155f, -100f, -180f };
    float[] upperLimits = {  160f,  120f,   45f,  155f,  100f,  180f };

    for (int iter = 0; iter < iterations; iter++)
    {
        for (int j = joints.Length - 1; j >= 0; j--)
        {
            Transform jt = joints[j].transform;
            Vector3 toEE     = endEffector.position - jt.position;
            Vector3 toTarget = targetPos            - jt.position;

            float angle = Vector3.SignedAngle(toEE, toTarget, jt.right);

            var drive = joints[j].xDrive;
            drive.target = Mathf.Clamp(drive.target + angle, lowerLimits[j], upperLimits[j]);
            joints[j].xDrive = drive;
            joints[j].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }
        yield return new WaitForFixedUpdate();
    }
}

1フレームで全関節を一気に動かす設計だったが、Vector3.SignedAngle が最大180度を返すため加算が爆発した。アームが画面内で暴れた。1イテレーションあたりの最大回転角を制限した。

float angle = Vector3.SignedAngle(toEE, toTarget, jt.right);
angle = Mathf.Clamp(angle, -5f, 5f);  // 1フレーム最大5度に制限

動きは落ち着いたが、成功率は0/120のまま変わらなかった。原因はArticulationBodyの物理遅延だ。drive.target をセットしても関節は即座に動かない。CCD IKがEndEffectorの古い位置を見ながら計算し続けるため収束しない。

純数学FK + 勾配降下IK

物理シミュレーションを使わず、FKの数式だけでIKを解く方針に切り替えた。SmartTrajectoryController.cs に実装されていた平面近似FKを流用した。

private Vector3 ForwardKinematics(float[] angles)
{
    float a0 = angles[0] * Mathf.Deg2Rad;
    float a1 = angles[1] * Mathf.Deg2Rad;
    // ... 中略
    float reach = L2 * Mathf.Cos(a1)
                + L3 * Mathf.Cos(a1 + a2)
                + L4 * Mathf.Cos(a1 + a2 + a3)
                + L5 * Mathf.Cos(a1 + a2 + a3 + a4);

    float height = L1
                 + L2 * Mathf.Sin(a1)
                 // ...
}

初回の出力：

j0: -0.222  j1: -0.232  j2: -0.160  ...（ほぼゼロ）

勾配降下の学習率0.5が小さすぎた。1イテレーションで関節角度が0.003度しか動かない。200回繰り返しても0.6度しか変化しない計算だ。

j0（ベース回転）はatan2で解析的に求め、j1〜j4の学習率を80に引き上げた。

// j0は解析解
float dx = targetPos.x - robotOriginInUnity.x;
float dz = targetPos.z - robotOriginInUnity.z;
angles[0] = Mathf.Atan2(dx, dz) * Mathf.Rad2Deg;

float lr = 80f;  // 0.5から引き上げ

成功率は11/273（4%）まで改善した。しかし次の問題が見えた。

URDF行列FKの失敗

SmartTrajectoryController.cs のFKは「平面近似」で、j3（ロール）が無視されている。URDFのジョイントデータを取り出して正確な変換行列を組んだ。

grep -A 8 "joint name=" mecharm_270_m5.urdf

取得した6ジョイントのoriginとrpy：

joint1_to_base:   xyz="0 0 0.1"      rpy="0 0 0"
joint2_to_joint1: xyz="0 0 0.038"    rpy="-1.5708 0 0"
joint3_to_joint2: xyz="0 -0.1 0"     rpy="0 0 0"
joint4_to_joint3: xyz="0.108 -0.005 -0.001" rpy="0 1.5708 0"
joint5_to_joint4: xyz="-0.001 0 0"   rpy="0 -1.5708 0"
joint6_to_joint5: xyz="0.06 0 0"     rpy="0 1.5708 0"

RPY変換行列とZ軸回転行列を組み合わせて連鎖積を計算した。

for (int i = 0; i < 6; i++)
{
    float theta = angles[i] * Mathf.Deg2Rad;
    T = T * TransMat(xyz[i]) * RPYMat(rpy[i,0], rpy[i,1], rpy[i,2]) * RotZMat(theta);
}

// URDF Z-up → Unity Y-up
float ux = robotOriginInUnity.x + T[0,3];
float uy = robotOriginInUnity.y + T[2,3];
float uz = robotOriginInUnity.z + T[1,3];

検証のため、LOG137で実機確認済みのPICK角度を入れた。

入力: J0=-46.93, J1=102.74, J2=-43.33, J3=3.6, J4=-60.11, J5=-31.9
FK出力: X=0.1456, Y=0.1262, Z=-0.1606
期待値: X=0.2401, Y=0.024,  Z=0.214

ZがマイナスになってUnityではプラスのはず。URDF Z-up→Unity Y-up の軸変換が根本的に間違っていた。成功率は11→0に悪化した。

根本に立ち返った

「FKをコードで再現しようとして何度も失敗している。でもUnityはFKを毎フレーム正確に計算している」

Unityは物理シミュレーションのために全ジョイントの位置・回転を正確に計算している。その結果が endEffector.transform.position に入っている。それを読めばいい。

FKコードは不要だった。

【今まで】
drive.target → 物理エンジン → 関節が動く → endEffector.position（正確）
                                                    ↑これを無視
     ↓
FKコード → 座標変換 → 計算結果（不正確）← ここで詰まっていた

【これから】
drive.target → 物理エンジン → 関節が動く → endEffector.position（正確）
                                                    ↑これを直接読む

Robot XY → Unity座標変換の確定

座標変換が正しいか検証するため、MotionLogPlayerで既存のPICK動作を再生しながらEndEffector位置を1秒ごとにログ出力した。

private void LogEndEffectorPosition()
{
    if (endEffector != null)
        Debug.Log($"[EE位置] World: X={endEffector.position.x:F4}, " +
                  $"Y={endEffector.position.y:F4}, Z={endEffector.position.z:F4}");
    Invoke(nameof(LogEndEffectorPosition), 1.0f);
}

Step 4: PICK のタイミングで取れた数値：

[EE位置] World: X=0.1609, Y=0.0266, Z=0.1454  ← PICK姿勢
[EE位置] World: X=0.0000, Y=0.2415, Z=0.1681  ← ホーム姿勢

LOG137の実機PICK角度 [-46.93, 102.74, -43.33, 3.6, -60.11, -31.9] がこの位置に届く。RedBlockのUnity座標 (0.2401, 0.024, 0.214) とは若干ズレているが、それはEndEffectorとブロック天面の高さ差だ。

これで座標変換が確定した。

Robot XY(mm) → Unity World座標(m)
Unity X = Robot Y × 0.001
Unity Z = Robot X × 0.001
Unity Y = 0.027（PICK高さ、EE実測値から）

GridAngleScanner v3

FKコードを完全に削除し、endEffector.transform.position を直接読む設計に作り直した。

/// GridAngleScanner v3
/// UnityのFKをそのまま使う。endEffector.transform.positionを直接読む。
/// FKコードは一切持たない。
/// 物理憲法遵守: drive.target のみ更新。stiffness/damping/anchorRotation には触れない。

private IEnumerator ScanRoutine()
{
    for (float rx = robotXMin; rx <= robotXMax + 0.001f; rx += gridStep)
    {
        for (float ry = robotYMin; ry <= robotYMax + 0.001f; ry += gridStep)
        {
            // Robot座標(mm) → Unity World座標(m)
            Vector3 targetPos = new Vector3(
                ry * 0.001f,     // Robot Y → Unity X
                pickHeightUnity, // 固定 0.027m
                rx * 0.001f      // Robot X → Unity Z
            );

            yield return StartCoroutine(MoveToAngles(new float[]{0,0,0,0,0,-30}, 0.5f));
            yield return StartCoroutine(RunCCDIK(targetPos));
            yield return new WaitForSeconds(settleWaitSeconds);

            // UnityのFKをそのまま使う
            float dist = Vector3.Distance(endEffector.position, targetPos);
            bool ikOk = dist < ikSuccessThreshold;

            // jointPositionを記録（drive.targetではなく実測値）
            float[] angles = new float[6];
            for (int i = 0; i < 6; i++)
            {
                if (joints[i].jointPosition.dofCount > 0)
                    angles[i] = joints[i].jointPosition[0] * Mathf.Rad2Deg;
            }
        }
    }
}

Inspectorパラメータ：

Robot Workspace:
  robotXMin = -112.5  (mm)
  robotXMax =  138.1  (mm)
  robotYMin = -202.6  (mm)
  robotYMax =  204.6  (mm)
  gridStep  =   20.0  (mm)  ← 最終的に10mmに下げる予定

Coordinate Transform:
  pickHeightUnity = 0.027  (m)

CCD IK:
  ccdIterations      = 100
  maxAngleStep       = 2.0   (度)
  settleWaitSeconds  = 1.0   (秒)
  ikSuccessThreshold = 0.015 (m)

テスト出力（gridStep=200、6点）：

robot_x_mm,robot_y_mm,j0,j1,j2,j3,j4,j5,ik_success
-112.5,-202.6,-127.997,21.013,32.021,79.996,83.993,-30.000,0
-112.5,-2.6,151.983,-75.024,-64.991,-154.998,-78.012,-30.000,0
87.5,-2.6,144.056,93.008,-4.989,-90.154,-87.733,0.481,0

角度は意味のある値に変化している。ik_success=0が続くのはCCD IKの収束問題だ。これはLOG150で解決する。

GridAngleScanner v3 フルコード

using UnityEngine;
using System.Collections;
using System.IO;

/// <summary>
/// GridAngleScanner v3
/// UnityのFKをそのまま使う。endEffector.transform.positionを直接読む。
/// FKコードは一切持たない。
/// 物理憲法遵守: drive.target のみ更新。stiffness/damping/anchorRotation には触れない。
/// </summary>
public class GridAngleScanner : MonoBehaviour
{
    [Header("Joints (link1〜link6 を Element0〜5 に設定)")]
    public ArticulationBody[] joints;

    [Header("EndEffector")]
    public Transform endEffector;

    [Header("Robot Workspace (mm)")]
    public float robotXMin = -112.5f;
    public float robotXMax =  138.1f;
    public float robotYMin = -202.6f;
    public float robotYMax =  204.6f;
    public float gridStep  =  20.0f;

    [Header("Coordinate Transform")]
    // Robot座標(mm) → Unity World座標(m)
    // Unity X = Robot Y * 0.001
    // Unity Z = Robot X * 0.001
    // Unity Y = pickHeightUnity (固定)
    public float pickHeightUnity = 0.027f;

    [Header("CCD IK")]
    public int   ccdIterations     = 100;
    public float maxAngleStep      = 2.0f;   // 1イテレーション最大回転角（度）
    public float settleWaitSeconds = 1.0f;
    public float ikSuccessThreshold = 0.015f;

    [Header("Debug")]
    public bool autoStartOnPlay = true;

    private float[] lowerLimits = { -160f, -75f, -175f, -155f, -100f, -180f };
    private float[] upperLimits = {  160f,  120f,   45f,  155f,  100f,  180f };

    void Start()
    {
        if (autoStartOnPlay)
            Invoke(nameof(StartScan), 2.0f);

        Invoke(nameof(LogEndEffectorPosition), 5.0f);
    }

    private void LogEndEffectorPosition()
    {
        if (endEffector != null)
            Debug.Log($"[EE位置] World: X={endEffector.position.x:F4}, " +
                      $"Y={endEffector.position.y:F4}, Z={endEffector.position.z:F4}");
        Invoke(nameof(LogEndEffectorPosition), 1.0f);
    }

    public void StartScan()
    {
        StartCoroutine(ScanRoutine());
    }

    private IEnumerator ScanRoutine()
    {
        Debug.Log("[GridScan] 開始（Unity FK使用）");

        string logsDir = Path.Combine(Application.dataPath, "..", "Logs");
        Directory.CreateDirectory(logsDir);
        string csvPath = Path.Combine(logsDir, "grid_angle_table.csv");
        var writer = new StreamWriter(csvPath, false) { AutoFlush = true };
        writer.WriteLine("robot_x_mm,robot_y_mm,j0,j1,j2,j3,j4,j5,ik_success");

        int total = 0, success = 0;

        for (float rx = robotXMin; rx <= robotXMax + 0.001f; rx += gridStep)
        {
            for (float ry = robotYMin; ry <= robotYMax + 0.001f; ry += gridStep)
            {
                // Robot座標(mm) → Unity World座標(m)
                // Robot X(前後) → Unity Z
                // Robot Y(左右) → Unity X
                Vector3 targetPos = new Vector3(
                    ry * 0.001f,     // Robot Y → Unity X
                    pickHeightUnity, // 固定: 0.027m
                    rx * 0.001f      // Robot X → Unity Z
                );

                // ホームに退避
                yield return StartCoroutine(MoveToAngles(
                    new float[]{0, 0, 0, 0, 0, -30}, 0.5f));

                // CCD IK（物理ベース）
                yield return StartCoroutine(RunCCDIK(targetPos));

                // 物理安定待ち
                yield return new WaitForSeconds(settleWaitSeconds);

                // EndEffectorの実際位置で成功判定（UnityのFKをそのまま使う）
                float dist = endEffector != null
                    ? Vector3.Distance(endEffector.position, targetPos)
                    : float.MaxValue;
                bool ikOk = dist < ikSuccessThreshold;

                // 実際のjointPositionを記録（drive.targetではなく実測値）
                float[] angles = new float[6];
                for (int i = 0; i < 6; i++)
                {
                    if (joints[i] != null && joints[i].jointPosition.dofCount > 0)
                        angles[i] = joints[i].jointPosition[0] * Mathf.Rad2Deg;
                }

                writer.WriteLine(
                    $"{rx:F1},{ry:F1}," +
                    $"{angles[0]:F3},{angles[1]:F3},{angles[2]:F3}," +
                    $"{angles[3]:F3},{angles[4]:F3},{angles[5]:F3}," +
                    $"{(ikOk ? 1 : 0)}"
                );

                total++;
                if (ikOk) success++;

                if (total % 10 == 0)
                    Debug.Log($"[GridScan] {total}点処理済み / 成功率: {success}/{total}");
            }
        }

        writer.Close();
        Debug.Log($"[GridScan] 完了。合計{total}点、成功{success}点 → {csvPath}");

#if UNITY_EDITOR
        UnityEditor.EditorApplication.isPlaying = false;
#endif
    }

    private IEnumerator RunCCDIK(Vector3 targetPos)
    {
        if (joints == null || endEffector == null) yield break;

        for (int iter = 0; iter < ccdIterations; iter++)
        {
            // 収束チェック（UnityのFKを直接使用）
            if (Vector3.Distance(endEffector.position, targetPos) < ikSuccessThreshold * 0.5f)
                yield break;

            for (int j = joints.Length - 1; j >= 0; j--)
            {
                if (joints[j] == null) continue;

                Transform jt = joints[j].transform;
                Vector3 toEE     = endEffector.position - jt.position;
                Vector3 toTarget = targetPos            - jt.position;

                if (toEE.sqrMagnitude < 1e-8f || toTarget.sqrMagnitude < 1e-8f) continue;

                float angle = Vector3.SignedAngle(toEE, toTarget, jt.right);
                angle = Mathf.Clamp(angle, -maxAngleStep, maxAngleStep);  // 発散防止

                var drive = joints[j].xDrive;
                drive.target = Mathf.Clamp(
                    drive.target + angle,
                    lowerLimits[j],
                    upperLimits[j]
                );
                joints[j].xDrive = drive;
                joints[j].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
            }

            yield return new WaitForFixedUpdate();
        }
    }

    private IEnumerator MoveToAngles(float[] targetAngles, float waitSeconds)
    {
        if (joints == null) yield break;
        for (int i = 0; i < joints.Length && i < 6; i++)
        {
            if (joints[i] == null) continue;
            var drive = joints[i].xDrive;
            drive.target = targetAngles[i];
            joints[i].xDrive = drive;
            joints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }
        yield return new WaitForSeconds(waitSeconds);
    }
}

今日わかったこと

FKを実装しようとして4回失敗した。物理CCD IKの発散、学習率の問題、URDF行列計算の座標系ミス、そして軸変換の方向違い。

正解はシンプルだった。Unityがすでに正確なFKを計算している。endEffector.transform.position を読むだけでいい。FKをコードで再現しようとすることは、Unityが毎フレームやっていることを外側から近似しようとする行為だった。

座標変換の数値は確定した。次はCCD IKの収束問題を解いてグリッドスキャンを完走させる。

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