mechArm 270がUnityシミュレーションでコーヒーカップを掴んで台座に置いた｜PickAndPlace完成

角度データをスクリプトに書いた。プレイボタンを押した。アームが動いた。

このプロジェクトについて

iPhoneで撮影した現実空間をUnityでデジタルツイン化し、ロボットアームの動作をシミュレーションで最適化してから実機に展開するパイプラインを個人で構築している。

ターゲットはシンプルだ。ロボットアームがコーヒーカップを掴み、台座の上に置く。その一連の動作を、iPhoneとUnityだけで設計できることを証明する。

この記録は「体験が初めて成立した」話だ。

8つのステートで一本の動作を作った

前回の記録で確定した4姿勢（HOME / GRASP / LIFT / PLACE）をベースに、実際に動かしてみると問題が次々と出てきた。修正を繰り返す中で、最終的に動作は8つのステートで構成されることになった。

MOVING_TO_CUP  → カップに向かって手を伸ばす
GRABBING       → カップを把持（キネマティック化）
LIFTING        → カップを持ち上げる
MOVING_TO_PLACE → 台座の上空に移動する
LOWERING       → 台座ギリギリまでゆっくり降ろす
PLACING        → カップを台座に置いてリリース
RETURNING_HOME → アームが初期姿勢に戻る
COMPLETE       → 完了

最初は4ステートで済むと思っていた。PLACEが「上から落とす」動作になっていたことに気づいて LOWERING を追加した。動作の終わりがわかりづらいという問題を解決するために RETURNING_HOME を追加した。体験として成立させるために、必要なステートが自然に増えた。

ログで動作を診断した

この実装で最も効いたのは、ログの活用だ。

SmartTrajectoryController.cs にCSV書き出しを実装し、EndEffectorとカップの位置・回転をフレームごとに記録した。

time, state, ee_x, ee_y, ee_z, cup_x, cup_y, cup_z, cup_rot_y

「カップが沈む」「横移動中にズレる」「台座より高い位置から落とされる」——目視で感じた違和感を、ログの数値で根拠を持って特定できた。感覚でパラメータを調整するのではなく、数値で原因を確認してから修正する。このサイクルが実装を支えた。

カップ追従の設計：ローカル座標が鍵だった

把持中、カップがEndEffectorに追従する処理で問題が起きた。

最初の実装はワールド座標のオフセットで追従させていた。

// NG：ワールド座標固定のため、アームが回転するとカップがズレる
graspOffset = cupObject.transform.position - endEffector.position;
cupObject.transform.position = endEffector.position + graspOffset;

J0（ベース回転軸）が40度から-55度へと約95度回転する動作では、これは致命的なズレになる。

解決策はEndEffectorのローカル座標でオフセットを記録することだった。

// OK：EEのローカル空間でオフセットを管理
graspOffset = endEffector.InverseTransformPoint(cupObject.transform.position);
graspLocalRotation = Quaternion.Inverse(endEffector.rotation) * cupObject.transform.rotation;

// 追従時
cupObject.transform.position = endEffector.TransformPoint(graspOffset);
cupObject.transform.rotation = endEffector.rotation * graspLocalRotation;

InverseTransformPoint でEEのローカル空間に変換して記録し、毎フレーム TransformPoint でワールド座標に戻す。アームがどの方向に回転しても、カップはEndEffectorに対して常に同じ相対位置・相対回転を保つ。

ティーチングで正解を確定した

LOWERING（台座に降ろす）姿勢の角度調整で、計算による当てずっぽうを何度繰り返しても収束しなかった。

方針を変えた。JointTeacherで手動でアームを操作し、「カップが台座の2cm上にある状態」を目視で作った。その時の関節角度を読み取って、そのまま SolvePlaceLowIK() に書き込んだ。

private float[] SolvePlaceLowIK()
{
    return new float[] { -44f, 75f, -20f, 0f, -55f, 0f };
}

計算で探すより、正解を直接確認する方が速い。前回の記録で「作り直した方が速いことがある」という教訓を書いたが、今回も同じ構造だった。既存の方法を改善し続けるより、アプローチを切り替えた方が解に早く辿り着く。

確定したパラメータ

カップのY座標下限クランプ: 0.025f（台座上面の高さ）
LOWERING軌道時間: trajectoryDuration × 1.5f（通常より50%遅く）

この記録から得た教訓

「体験の成立は、ステートの数と比例する。」

最初に設計した4ステートは動作の骨格だった。8ステートになったのは、そのひとつひとつが「体験として自然に見えるかどうか」の問いに答えた結果だ。

ロボットが物を置く動作に「降ろす」フェーズが必要なのは、人間が物を置くときに自然にそうするからだ。設計の根拠は常に「人間の動作に近づけるか」にある。

次のステップ：動作モックの大量生成

pick and placeの「体験が成立する状態」は確立した。

ただし今回の動作はひとつの「正解」にすぎない。1種類の動作で最適解かどうかは判断できない。

次のフェーズで取り組む構想がある。動作モックの大量生成だ。

ウェブサービスのUIデザインでは、いきなり完成形を作るのではなく、まず複数のワイヤーフレームを作って比較する。ロボットの動作設計でも同じアプローチが使えるはずだ。

アプローチ × 速度 = 動作のバリエーション

standard（正面から）× slow（5秒）
standard           × normal（3秒）
standard           × fast（1.5秒）
high（上から）     × slow
high               × normal
high               × fast
low（低い軌道）    × slow
...

4アプローチ × 3速度 = 12パターンのCSVを生成し、連続再生しながら各動作のログを記録する。目視で「自然/不自然」を判断し、最適な動作を選別する。

将来的には、ログデータをAIが自動評価して最適解を選ぶことも視野に入れている。今は1台で順番に再生して目視評価する。ゆくゆくはN台並列で同時再生し、ログをAIが一括評価する構造へ発展させる。

これは「ティーチング」で終わらない設計だ。動作モックを大量に作り、最適解を探す仕組みを作ること——それがこのプロジェクトのコアにある問いだ。

実装コード（SmartTrajectoryController.cs）

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

/// <summary>
/// SmartTrajectoryController
/// 1回だけ数値IK計算 → 5次多項式軌道生成 → FixedUpdateで再生
/// 物理憲法遵守: driveType / stiffness / damping には一切触れない
/// </summary>
public class SmartTrajectoryController : MonoBehaviour
{
    [Header("Joints")]
    public ArticulationBody[] joints;   // link1〜link6

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

    [Header("Targets")]
    public Transform cupTarget;
    public Transform placeTarget;
    public GameObject cupObject;

    [Header("Trajectory")]
    public float trajectoryDuration = 2.0f;
    public float fps = 50f;

    private enum State { IDLE, MOVING_TO_CUP, GRABBING, LIFTING, MOVING_TO_PLACE, LOWERING, PLACING, RETURNING_HOME, COMPLETE }
    private State currentState = State.IDLE;

    private List<float[]> currentTrajectory = new List<float[]>();
    private int frameIndex = 0;
    private bool isPlaying = false;

    private Vector3 graspOffset = Vector3.zero;
    private Quaternion graspRotation = Quaternion.identity;
    private Quaternion graspLocalRotation = Quaternion.identity;

    private StreamWriter logWriter;

    void Start()
    {
        string logsDir = Path.Combine(Application.dataPath, "..", "Logs");
        Directory.CreateDirectory(logsDir);
        string logPath = Path.Combine(logsDir, "smart_controller_log.csv");
        logWriter = new StreamWriter(logPath, false) { AutoFlush = true };
        logWriter.WriteLine("time,state,frameIndex,ee_x,ee_y,ee_z,cup_x,cup_y,cup_z,cup_rot_y,j0_target,j0_pos");

        Invoke(nameof(StartMovingToCup), 2.0f);
    }

    void FixedUpdate()
    {
        if (!isPlaying) return;
        if (currentTrajectory == null || frameIndex >= currentTrajectory.Count) return;

        float[] targets = currentTrajectory[frameIndex];
        for (int i = 0; i < joints.Length && i < 6; i++)
        {
            if (joints[i] == null) continue;
            var drive = joints[i].xDrive;
            // 【憲法遵守】target のみ更新
            drive.target = targets[i];
            joints[i].xDrive = drive;
            joints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }

        frameIndex++;

        // カップ追従（把持中）— EEのローカル座標で追従
        if (currentState == State.GRABBING || currentState == State.LIFTING ||
            currentState == State.MOVING_TO_PLACE || currentState == State.LOWERING)
        {
            if (cupObject != null && endEffector != null)
            {
                Vector3 newCupPos = endEffector.TransformPoint(graspOffset);
                newCupPos.y = Mathf.Max(newCupPos.y, 0.025f); // 台座上面より下に沈まない
                cupObject.transform.position = newCupPos;
                cupObject.transform.rotation = endEffector.rotation * graspLocalRotation;
            }
        }

        if (frameIndex >= currentTrajectory.Count)
            OnTrajectoryComplete();
    }

    private void OnTrajectoryComplete()
    {
        isPlaying = false;

        switch (currentState)
        {
            case State.MOVING_TO_CUP:
                currentState = State.GRABBING;
                if (cupObject != null)
                {
                    var rb = cupObject.GetComponent<Rigidbody>();
                    if (rb != null) { rb.velocity = Vector3.zero; rb.angularVelocity = Vector3.zero; rb.isKinematic = true; }
                    if (endEffector != null)
                    {
                        // ローカル座標でオフセット・回転を記録
                        graspOffset = endEffector.InverseTransformPoint(cupObject.transform.position);
                        graspRotation = cupObject.transform.rotation;
                        graspLocalRotation = Quaternion.Inverse(endEffector.rotation) * cupObject.transform.rotation;
                    }
                }
                Invoke(nameof(StartLifting), 0.5f);
                break;

            case State.LIFTING:
                currentState = State.MOVING_TO_PLACE;
                PlayTrajectory(SolvePlaceHighIK());
                break;

            case State.MOVING_TO_PLACE:
                currentState = State.LOWERING;
                // LOWERINGは1.5倍の時間でゆっくり
                PlayTrajectory(SolvePlaceLowIK(), trajectoryDuration * 1.5f);
                break;

            case State.LOWERING:
                currentState = State.PLACING;
                if (cupObject != null && placeTarget != null)
                {
                    cupObject.transform.position = new Vector3(
                        placeTarget.position.x,
                        placeTarget.position.y + 0.02f,
                        placeTarget.position.z);
                    cupObject.transform.rotation = Quaternion.Euler(-90f, 0f, -15f);
                    var rb = cupObject.GetComponent<Rigidbody>();
                    if (rb != null) { rb.isKinematic = false; rb.velocity = Vector3.zero; rb.angularVelocity = Vector3.zero; }
                }
                currentState = State.RETURNING_HOME;
                PlayTrajectory(new float[] { 0f, 0f, 0f, 0f, 0f, 0f });
                break;

            case State.RETURNING_HOME:
                currentState = State.COMPLETE;
                break;
        }
    }

    private void StartMovingToCup()
    {
        currentState = State.MOVING_TO_CUP;
        PlayTrajectory(new float[] { 40f, 82f, -36f, 0f, -50f, 0f });
    }

    private void StartLifting()
    {
        currentState = State.LIFTING;
        PlayTrajectory(new float[] { 40f, 50f, -20f, 0f, -42f, 0f });
    }

    private float[] SolvePlaceHighIK() => new float[] { -55f, 50f, -25f, 0f, -38f, 0f };
    private float[] SolvePlaceLowIK()  => new float[] { -44f, 75f, -20f, 0f, -55f, 0f };

    private void PlayTrajectory(float[] toAngles, float duration = -1f)
    {
        currentTrajectory = GenerateTrajectory(GetCurrentAngles(), toAngles, duration);
        frameIndex = 0;
        isPlaying = true;
    }

    private List<float[]> GenerateTrajectory(float[] from, float[] to, float duration = -1f)
    {
        var traj = new List<float[]>();
        float d = duration > 0 ? duration : trajectoryDuration;
        int n = Mathf.Max(2, Mathf.RoundToInt(d * fps));
        for (int i = 0; i < n; i++)
        {
            float t = (float)i / (n - 1);
            // 5次多項式（最小ジャーク）
            float s = 10 * Mathf.Pow(t, 3) - 15 * Mathf.Pow(t, 4) + 6 * Mathf.Pow(t, 5);
            float[] frame = new float[6];
            for (int j = 0; j < 6; j++) frame[j] = from[j] + (to[j] - from[j]) * s;
            traj.Add(frame);
        }
        return traj;
    }

    private float[] GetCurrentAngles()
    {
        float[] angles = new float[6];
        if (joints == null) return angles;
        for (int i = 0; i < 6 && i < joints.Length; i++)
            if (joints[i] != null) angles[i] = joints[i].xDrive.target;
        return angles;
    }

    void OnDestroy() { logWriter?.Close(); }
}

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