LOG133R2R2R2026/4/9

Unityと実機mechArm 270の動作時間が一致した。シミュレーション49秒、実機49秒。

実機の動作時間とUnityの再生時間が一致した。そしてグリッパーを、シミュレーターに取り付けようとした一日。

💡 Key Takeaway

「Real → Renderの時間軸が揃った」。実機で49秒かかった動作が、Unityでも49秒で再生される。タイムスタンプ駆動のLerp補間が、それを実現した。グリッパー組み立ては道半ば。でも今日の主役は時間軸の一致だった。

やったこと	起きたこと	判断
MotionLogPlayerをタイムスタンプ駆動に修正した	固定interval再生から実測時間差分再生に変わった	Clineで1本修正するだけで解決
ポーズ間をLerp補間に変更した	瞬間移動が消えて実機に近い動きになった	座標値は正しい、あとは速度だけの問題だった
実機とUnityの再生時間を比較した	49秒と49秒で完全一致した	時間軸が揃った
グリッパーDAEを公式GitHubから取得した	mechArm 270 Pi用のモデルが存在した	adaptive_gripper フォルダに7ファイル
GripperBuilderで自動組み立てを試みた	座標変換とDictionaryの順序問題でズレた	最終的に手動で仮組み完了

時間軸を合わせる

— 「速すぎる」という感覚から始まった。

LOG132でUnityに実機の動きを再生できるようになった。でも、ポーズとポーズの間が瞬間移動していた。CSVにはタイムスタンプが記録されているのに、再生は固定の stepInterval = 1.5f で動いていた。

修正の方向は明確だった。タイムスタンプの差分を計算して、その秒数かけてLerp補間する。

MotionLogPlayer.cs を2段階で修正した。

Step 1: タイムスタンプ駆動

if (DateTime.TryParseExact(cols[0], "yyyy-MM-dd HH:mm:ss.ffffff", ...))
{
    _times.Add(timestamp);
}

// 次のステップまでの待機時間をタイムスタンプ差分で計算
double timeDiff = (_times[i + 1] - _times[i]).TotalSeconds;
if (timeDiff > 0) waitTime = (float)timeDiff;

Step 2: Lerp補間

IEnumerator LerpToNextFrame(float[] fromAngles, float[] toAngles, float duration)
{
    float elapsed = 0f;
    while (elapsed < duration)
    {
        elapsed += Time.deltaTime;
        float t = Mathf.Clamp01(elapsed / duration);
        for (int j = 0; j < joints.Length; j++)
        {
            var drive = joints[j].xDrive;
            drive.target = Mathf.Lerp(fromAngles[j], toAngles[j], t);
            joints[j].xDrive = drive;
        }
        yield return null;
    }
    SetJointAngles(toAngles);
}

Play Motionを実行した。Consoleのタイムスタンプを確認した。

[16:41:17] 再生開始
[16:41:17] Step 1: HOME
[16:41:21] Step 2: GRIPPER_OPEN     → 4秒後
[16:41:25] Step 3: APPROACH_PICK    → 4秒後
[16:41:30] Step 4: PICK             → 5秒後
...
[16:42:06] Step 11: HOME_RETURN
[16:42:06] 再生完了

実機CSVの開始は 14:55:41、終了は 14:56:30。49秒。

Unity再生の開始は 16:41:17、終了は 16:42:06。49秒。

完全一致した。

グリッパーを取り付ける

— シミュレーターにグリッパーがない、という事実に気づいた。

Real → Renderの時間軸が揃ったとき、次の問題が見えた。Unityのロボットにグリッパーがない。実機にはAdaptive Gripperが取り付いているのに、シミュレーターにはlink6の先端がむき出しのまま。

3Dモデルを探した。

まず github.com/elephantrobotics/mecharm → 404。次に mycobot_ros リポジトリの mecharm フォルダ → meshesなし。

mycobot_description/urdf/ まで掘り下げたところで見つかった。

urdf/adaptive_gripper/
├── gripper_base.dae
├── gripper_left1.dae
├── gripper_left2.dae
├── gripper_left3.dae
├── gripper_right1.dae
├── gripper_right2.dae
├── gripper_right3.dae
└── mycobot_adaptive_gripper.urdf

sparse-checkoutで必要なフォルダだけ取得した。

git clone --depth 1 --filter=blob:none --sparse \
  https://github.com/elephantrobotics/mycobot_ros.git
cd mycobot_ros
git sparse-checkout set mycobot_description/urdf/adaptive_gripper

DAEファイルをUnityにコピーして、gripper_base.dae をSceneにドラッグした。実機のグリッパーと同じ形状だった。

GripperBuilderを実装した

— URDFから座標を読んで自動で組み立てる。

手動でパーツを配置するより、URDFの joint 定義から座標を読んで自動組み立てするほうが再現性が高い。Assets/Editor/GripperBuilder.cs をClineで実装した。Unityのメニューバーに「R2R2R → Build Gripper」が追加される。

[MenuItem("R2R2R/Build Gripper")]
public static void BuildGripper()
{
    GameObject gripperBase = GameObject.Find("gripper_base");
    if (gripperBase == null) { Debug.LogError("gripper_base not found"); return; }

    Dictionary<string, JointData> jointData = ParseURDF();
    BuildGripperHierarchy(gripperBase, jointData);
    Debug.Log("Gripper built successfully!");
}

URDFを読んでjointのxyz・rpyを取得し、ROS→Unity座標変換をかける。

// ROS -> Unity 座標変換
// Unity.x = ROS.x
// Unity.y = ROS.z
// Unity.z = ROS.y
Vector3 rosPos = data.position;
Vector3 unityPos = new Vector3(rosPos.x, rosPos.z, rosPos.y);

URDFのjoint構造を読み解くと、階層はこうなっていた。

gripper_base
├── gripper_left3
│   └── gripper_left1
├── gripper_left2
├── gripper_right3
│   └── gripper_right1
└── gripper_right2

ROSとUnityの座標系変換（右手系 → 左手系）も実装して実行した。形は出たが位置がずれた。

原因は2つあった。まずDictionaryは挿入順序を保証しないため、gripper_left1（gripper_left3の子）が親より先に作られた。リスト方式に変更して解決した。

// Dictionaryをやめてリスト方式に変更（作成順序を明示的に保証）
List<GripperPartData> gripperParts = new List<GripperPartData>
{
    new GripperPartData { name="gripper_left3",  parentName="gripper_base",  ... },
    new GripperPartData { name="gripper_left1",  parentName="gripper_left3", ... }, // 親より後に作成
    new GripperPartData { name="gripper_left2",  parentName="gripper_base",  ... },
    new GripperPartData { name="gripper_right3", parentName="gripper_base",  ... },
    new GripperPartData { name="gripper_right1", parentName="gripper_right3",...  },
    new GripperPartData { name="gripper_right2", parentName="gripper_base",  ... },
};

次に gripper_base の Rotation（X:90, Y:90）がローカル座標変換に影響していた。座標変換ルールを何度か調整したが、今日中に完全な一致は取れなかった。最終的に手動で各パーツの位置を目視確認しながら仮組みした。

GripperBuilder.cs 最終版

座標値をハードコードした最終版がこれだ。

#if UNITY_EDITOR
using UnityEngine;
using UnityEditor;
using UnityEditor.SceneManagement;
using System.Collections.Generic;

public class GripperBuilder
{
    struct GripperPartData
    {
        public string name;
        public Vector3 localPosition;
        public Vector3 localRotation;
        public string parentName;
    }

    [MenuItem("R2R2R/Build Gripper")]
    public static void BuildGripper()
    {
        GameObject gripperBase = GameObject.Find("gripper_base");
        if (gripperBase == null) { Debug.LogError("❌ gripper_base not found!"); return; }
        BuildGripperHierarchy(gripperBase);
        Debug.Log("✅ Gripper built successfully!");
    }

    static void BuildGripperHierarchy(GameObject gripperBase)
    {
        // 既存の gripper_* 子オブジェクトを削除
        List<Transform> childrenToRemove = new List<Transform>();
        foreach (Transform child in gripperBase.transform)
            if (child.name.StartsWith("gripper_")) childrenToRemove.Add(child);
        foreach (Transform child in childrenToRemove)
            Object.DestroyImmediate(child.gameObject);

        // 作成順序を明示的に定義（親より先に子を作らない）
        List<GripperPartData> gripperParts = new List<GripperPartData>
        {
            new GripperPartData { name="gripper_left3",
                localPosition=new Vector3(0.0361f,0f,0.0042f),
                localRotation=new Vector3(0f,0f,0f), parentName="gripper_base" },
            new GripperPartData { name="gripper_left1",
                localPosition=new Vector3(-0.0394f,0.0075f,-0.0039f),
                localRotation=new Vector3(0f,0f,180f), parentName="gripper_left3" },
            new GripperPartData { name="gripper_left2",
                localPosition=new Vector3(0.025f,0f,0.027f),
                localRotation=new Vector3(0f,0f,0f), parentName="gripper_base" },
            new GripperPartData { name="gripper_right3",
                localPosition=new Vector3(0.08f,0f,0.005f),
                localRotation=new Vector3(0f,0f,180f), parentName="gripper_base" },
            new GripperPartData { name="gripper_right1",
                localPosition=new Vector3(0.0784f,-0.0074f,-0.0039f),
                localRotation=new Vector3(0f,0f,0f), parentName="gripper_right3" },
            new GripperPartData { name="gripper_right2",
                localPosition=new Vector3(0.005f,0f,0.027f),
                localRotation=new Vector3(0f,0f,0f), parentName="gripper_base" },
        };

        Dictionary<string, GameObject> createdParts = new Dictionary<string, GameObject>();
        createdParts["gripper_base"] = gripperBase;

        foreach (var partData in gripperParts)
        {
            GameObject parent = createdParts.ContainsKey(partData.parentName)
                ? createdParts[partData.parentName] : gripperBase;
            GameObject part = new GameObject(partData.name);
            part.transform.SetParent(parent.transform, false);
            part.transform.localPosition = partData.localPosition;
            part.transform.localRotation = Quaternion.Euler(partData.localRotation);

            string meshPath = $"Assets/Models/gripper/{partData.name}.dae";
            GameObject meshPrefab = AssetDatabase.LoadAssetAtPath<GameObject>(meshPath);
            if (meshPrefab != null)
            {
                GameObject meshInstance = Object.Instantiate(meshPrefab, part.transform);
                meshInstance.name = partData.name + "_mesh";
                meshInstance.transform.localPosition = Vector3.zero;
                meshInstance.transform.localRotation = Quaternion.identity;
            }
            createdParts[partData.name] = part;
        }
        EditorSceneManager.MarkSceneDirty(EditorSceneManager.GetActiveScene());
    }
}
#endif

座標値は手動仮組みで確認した実測値。URDFのjoint定義から読んだ値ではなく、Unityのローカル座標系で直接確認した数値をそのまま使っている。LOG134で正確な値に詰める。

グリッパー、仮組み完了

GripperBuilderの座標変換が安定しなかったため、最終的に手動で各パーツの位置を調整して仮組みした。

gripper_base（link6の子、Rotation X:90 Y:90）
├── gripper_left3
│   └── gripper_left1
├── gripper_left2
├── gripper_right3
│   └── gripper_right1
└── gripper_right2

実機写真と見比べながら位置を合わせた。完全な精度ではないが、グリッパーの形がシミュレーターに乗った。

GripperBuilderにこの手動確認済みの座標をハードコードして自動再現できるようにする作業は持ち越す。成果は「仮組み完了」だ。