LOG150R2R2R2026/4/16

CCD IKを捨てて、確認済み姿勢からFKで空間を埋めた

CCD IKの収束問題を解こうとして何度も失敗した。anchorRotation補正、SetJointPositions導入、1ジョイントずつ更新——全部試してダメだった。方針を根本から変えた。IKを使わず、LOG137で確認済みのPICK姿勢を起点に関節角度をグリッドサーチしてFKで記録する。20体のデジタルツインを並列稼働させ、481点のlookup tableを生成した。

💡 Key Takeaway

IKを解こうとするのをやめた。確認済みの姿勢を起点に関節角度をズラし、EndEffectorの実座標をUnityのFKで記録する。IKの収束問題は存在しない。20体並列シミュレーションで481点を一気に取得した。

やったこと	起きたこと	判断
anchorRotation補正でCCD IK修正	成功率0継続、根本解決せず	物理遅延との構造的相性問題
SetJointPositions導入	ぐにゃぐにゃの動き→発振	maxAngleStep上げすぎで悪化
1ジョイントずつSetJointPositions更新	動きは改善、成功率0継続	CCD IK自体を捨てる判断
ikpyで解析IK試み	座標系がズレて全角度が実機と不一致	ikpyはこの用途に使えない
確認済みPICK角度起点のFKスキャンに切り替え	481点のlookup tableを20体並列で生成	方針確定
GridAngleScannerにMotionLogモード追加	基準軌跡のEE位置を記録	LOG149実測値と一致

CCD IKとの最後の戦い

持ち越したCCD IKの収束問題に着手した。

原因の仮説は正しかった

jt.right（ジョイントのローカルX軸）が回転軸として使われていたが、ArticulationBodyのxDriveはアンカーフレームのX軸を使う。link1 のInspectorを確認したところ Anchor Rotation Z=270° が全ジョイントに乗っていた。

// 修正前：ジョイントのTransformX軸（間違い）
float angle = Vector3.SignedAngle(toEE, toTarget, jt.right);

// 修正後：アンカーフレームのX軸（正しい）
Quaternion anchorWorldRot = jt.rotation * joints[j].anchorRotation;
Vector3 rotationAxis = anchorWorldRot * Vector3.right;
float angle = Vector3.SignedAngle(toEE, toTarget, rotationAxis);

anchorRotation は読み取りのみ。書き込み禁止の物理憲法は遵守している。しかし成功率0は変わらなかった。

本当の原因：ArticulationBodyの物理遅延

CCD IKが期待している動作:
drive.target を更新 → 関節が即座に動く → EE位置が更新される

実際の動作:
drive.target を更新 → Spring/Dampingで徐々に動く
                    → WaitForFixedUpdate（0.02秒）
                    → EE位置はまだ古い値
                    → CCD IKが古い位置を見てまた加算
                    → 100イテレーション × 0.02秒 = 2秒間、誤った方向に力を加え続ける

SetJointPositionsで物理をバイパス

ArticulationBody.SetJointPositions() + Physics.SyncTransforms() で物理エンジンを完全スキップして角度を瞬時にセットできる。1ジョイントずつ逐次更新する SolveCCDIK：

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

    float[] anglesDeg = new float[6];
    for (int i = 0; i < 6; i++)
        anglesDeg[i] = joints[i] != null ? joints[i].xDrive.target : 0f;

    for (int iter = 0; iter < ccdIterations; iter++)
    {
        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;

            Quaternion anchorWorldRot = jt.rotation * joints[j].anchorRotation;
            Vector3 rotationAxis = anchorWorldRot * Vector3.right;
            float angle = Vector3.SignedAngle(toEE, toTarget, rotationAxis);
            angle = Mathf.Clamp(angle, -maxAngleStep, maxAngleStep);

            anglesDeg[j] = Mathf.Clamp(
                anglesDeg[j] + angle,
                lowerLimits[j], upperLimits[j]);

            // 1ジョイント動かすたびに即座に反映
            var positions = new List<float>();
            for (int i = 0; i < 6; i++)
                positions.Add(anglesDeg[i] * Mathf.Deg2Rad);
            rootArticulationBody.SetJointPositions(positions);
            Physics.SyncTransforms();
        }

        yield return null;
    }

    // drive.target を最終角度に同期（settleWait中に物理が戻すのを防ぐ）
    for (int i = 0; i < 6; i++)
    {
        if (joints[i] == null) continue;
        var drive = joints[i].xDrive;
        drive.target = anglesDeg[i];
        joints[i].xDrive = drive;
        joints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
    }
}

動きは安定した。しかし成功率は0のまま。

CCD IKと戦い続けた。6DOF + 関節制限付きの空間でのCCDは局所最適解に陥りやすい。mechArmの特殊なリンク構造（Z=270°オフセット）が回転軸計算を複雑にしている。どれだけ実装を改善しても、この構造的な問題は残る。CCD IKを捨てた。

ikpyによる解析IK、そして失敗

generate_grid_angles.py：

import ikpy.chain
import numpy as np
import csv

URDF_PATH = "/Users/shimazueiichi/Robotics_UX/Robotics_UX_New/Assets/mecharm_270_m5/mycobot_description/urdf/mecharm_270_m5/mecharm_270_m5.urdf"

ROBOT_X_MIN, ROBOT_X_MAX = -112.5, 138.1
ROBOT_Y_MIN, ROBOT_Y_MAX = -202.6, 204.6
GRID_STEP = 10.0
PICK_HEIGHT = 0.027  # m

chain = ikpy.chain.Chain.from_urdf_file(
    URDF_PATH,
    base_elements=["base"]
)

output_path = "/Users/shimazueiichi/r2r2r-scripts/grid_angles.csv"
with open(output_path, "w", newline="") as f:
    writer = csv.writer(f)
    writer.writerow(["robot_x_mm","robot_y_mm","j0","j1","j2","j3","j4","j5","ik_success"])

    rx = ROBOT_X_MIN
    while rx <= ROBOT_X_MAX + 0.001:
        ry = ROBOT_Y_MIN
        while ry <= ROBOT_Y_MAX + 0.001:
            target = [ry * 0.001, PICK_HEIGHT, rx * 0.001]
            try:
                angles = chain.inverse_kinematics(target)
                j = angles[1:7]
                j_deg = [round(np.degrees(a), 3) for a in j]
                writer.writerow([rx, ry] + j_deg + [1])
            except Exception:
                writer.writerow([rx, ry, 0, 0, 0, 0, 0, 0, 0])
            ry += GRID_STEP
        rx += GRID_STEP

print(f"完了 → {output_path}")

1,066点が生成され、全点 ik_success=1 だった。しかし実機確認済みの角度と全関節で大きくズレていた：

実機確認済み (X≈138.1, Y≈-172.9):
j0=-46.93,  j1=102.74, j2=-43.33, j3=3.6,   j4=-60.11, j5=-31.9

ikpy出力 (X=137.5, Y=-172.6):
j0=151.619, j1=45.538,  j2=-4.43,  j3=92.127, j4=81.671, j5=0.0

j1の分布を確認したところ：

277点 → j1=119.995°（上限120°に張り付き）

277点／1,066点（約26%）が上限値に張り付いていた。ikpyはこの用途に使えないと判断した。

方針転換：確認済み姿勢からFKで空間を埋める

「今の正しい値から少しだけズラしたデータをまずはたくさん作ればいいんじゃないの？」

確認済みPICK角度: [-46.93, 102.74, -43.33, 3.6, -60.11, -31.9]
↓
j0とj1を起点から少しずつズラす
↓
各角度でのEndEffector位置をUnityのFKで記録
↓
「角度→座標」の対応表ができる

IKを解く必要がない。UnityはFKを毎フレーム正確に計算している。endEffector.transform.position を読むだけでいい。

ParallelFKScanner.cs フルコード

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

/// <summary>
/// ParallelFKScanner
/// 確認済みPICK角度を起点にj0とj1をグリッドサーチし、
/// 各点のEndEffector位置をFKで記録するスキャナー。IKは一切使わない。
/// 20体のロボットを並列稼働させ、約25点/体を分担処理する。
/// 物理憲法遵守: drive.target のみ更新。stiffness/damping/anchorRotation には触れない。
/// </summary>
public class ParallelFKScanner : MonoBehaviour
{
    // ─── 起点角度（確認済みPICK姿勢） ───────────────────────────
    private static readonly float[] BaseAngles = { -46.93f, 102.74f, -43.33f, 3.6f, -60.11f, -30f };

    // ─── グリッド定義 ────────────────────────────────────────────
    // j0: -90〜+90°、5°刻み → 37点
    private const float J0Min  = -90f;
    private const float J0Max  =  90f;
    private const float J0Step =   5f;

    // j1: 起点102.74からの相対 -30〜+30°、5°刻み → 13点
    private const float J1RelMin  = -30f;
    private const float J1RelMax  =  30f;
    private const float J1RelStep =   5f;

    // ─── ロボット数 ──────────────────────────────────────────────
    private const int RobotCount = 20;

    // ─── Prefab ──────────────────────────────────────────────────
    [Header("Prefab")]
    public GameObject mecharmPrefab;

    // ─── ロボット1体分の設定 ─────────────────────────────────────
    [System.Serializable]
    public class RobotConfig
    {
        public ArticulationBody rootArticulationBody; // base
        public ArticulationBody[] joints;             // link1〜link6 (Element0〜5)
        public Transform endEffector;
    }

    [Header("Robot Configs (20体分)")]
    public RobotConfig[] robots = new RobotConfig[RobotCount];

    // ─── 内部状態 ─────────────────────────────────────────────────
    private List<string[]> _allRows = new List<string[]>();
    private int _doneCount = 0;

    // ─────────────────────────────────────────────────────────────
    void Start()
    {
        var grid = BuildGrid();
        Debug.Log($"[ParallelFKScanner] グリッド総点数: {grid.Count}、ロボット数: {RobotCount}");

        // Scene上のmecharmを名前順で取得
        robots = new RobotConfig[RobotCount];
        var foundRobots = new List<GameObject>();
        for (int r = 1; r <= RobotCount; r++)
        {
            var go = GameObject.Find($"mecharm270mpi_{r}_0");
            if (go != null) foundRobots.Add(go);
            else Debug.LogWarning($"[ParallelFKScanner] mecharm270mpi_{r}_0 が見つかりません");
        }

        for (int r = 0; r < foundRobots.Count; r++)
        {
            var instance = foundRobots[r];
            var cfg = new RobotConfig();

            // rootArticulationBody（base）を取得
            foreach (var ab in instance.GetComponentsInChildren<ArticulationBody>())
            {
                if (ab.isRoot) { cfg.rootArticulationBody = ab; break; }
            }

            // link1〜link6を順番に取得
            var jointList = new List<ArticulationBody>();
            foreach (var ab in instance.GetComponentsInChildren<ArticulationBody>())
                if (!ab.isRoot) jointList.Add(ab);
            cfg.joints = jointList.GetRange(0, Mathf.Min(6, jointList.Count)).ToArray();

            // EndEffectorを取得（link6の末端）
            var eeTransform = instance.transform.Find("base/link1/link2/link3/link4/link5/link6/link6_flange");
            cfg.endEffector = eeTransform != null
                ? eeTransform
                : cfg.joints[cfg.joints.Length - 1].transform;

            robots[r] = cfg;
            Debug.Log($"[ParallelFKScanner] Robot[{r}] = {instance.name} / root={cfg.rootArticulationBody?.name} / joints={cfg.joints.Length} / ee={cfg.endEffector?.name}");
        }

        int chunkSize = Mathf.CeilToInt((float)grid.Count / RobotCount);
        for (int r = 0; r < RobotCount; r++)
        {
            int start = r * chunkSize;
            if (start >= grid.Count) break;
            int end = Mathf.Min(start + chunkSize, grid.Count);
            var chunk = grid.GetRange(start, end - start);
            StartCoroutine(ScanRoutine(r, chunk));
        }
    }

    // ─── グリッド生成 ─────────────────────────────────────────────
    private List<(float j0, float j1)> BuildGrid()
    {
        var list = new List<(float, float)>();
        for (float j0 = J0Min; j0 <= J0Max + 0.001f; j0 += J0Step)
        {
            for (float j1rel = J1RelMin; j1rel <= J1RelMax + 0.001f; j1rel += J1RelStep)
            {
                list.Add((j0, BaseAngles[1] + j1rel));
            }
        }
        return list;
    }

    // ─── 1ロボット分のスキャンルーティン ─────────────────────────
    private IEnumerator ScanRoutine(int robotIndex, List<(float j0, float j1)> chunk)
    {
        var cfg = robots[robotIndex];
        if (cfg == null || cfg.rootArticulationBody == null || cfg.joints == null || cfg.endEffector == null)
        {
            Debug.LogWarning($"[ParallelFKScanner] Robot[{robotIndex}] の設定が不完全です。スキップします。");
            OnRobotDone();
            yield break;
        }

        var localRows = new List<string[]>();

        foreach (var (j0, j1) in chunk)
        {
            float[] angles = new float[6]
            {
                j0,
                j1,
                BaseAngles[2],  // -43.33（固定）
                BaseAngles[3],  //   3.6（固定）
                BaseAngles[4],  // -60.11（固定）
                BaseAngles[5]   // -30（固定）
            };

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

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

            yield return new WaitForSeconds(0.5f);

            // EE位置をルートからの相対座標で記録
            // ※ ワールド座標ではなく相対座標を使う（20体が異なる位置に配置されているため）
            Vector3 ee = cfg.endEffector.position - cfg.rootArticulationBody.transform.position;
            localRows.Add(new string[]
            {
                ee.x.ToString("F6"),
                ee.y.ToString("F6"),
                ee.z.ToString("F6"),
                angles[0].ToString("F3"),
                angles[1].ToString("F3"),
                angles[2].ToString("F3"),
                angles[3].ToString("F3"),
                angles[4].ToString("F3"),
                angles[5].ToString("F3")
            });
        }

        lock (_allRows)
        {
            _allRows.AddRange(localRows);
        }

        Debug.Log($"[ParallelFKScanner] Robot[{robotIndex}] 完了 ({chunk.Count}点)");
        OnRobotDone();
    }

    // ─── 全ロボット完了チェック ───────────────────────────────────
    private void OnRobotDone()
    {
        _doneCount++;
        if (_doneCount >= RobotCount)
            StartCoroutine(WriteCSVAndQuit());
    }

    // ─── CSV 統合出力 ─────────────────────────────────────────────
    private IEnumerator WriteCSVAndQuit()
    {
        yield return null;

        string logsDir = Path.Combine(Application.dataPath, "..", "Logs");
        Directory.CreateDirectory(logsDir);
        string csvPath = Path.Combine(logsDir, "fk_scan_table.csv");

        using (var writer = new StreamWriter(csvPath, false) { AutoFlush = true })
        {
            writer.WriteLine("ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5");
            lock (_allRows)
            {
                foreach (var row in _allRows)
                    writer.WriteLine(string.Join(",", row));
            }
        }

        Debug.Log($"[ParallelFKScanner] 完了。{_allRows.Count}点 → {csvPath}");

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

GridAngleScanner.cs フルコード（v4 + MotionLogモード）

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

/// <summary>
/// GridAngleScanner v4
/// UnityのFKをそのまま使う。endEffector.transform.positionを直接読む。
/// FKコードは一切持たない。
/// 物理憲法遵守: drive.target のみ更新。stiffness/damping/anchorRotation には触れない。
/// SetJointPositions で瞬時テレポート（物理バイパス）を追加。
/// useMotionLog=true のとき motion_log_block.csv を再生してEE位置を記録する。
/// </summary>
public class GridAngleScanner : MonoBehaviour
{
    [Header("Root ArticulationBody")]
    public ArticulationBody rootArticulationBody;

    [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;
    public float settleWaitSeconds = 1.0f;
    public float ikSuccessThreshold = 0.015f;

    [Header("Motion Log CSV")]
    public string motionLogCsvPath = "motion_log_block.csv";
    public bool useMotionLog = false;

    [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)
        {
            if (useMotionLog)
                Invoke(nameof(StartMotionLogScan), 2.0f);
            else
                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());
    }

    public void StartMotionLogScan()
    {
        StartCoroutine(MotionLogScanRoutine());
    }

    // ─── MotionLog再生モード ──────────────────────────────────────
    private IEnumerator MotionLogScanRoutine()
    {
        // 絶対パスならそのまま、相対パスならApplication.dataPathを基点に
        string csvPath = Path.IsPathRooted(motionLogCsvPath)
            ? motionLogCsvPath
            : Path.Combine(Application.dataPath, motionLogCsvPath);

        if (!File.Exists(csvPath))
        {
            Debug.LogError($"[MotionLogScan] CSVが見つかりません: {csvPath}");
            yield break;
        }

        Debug.Log("[MotionLogScan] 開始");
        string logsDir = Path.Combine(Application.dataPath, "..", "Logs");
        Directory.CreateDirectory(logsDir);
        string outPath = Path.Combine(logsDir, "motion_log_fk.csv");
        var writer = new StreamWriter(outPath, false) { AutoFlush = true };
        writer.WriteLine("ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5");

        // motion_log_block.csvのカラム構造:
        // cols[0]=timestamp, cols[1]=label, cols[2..7]=J1..J6
        var lines = File.ReadAllLines(csvPath);
        foreach (var line in lines)
        {
            if (line.StartsWith("timestamp") || string.IsNullOrWhiteSpace(line)) continue;
            var cols = line.Split(',');
            if (cols.Length < 8) continue;

            float[] angles = new float[6];
            for (int i = 0; i < 6; i++)
                float.TryParse(cols[i + 2], out angles[i]);

            var positions = new List<float>();
            for (int i = 0; i < 6; i++)
                positions.Add(angles[i] * Mathf.Deg2Rad);
            rootArticulationBody.SetJointPositions(positions);
            Physics.SyncTransforms();

            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);
            }

            yield return new WaitForSeconds(settleWaitSeconds);

            Vector3 ee = endEffector.position;
            writer.WriteLine(
                $"{ee.x:F6},{ee.y:F6},{ee.z:F6}," +
                $"{angles[0]:F3},{angles[1]:F3},{angles[2]:F3}," +
                $"{angles[3]:F3},{angles[4]:F3},{angles[5]:F3}");
        }

        writer.Close();
        Debug.Log($"[MotionLogScan] 完了 → {outPath}");
    }

    // ─── グリッドスキャンモード（CCD IK） ────────────────────────
    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)
            {
                Vector3 targetPos = new Vector3(
                    ry * 0.001f,
                    pickHeightUnity,
                    rx * 0.001f
                );

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

                float dist = endEffector != null
                    ? Vector3.Distance(endEffector.position, targetPos)
                    : float.MaxValue;
                bool ikOk = dist < ikSuccessThreshold;

                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
    }

    // ─── CCD IK（SetJointPositions + 1ジョイント逐次更新） ────────
    private IEnumerator SolveCCDIK(Vector3 targetPos)
    {
        if (joints == null || endEffector == null || rootArticulationBody == null) yield break;

        float[] anglesDeg = new float[6];
        for (int i = 0; i < 6; i++)
            anglesDeg[i] = joints[i] != null ? joints[i].xDrive.target : 0f;

        for (int iter = 0; iter < ccdIterations; iter++)
        {
            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;

                Quaternion anchorWorldRot = jt.rotation * joints[j].anchorRotation;
                Vector3 rotationAxis = anchorWorldRot * Vector3.right;
                float angle = Vector3.SignedAngle(toEE, toTarget, rotationAxis);
                angle = Mathf.Clamp(angle, -maxAngleStep, maxAngleStep);

                anglesDeg[j] = Mathf.Clamp(
                    anglesDeg[j] + angle,
                    lowerLimits[j], upperLimits[j]);

                var positions = new List<float>();
                for (int i = 0; i < 6; i++)
                    positions.Add(anglesDeg[i] * Mathf.Deg2Rad);
                rootArticulationBody.SetJointPositions(positions);
                Physics.SyncTransforms();
            }

            yield return null;
        }

        for (int i = 0; i < 6; i++)
        {
            if (joints[i] == null) continue;
            var drive = joints[i].xDrive;
            drive.target = anglesDeg[i];
            joints[i].xDrive = drive;
            joints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }
    }

    // ─── ホームへの移動 ───────────────────────────────────────────
    private IEnumerator MoveToAngles(float[] targetAngles, float waitSeconds)
    {
        if (joints == null) yield break;
        var positions = new List<float>();
        for (int i = 0; i < 6; i++)
            positions.Add(targetAngles[i] * Mathf.Deg2Rad);
        rootArticulationBody.SetJointPositions(positions);
        Physics.SyncTransforms();
        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);
    }
}

EE位置のワールド座標 vs 相対座標問題

最初のCSVで ee_x = 27.87m という異常値が出た。原因は20体をX方向に2m間隔で配置したため、Robot[14]のワールドX座標が28mになっていたことだ。

// 変更前：ワールド座標（間違い）
Vector3 ee = cfg.endEffector.position;

// 変更後：ルートからの相対座標（正しい）
Vector3 ee = cfg.endEffector.position - cfg.rootArticulationBody.transform.position;

修正後：

ee_x範囲: -0.244 〜 +0.244 m  ← mechArmのリーチ（360mm）と一致
ee_z範囲: -0.031 〜 +0.244 m

出力データ

fk_scan_table.csv 先頭5行：

ee_x,ee_y,ee_z,j0,j1,j2,j3,j4,j5
-0.205389,0.021413,0.006756,90.000,107.740,-43.330,3.600,-60.110,-30.000
-0.193224,0.017523,0.002474,90.000,112.740,-43.330,3.600,-60.110,-30.000
-0.182392,-0.013349,0.035686,90.000,117.740,-43.330,3.600,-60.110,-30.000
-0.151460,-0.033341,-0.030841,90.000,122.740,-43.330,3.600,-60.110,-30.000

motion_log_fk.csvとLOG149実測値の照合：

PICK:  DAY149実測 X=0.1609  / DAY150取得 X=0.1604  → 差分 0.001m
PLACE: DAY149実測 X=-0.1363 / DAY150取得 X=-0.1361 → 差分 0.000m

わかったこと

CCD IKは失敗し続けた。正解は「IKを解くのをやめる」だった。

確認済みの姿勢があれば、そこを起点にFKで空間をサンプリングできる。UnityはFKを毎フレーム正確に計算している。それを読むだけでいい。IKは逆問題だ。FKは順問題だ。順問題に失敗する理由はない。

Motion Log Playerの8ステップ全体を起点に各姿勢周辺をサンプリングして、より多様な姿勢群をカバーするデータを取得する。

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