4コママンガ 4コママンガarrow_forward English 4-Komaarrow_forward
メッセージarrow_forward
mechArm 270 PickAndPlaceのUnityステートマシンを解剖した日。REACH_CUP→GRASP→LIFT→PLACEの6ステート設計
LOG94R2R2R2026/3/15

mechArm 270 PickAndPlaceのUnityステートマシンを解剖した日。REACH_CUP→GRASP→LIFT→PLACEの6ステート設計

ロボットアームが「コーヒーカップを掴んで置く」という動作を、何度繰り返しても崩れずに続けられるようにする。それが今回のテーマだった。

※注釈：デジタルツイン学習（R2R2R）の検証プロセス 本プロジェクトは、AIの判断を物理的動作へ変換する設計を検証するものです。現実空間をデジタルツイン化し、仮想空間での大量試行（Real-to-Render）から得た最適解を、実世界のモーター駆動（to-Real）へ還元する「R2R2Rパイプライン」の実装過程を記録・考察します。

やったこと

コンソールログのファイル出力

デバッグの第一歩として、Unityのコンソールログをファイルに書き出す仕組みを実装した。

Application.logMessageReceived += (log, stack, type) => {
    string dir = Path.Combine(Application.dataPath, "..", "Logs");
    string path = Path.Combine(dir, "console_log.txt");
    File.AppendAllText(path, $"{Time.time:F4},{type},{log}\n");
};

これで robot_event_log.csv・robot_frame_log.csv・console_log.txt の3つのログが揃い、動作の何が起きているかを数字で追えるようになった。ログがなければ「なんかおかしい」で終わっていた問題が、「3.21秒でGRASP、6.35秒で2周目…10周目は0.1秒」という具体的な証拠として見えるようになる。

drive.target蓄積問題の根本修正

ログを見て最初に気づいたのが、ループを重ねるたびにアームが加速していく問題だった。1周目は20秒、2周目は3秒、10周目以降は0.1秒以下。これはIKの収束ではなく、drive.targetに誤差が蓄積して「たまたまgraspThresholdを下回る」状態だった。

修正は SyncDriveTargetsToPhysics() の導入。PLACEステートに入った瞬間、全関節のdrive.targetを実際の物理角度（jointPosition）に即時同期する。

private void SyncDriveTargetsToPhysics()
{
    foreach (var joint in targetJoints)
    {
        var drive = joint.xDrive;
        drive.target = GetDOF(joint.jointPosition);
        joint.xDrive = drive;
    }
}

これでループ加速が止まり、GRASP間隔が毎周1.0〜1.3秒で安定した。

ステートマシンに3つのステートを追加

ステートは4つだった。

REACH_CUP → GRASP → REACH_PLACE → PLACE

これでは「掴む→横に移動→置く」の動作しか表現できない。カップが地面をすり抜けたり、上から落としたりする問題が続いたため、ステートを3つ追加した。

REACH_CUP → GRASP → LIFT → REACH_PLACE → LOWER → PLACE

  • LIFT：掴んだ直後、真上に持ち上げる。ターゲットを handlePoint.position + Vector3.up * liftHeight にキャッシュする（handlePointは動くためキャッシュが必須）

  • LOWER：PlaceTarget真上から地面レベルまで下降する

  • PLACE：解放してループに戻る

<embed src="day94_stateMachine.html" width="100%" height="420" />

CancelInvokeによるタイマー多重発火の修正

各ステートで Invoke(nameof(ResetCooldown), x秒) を呼んでいたが、前のステートのInvokeがキャンセルされずに残り、意図しないタイミングでisCooldownがfalseになっていた。全5箇所に CancelInvoke(nameof(ResetCooldown)) を追加して修正。

誤GRASP防止（hasApproachedFromFar）

PLACE直後、アームはまだPlaceTarget付近にいる。この状態でREACH_CUPに戻ると、すぐにgraspThresholdを下回ってGRASPが発動してしまう。

hasApproachedFromFar フラグを導入し、「一定距離（graspThreshold × 3）以上離れてから近づいた」場合のみGRASPに進むようにした。

戻り速度の分離（isReturning）

REACH_CUPステートは「最初にカップへ向かう」場面と「PLACE後に戻る」場面の2つで使われる。戻る時だけゆっくり動かしたいため、isReturning フラグで速度を分離した。

float speed = (currentState == State.REACH_CUP && isReturning) 
    ? returnSpeed 
    : ccdSpeed;

学んだこと

ステートマシンは「遷移の設計」がすべてだ。

コードを書く前にステートの数と遷移条件を紙に書いた方が速い。実装しながら「あ、ここにステートが必要だ」と気づくことが多く、後追いで追加するたびに副作用が出た。

ログがないと「なんかおかしい」で終わる。数字で見れると原因が特定できる。最初にやったコンソールログの出力は、その後の全修正を可能にした基盤だった。

終了時点のRobotArmController.csコード

c#

using UnityEngine;
using System.IO;

public class RobotArmController : MonoBehaviour
{
    // ── フィールド ────────────────────────────────────────
    [Header("Joints")]
    public ArticulationBody[] targetJoints; // Element0=link1 〜 Element5=link6

    [Header("PickAndPlace")]
    public Transform  handlePoint;          // カップの把持点
    public GameObject placeTarget;          // 置き場所
    public GameObject cupObject;            // Inspector で coffeecup_3 を割り当てる
    private bool isGrasping = false;        // 把持中フラグ

    [Header("EndEffector")]
    public Transform endEffector;           // link6 配下の EndEffector

    public float graspThreshold = 0.05f;    // 到達判定距離 [m]
    public float ccdSpeed = 0.05f;          // CCD IK 速度係数
    public float liftHeight = 0.15f;        // リフト高さ [m]
    public float approachHeight = 0.1f;     // アプローチ高さ [m]
    public float returnSpeed = 0.01f;       // REACH_CUP戻り速度 [m]

    // ── 内部状態 ──────────────────────────────────────────
    private enum State { REACH_CUP, GRASP, LIFT, REACH_PLACE, LOWER, PLACE }
    private State currentState = State.REACH_CUP;
    private bool isCooldown = false;

    private const int CCD_ITERATIONS = 1;   // 1フレームあたりの反復回数

    private Vector3 cachedHandlePosition;
    private Vector3 cachedPlacePosition;
    private bool positionsCached = false;
    private Vector3 cupInitialPosition;
    private Vector3 graspOffset = Vector3.zero;
    private Vector3 cachedLiftPosition;
    private bool hasApproachedFromFar = false;
    private bool isReturning = false;

    // ── CSV出力 ────────────────────────────────────────────
    private StreamWriter frameLogWriter;
    private StreamWriter eventLogWriter;
    private string logsDirectory;

    // ── Unity ライフサイクル ──────────────────────────────
    void Awake()
    {
        Application.logMessageReceived += (log, stack, type) => {
            string dir = Path.Combine(Application.dataPath, "..", "Logs");
            Directory.CreateDirectory(dir);
            string path = Path.Combine(dir, "console_log.txt");
            string entry = $"{Time.time:F4},{type},{log}\n";
            File.AppendAllText(path, entry);
        };
    }

    void Start()
    {
        if (targetJoints == null) return;

        foreach (var joint in targetJoints)
        {
            if (joint == null) continue;
            var drive = joint.xDrive;
            drive.driveType  = ArticulationDriveType.Target;
            drive.forceLimit = 1000000f;
            joint.xDrive = drive;
        }

        if (cupObject != null)
        {
            var cupColliders = cupObject.GetComponentsInChildren<Collider>();
            foreach (var joint in targetJoints)
            {
                if (joint == null) continue;
                var armColliders = joint.GetComponentsInChildren<Collider>();
                foreach (var armCol in armColliders)
                    foreach (var cupCol in cupColliders)
                        Physics.IgnoreCollision(armCol, cupCol, true);
            }
        }

        cachedHandlePosition = handlePoint.position;
        cachedPlacePosition = placeTarget.transform.position;
        positionsCached = true;
        cupInitialPosition = cupObject.transform.position;

        logsDirectory = Path.Combine(Application.dataPath, "..", "Logs");
        Directory.CreateDirectory(logsDirectory);

        string frameLogPath = Path.Combine(logsDirectory, "robot_frame_log.csv");
        frameLogWriter = new StreamWriter(frameLogPath, false) { AutoFlush = true };
        frameLogWriter.WriteLine("time,state,dist,j0_pos,j1_pos,j2_pos,j3_pos,j4_pos,j5_pos,...");

        string eventLogPath = Path.Combine(logsDirectory, "robot_event_log.csv");
        eventLogWriter = new StreamWriter(eventLogPath, false) { AutoFlush = true };
        eventLogWriter.WriteLine("time,event,dist,ee_x,ee_y,ee_z");

        isCooldown = true;
        Invoke(nameof(ResetCooldown), 3.0f);
    }

    void LateUpdate()
    {
        if (targetJoints == null || targetJoints.Length < 6) return;
        if (endEffector == null) return;

        Vector3 targetPosition = GetTargetPosition();

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

        if (!isCooldown)
        {
            for (int iter = 0; iter < CCD_ITERATIONS; iter++)
            {
                for (int i = targetJoints.Length - 1; i >= 0; i--)
                {
                    if (targetJoints[i] == null) continue;
                    Transform jointTransform = targetJoints[i].transform;
                    Vector3 jointAxis = targetJoints[i].anchorRotation * Vector3.right;
                    jointAxis = jointTransform.TransformDirection(jointAxis);
                    Vector3 toEffector = Vector3.ProjectOnPlane(
                        endEffector.position - jointTransform.position, jointAxis).normalized;
                    Vector3 toTarget = Vector3.ProjectOnPlane(
                        targetPosition - jointTransform.position, jointAxis).normalized;
                    float angle = Vector3.SignedAngle(toEffector, toTarget, jointAxis);

                    var drive = targetJoints[i].xDrive;
                    drive.forceLimit = 1000000f;
                    // ★ 戻り時はreturnSpeed、それ以外はccdSpeed
                    float speed = (currentState == State.REACH_CUP && isReturning)
                        ? returnSpeed : ccdSpeed;
                    drive.target += angle * speed;
                    drive.target  = Mathf.Clamp(drive.target, lowerLimits[i], upperLimits[i]);
                    targetJoints[i].xDrive = drive;
                    targetJoints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
                }
            }
        }

        float dist = Vector3.Distance(endEffector.position, targetPosition);

        switch (currentState)
        {
            case State.REACH_CUP:
                // ★ 十分離れてから近づいた場合のみGRASPへ
                if (dist > graspThreshold * 3f) hasApproachedFromFar = true;
                if (dist <= graspThreshold && !isCooldown && hasApproachedFromFar)
                {
                    isCooldown = true;
                    CancelInvoke(nameof(ResetCooldown));
                    Invoke(nameof(ResetCooldown), 1.5f);
                    currentState = State.GRASP;
                    WriteEventLog("TO_GRASP", dist);
                }
                break;

            case State.GRASP:
                if (!isGrasping && cupObject != null)
                {
                    var cupRb = cupObject.GetComponent<Rigidbody>();
                    if (cupRb != null) { cupRb.velocity = Vector3.zero; cupRb.isKinematic = true; }
                    isGrasping = true;
                    graspOffset = cupObject.transform.position - endEffector.position;
                    // ★ LIFTターゲットをキャッシュ（handlePointは動くため）
                    cachedLiftPosition = handlePoint.position + Vector3.up * liftHeight;
                    WriteEventLog("GRASP_START", dist);
                    isReturning = false;
                    currentState = State.LIFT;
                    WriteEventLog("TO_LIFT", dist);
                }
                break;

            case State.LIFT:
                if (dist <= graspThreshold && !isCooldown)
                {
                    isCooldown = true;
                    CancelInvoke(nameof(ResetCooldown));
                    Invoke(nameof(ResetCooldown), 1.0f);
                    currentState = State.REACH_PLACE;
                    WriteEventLog("TO_REACH_PLACE", dist);
                }
                break;

            case State.REACH_PLACE:
                if (dist <= graspThreshold && !isCooldown)
                {
                    isCooldown = true;
                    CancelInvoke(nameof(ResetCooldown));
                    Invoke(nameof(ResetCooldown), 1.5f);
                    currentState = State.LOWER;
                    WriteEventLog("TO_LOWER", dist);
                }
                break;

            case State.LOWER:
                if (dist <= graspThreshold && !isCooldown)
                {
                    isCooldown = true;
                    CancelInvoke(nameof(ResetCooldown));
                    Invoke(nameof(ResetCooldown), 1.0f);
                    currentState = State.PLACE;
                    WriteEventLog("TO_PLACE", dist);
                }
                break;

            case State.PLACE:
                SyncDriveTargetsToPhysics();
                if (isGrasping && cupObject != null)
                {
                    isGrasping = false;
                    ResetCupPosition();
                    WriteEventLog("PLACE_DONE", dist);
                }
                isCooldown = true;
                CancelInvoke(nameof(ResetCooldown));
                Invoke(nameof(ResetCooldown), 3.5f);
                hasApproachedFromFar = false;
                isReturning = true;
                currentState = State.REACH_CUP;
                WriteEventLog("TO_REACH_CUP", dist);
                break;
        }

        // 把持中はカップをEndEffectorに追従
        if (isGrasping && cupObject != null && endEffector != null)
            cupObject.transform.position = endEffector.position + graspOffset;
    }

    private void SyncDriveTargetsToPhysics()
    {
        if (targetJoints == null) return;
        foreach (var joint in targetJoints)
        {
            if (joint == null) continue;
            var drive = joint.xDrive;
            drive.target = GetDOF(joint.jointPosition);
            joint.xDrive = drive;
            joint.SetDriveTarget(ArticulationDriveAxis.X, drive.target);
        }
    }

    private void ResetCooldown()
    {
        SyncDriveTargetsToPhysics();
        isCooldown = false;
    }

    private void ResetCupPosition()
    {
        if (cupObject == null) return;
        var cupRb = cupObject.GetComponent<Rigidbody>();
        if (cupRb != null) cupRb.isKinematic = true;
        cupObject.transform.position = cupInitialPosition;
        if (cupRb != null) cupRb.isKinematic = false;
    }

    private void WriteEventLog(string eventName, float dist)
    {
        if (eventLogWriter == null) return;
        Vector3 ee = endEffector != null ? endEffector.position : Vector3.zero;
        eventLogWriter.WriteLine($"{Time.time:F4},{eventName},{dist:F4},{ee.x:F4},{ee.y:F4},{ee.z:F4}");
    }

    private float GetDOF(ArticulationReducedSpace s) => s.dofCount > 0 ? s[0] : 0f;

    void OnDestroy() { frameLogWriter?.Close(); eventLogWriter?.Close(); }

    private Vector3 GetTargetPosition()
    {
        switch (currentState)
        {
            case State.REACH_CUP:
            case State.GRASP:
                return handlePoint != null ? handlePoint.position : transform.position;
            case State.LIFT:
                return cachedLiftPosition;
            case State.REACH_PLACE:
                return new Vector3(cachedPlacePosition.x,
                    cupInitialPosition.y + approachHeight, cachedPlacePosition.z);
            case State.LOWER:
            case State.PLACE:
                // ★ graspOffset.yを補正してカップが地面レベルに来るようにする
                return new Vector3(cachedPlacePosition.x,
                    cupInitialPosition.y - graspOffset.y, cachedPlacePosition.z);
            default:
                return transform.position;
        }
    }
}

Series | デジタルツイン学習-R2R2R（Real to Render to Real）の記録

CCD IKを捨てた
LOG97R2R2R2026/3/15

CCD IKを捨てた
PythonログとUnity可視化の乖離。mechArm 270の動作ログが「成功」でも目視がおかしかった理由
LOG96R2R2R2026/3/15

PythonログとUnity可視化の乖離。mechArm 270の動作ログが「成功」でも目視がおかしかった理由
UnityのArticulationBodyが初めて物理的に正しく動いた｜mechArm 270関節制御の転換点
LOG95R2R2R2026/3/15

UnityのArticulationBodyが初めて物理的に正しく動いた｜mechArm 270関節制御の転換点
mechArm 270のPickAndPlaceデータ収集を始めた｜Unity × Python記録設計
LOG93R2R2R2026/3/14

mechArm 270のPickAndPlaceデータ収集を始めた｜Unity × Python記録設計
FixedJointで「掴む」が成立した
LOG92R2R2R2026/3/14

FixedJointで「掴む」が成立した
FixedJointで「掴む」を実装した
LOG91R2R2R2026/3/13

FixedJointで「掴む」を実装した
デジタルツイン学習-R2R2R（Real to Render to Real）の記録をすべて見る（185本）arrow_forward