ロボットアームはどうやってカップを「掴む」のか？PickAndPlace実装の設計判断

Unityでのロボットアームをコーヒーカップへ接近・把持・設置させる「PickAndPlace」実装に向けて、本格的な技術設計を行った。コードを書く前に、なぜその設計なのかを徹底的に整理する作業だ。

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

把持方法の選択：FixedJoint vs Parenting

最初に決めなければならなかったのが、「ロボットアームがカップを掴む」という動作をUnityの物理エンジン上でどう表現するか、という問題だ。

主な選択肢は2つあった。

結論：FixedJointを採用。

Parentingは一見シンプルに見えるが、ArticulationBodyの階層構造を壊す危険性がある。ロボットアーム側はArticulationBody、カップ側はRigidbodyという異なる物理コンポーネントが共存しているため、親子化するとRigidbodyが無効化されるか、物理演算が破綻するリスクが非常に高い。

FixedJointであれば、アーム先端とカップを物理的に「固定」するだけなので、既存の物理設定を一切変えずに把持を実現できる。

ArticulationBodyとRigidbodyの「強者と弱者」関係

このプロジェクトで最も注意すべき物理的リスクが、アーム（ArticulationBody）とカップ（Rigidbody）の干渉問題だ。

現在のアームにはStiffness 20,000という非常に強力な剛性が設定されている。これはアームが目標位置へ正確に追従するために必要な値だが、裏を返せば「アームが少しでもカップに食い込んだ状態で把持判定が発生すると、カップが弾丸のように吹き飛ぶ」ということでもある。

この問題への対処として、2つの設定を事前に行う。

① coffeecup_3のCollision DetectionをContinuous Dynamicに変更

デフォルトのDiscreteでは、高速な物理的接触の瞬間を見逃す可能性がある。Continuous Dynamicに変更することで、フレームをまたぐ瞬間的な貫通や衝突の見逃しを防ぐ。

② 把持判定は距離0.05m以下で行う

アームの先端がカップの取っ手に十分近づいた状態（0.05m以内）でのみ把持判定を発生させることで、「食い込みながら掴む」という最悪のケースを回避する。

Clineへの指示設計：憲法を守らせる

コード生成はClineに依頼するが、AIへの指示が曖昧だと意図しない実装を誘発する。特に危険なのが以下の2パターンだ。

NG指示とOK指示の対比

把持の方法について

• ❌NG：「よしなに近づけて掴んで」

• ✅OK：「targetJoint.xDrive.targetを徐々に変化させて接近し、取っ手のBox Colliderとの距離が0.05m以下になったらFixedJointを生成せよ」

曖昧な指示は、物理演算を無視したTransform.positionの直接書き換えを誘発する。これをやるとアームの物理挙動が完全に壊れる。

物理設定について

• ❌NG：「物理設定を調整して」

• ✅OK：「剛性とダンピングの値は不変とし、xDrive.targetとforceLimitの操作のみで制御せよ」

このプロジェクトの「憲法」の本質は数値そのものではなく、「物理演算が破綻しない範囲でArticulationDriveによる制御を維持する」ことにある。今日の実装でStiffnessを500,000に引き上げたのも、この大方針の範囲内でアームの自重に合わせてチューニングした結果だ。Clineに渡してはいけないのは、物理を無視したURDF初期値（10²⁵）レベルの設定や、Transform.positionの直接書き換えだ。

VCS Check-inを3回刻む理由

PickAndPlaceは「接近」「把持」「設置」の3フェーズに分かれており、それぞれで物理的な状態が大きく変化する。どこかで挙動が壊れても直前の安全な状態に戻れるよう、各フェーズの成功時にVCS Check-inを行う。

1. 接近成功時  ── アームが取っ手付近でピタッと止まれる状態
2. 把持成功時  ── FixedJointでカップを持ち上げても震えない状態
3. 設置完了時  ── コーヒーメーカーへ移動、ジョイント解除、カップを置けた状態

LOG83でのgit cleanによるシーン消失事故を経験しているからこそ、この粒度でのバックアップが必要だと確信している。

今日時点の実装コード（接近制御スクリプト）

LOG86終了時点でのRobotArmController.cs全文を記録する。

using UnityEngine;

public class RobotArmController : MonoBehaviour
{
    public ArticulationBody[] targetJoints;
    public float link1TargetAngle = 0f;   // link1を正面に向ける
    public float link2TargetAngle = 45f;  // link2を前に倒す
    public float moveSpeed = 20f;         // 1秒間に動く度数

    private float[] currentAngles;

    void Start()
    {
        currentAngles = new float[targetJoints.Length];
    }

    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 = 500000f;
        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 = 500000f;
        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();

        Debug.Log($"Link1 target: {currentAngles[0]:F2}, Link2 target: {currentAngles[1]:F2}");
    }
}

⚠️ Stiffness 20,000 → 500,000：矛盾か、チューニングか？

コードを見た瞬間、「設計方針のStiffness 20,000と矛盾している」と感じた。しかしGeminiの分析によると、これは矛盾ではなく工学的な最適化だという。整理しておく。

なぜ数値が変わったのか

設計方針で想定していたのは「標準的なロボットの関節の硬さ」だった。しかしlink1はアーム全体の自重と慣性を支える土台であり、Stiffness 20,000では重力に負けて「おじぎ」をしてしまっていた（これが先ほどの「倒れちゃってるよ」の原因だった）。

500,000はバネを「ガチガチ」に硬くしたイメージ。これにより目標角度に対して重力に負けず、シャキッと姿勢を保持できるようになった。

設計方針の「真の意図」を再定義する

「Stiffness 20,000は不変」というルールの本質は、数値そのものではなく「物理演算が破綻しない範囲での制御を維持する」ことにある。

• 10²⁵は物理を無視した「テレポート」→NG

• 500,000は物理に従いつつアームの質量に最適化した「筋力チューニング」→OK

大方針（ArticulationDriveによる物理ベースの制御）は守られている。

今後の判断基準

アームがガタガタ震え出す（オーバーシュート）  → 500,000は硬すぎ → 下げる
まだ自重で垂れ下がる                         → 500,000は柔らかすぎ → 上げる

現時点でアームが立って動いているなら、これがこのアームにとっての最適解だ。

次の作業順序

設計判断はすべて完了した。金曜日は以下の順序で実装に入る。

1. VCS Check-in（作業開始前）
2. coffeecup_3のCollision Detection → Continuous Dynamicに変更
3. VCS Check-in
4. Clineに接近スクリプト依頼 → コードを読む → 実行
5. 接近成功 → VCS Check-in
6. 把持スクリプト → コードを読む → 実行
7. 把持成功 → VCS Check-in
8. 設置スクリプト → コードを読む → 実行
9. 設置完了 → VCS Check-in

コードを書く前に設計を固める。これが実際の作業から学んだ最大の教訓。