LOG88R2R2R2026/3/13

Unityシミュレーションでコーヒーカップが初めて動いた｜mechArm 270 PickAndPlace検証

PickAndPlaceの実装に取り組んだ一日だった。

結論から言うと、「カップをロボットアームが掴んで、別の場所に移動する」という動作が、シミュレーション上で成立した。完璧ではない。精度はまだ荒い。でも、動いた。

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

何をやったか

ステートマシンを3つのフェーズで設計した。

APPROACH — アームがカップに近づく。
GRASP — 距離が閾値以下になったら、FixedJointを動的に生成してカップをアームに固定する。
PLACE — 指定した座標に向かってアームが動き、到達したらFixedJointを破棄してカップを置く。

シンプルな構造だ。でも実際に動かすまでに、いくつかの壁があった。

つまずきと学び

つまずき1：AIモデルの選択

最初、ClineのバックエンドモデルをClaude Haikuで動かしていた。速くて安い。でも今日のような複雑なC#実装では、メソッドが二重定義されたコードが返ってきた。構文エラーで動かない。

Claude Sonnetに変えた瞬間、コードの品質が明らかに変わった。同じ指示でも、出力が全然違う。モデルの選択はコストの問題ではなく、作業の質の問題だと実感した。

つまずき2：ArticulationBodyにRigidbodyはない

FixedJointで把持を実装しようとして、connectedBodyにlink2のRigidbodyを指定しようとした。が、ArticulationBodyコンポーネントにRigidbodyは存在しない。別物だ。

解決策はconnectedArticulationBodyを使うこと。これに変えた瞬間、カップがアームにくっついて動いた。

つまずき3：関節角度制御の座標系

PLACEステートで、「PlaceTargetの方向に向かう角度を自動計算しよう」と試みた。三角関数で仰角とヨー角を計算する方法だ。でもロボットの軸定義と計算の符号が噛み合わず、アームが上を向いたり横を向いたりした。

最終的に「固定角度を手動で決める」方式に切り替えた。placeLink1AngleとplaceLink2Angleという2つのパラメータをInspectorから調整できるようにして、実際に動かしながら値を探した。

精度より体験の成立を優先する、というのがこのプロジェクトの哲学だ。今日もその判断をした。

実装したコード

using UnityEngine;

public class RobotArmController : MonoBehaviour
{
    public ArticulationBody[] targetJoints;
    public float link1TargetAngle = 0f;
    public float link2TargetAngle = 45f;
    public float moveSpeed = 20f;

    [Header("PickAndPlace")]
    public GameObject graspTarget;   // coffeecup_3
    public Transform  handlePoint;   // coffeecup_3/HandlePoint
    public GameObject placeTarget;   // PlaceTarget
    public float graspThreshold = 0.05f;

    [Header("Place Angles")]
    public float placeLink1Angle = 0f;
    public float placeLink2Angle = 45f;

    private float[] currentAngles;
    private FixedJoint fixedJoint;

    private enum State { APPROACH, GRASP, PLACE }
    private State currentState = State.APPROACH;

    private Transform link2Transform;

    void Start()
    {
        currentAngles = new float[targetJoints.Length];
        if (targetJoints != null && targetJoints.Length >= 2)
            link2Transform = targetJoints[1].transform;
    }

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

        // link1（旋回）
        currentAngles[0] = Mathf.MoveTowards(currentAngles[0], link1TargetAngle, moveSpeed * Time.deltaTime);
        var drive1 = targetJoints[0].xDrive;
        drive1.stiffness = 20000f;
        drive1.forceLimit = 1000000f;
        drive1.target = currentAngles[0];
        drive1.driveType = ArticulationDriveType.Target;
        targetJoints[0].xDrive = drive1;
        targetJoints[0].SetDriveTarget(ArticulationDriveAxis.X, currentAngles[0]);

        // link2（肩）
        currentAngles[1] = Mathf.MoveTowards(currentAngles[1], link2TargetAngle, moveSpeed * Time.deltaTime);
        var drive2 = targetJoints[1].xDrive;
        drive2.stiffness = 20000f;
        drive2.forceLimit = 1000000f;
        drive2.target = currentAngles[1];
        drive2.driveType = ArticulationDriveType.Target;
        targetJoints[1].xDrive = drive2;
        targetJoints[1].SetDriveTarget(ArticulationDriveAxis.X, currentAngles[1]);

        targetJoints[0].WakeUp();
        targetJoints[1].WakeUp();

        UpdatePickAndPlace();
    }

    private void UpdatePickAndPlace()
    {
        if (handlePoint == null || graspTarget == null || placeTarget == null) return;
        if (link2Transform == null) return;

        switch (currentState)
        {
            case State.APPROACH: UpdateApproach(); break;
            case State.GRASP:    UpdateGrasp();    break;
            case State.PLACE:    UpdatePlace();    break;
        }
    }

    // APPROACH: HandlePointとlink2の距離が閾値以下でGRASPへ
    private void UpdateApproach()
    {
        float dist = Vector3.Distance(link2Transform.position, handlePoint.position);
        if (dist <= graspThreshold)
        {
            currentState = State.GRASP;
            Debug.Log("[RobotArm] → GRASP");
        }
    }

    // GRASP: FixedJointを動的生成してカップをlink2に固定
    private void UpdateGrasp()
    {
        if (fixedJoint != null)
        {
            currentState = State.PLACE;
            return;
        }

        fixedJoint = graspTarget.AddComponent<FixedJoint>();
        fixedJoint.connectedArticulationBody = targetJoints[1];

        Debug.Log("[RobotArm] FixedJoint生成: " + (fixedJoint != null));
        currentState = State.PLACE;
        Debug.Log("[RobotArm] → PLACE");
    }

    // PLACE: 固定角度でPlaceTargetへ向かい、到達したらFixedJointを破棄
    private void UpdatePlace()
    {
        link1TargetAngle = placeLink1Angle;
        link2TargetAngle = placeLink2Angle;

        float dist = Vector3.Distance(link2Transform.position,
                                      placeTarget.transform.position);
        if (dist <= graspThreshold)
        {
            if (fixedJoint != null)
            {
                Destroy(fixedJoint);
                fixedJoint = null;
            }
            currentState = State.APPROACH;
            Debug.Log("[RobotArm] → APPROACH (placed)");
        }
    }
}