UnityとGeminiでロボットを動かす！IK (Inverse Kinematics) 自作への挑戦 ~ その４

Unityで挑むパラレルグリッパー制作：ArticulationBodyと物理演算によるPick and Place

前回の記事では、Gemini Robotics-ERを用いた物体検出に成功しました。

今回は、自作のロボットアームIK（逆運動学）を制御し、実際にワークを掴んで運ぶ 「ピック＆プレイス動作」 の実装に挑戦します。特に、エンドエフェクター（ロボットの手）の構造と、搬送時の物理的な安定性に焦点を当てます。

1. グリッパー設計の重要性とパラレル方式の採用

産業用ロボットにおいて、ワークと直接接するグリッパー（End Effector）の設計はシミュレーションの成否を分ける生命線です。

これまでハサミのような「スイングタイプ」で実験してきましたが、接触面積が小さく、ワークが滑り落ちやすい課題がありました。そこで今回は、 パラレルグリッパー（平行開閉グリッパー） を新たに設計しました。

2本指パラレルグリッパーの構造と駆動メカニズム

今回制作した3Dモデルは、サーボモーターの回転運動を「平行四辺形リンク機構」によって平行なスライド運動に変換する構造をとっています。

• 駆動源の配置: サーボモーター（Unity上では ArticulationBody）は、リンクの根元である J1R（右側） および J1L（左側） に装備されています。
• 平行維持の仕組み: モーターが FingerR / FingerL を回転させると、それと対になる補助リンクが連動し、先端のジョー（爪）が取り付けられたプレート（PlateR / PlateL）を常に垂直に保ちます。
• 同期制御: 物理的なループ構造を ArticulationBody で組むことはできないため、スクリプト側で J1 の回転角を J2 や J3 へ適切に伝達・同期させることで、視覚的にも物理的にも正しい平行開閉動作を実現しています（詳細を後述）。

この構造により、ハサミ状のグリッパーとは異なり、ワークの面に均一な圧力をかけられるため、搬送時の安定性が飛躍的に向上しました。

2. ArticulationBodyの適用とループ構造の制約

ロボットアームの関節には、従来の HingeJoint よりも計算が安定し、設定も容易な ArticulationBody を全面的に採用しています。

階層構造の課題（ループの回避）

ArticulationBody は強力ですが、 「ループ構造をとれない（ツリー構造のみ）」 という制約があります。パラレルグリッパー特有の平行四辺形リンクは物理的なループになりますが、これをそのまま構成することはできません。

解決策： ループの一部を物理的に接続するのを諦め、スクリプトによる同期制御で平行四辺形を維持するように実装しました。物理的な結合がなくても、ジョーがワークを挟む際の摩擦力と垂直抗力は物理エンジンによってリアルに計算されます。

具体的には、駆動軸である FingerR / FingerL の回転角度をマスターとし、それに応じて PlateR / PlateL および Finger2R / Finger2L の target 角度を逆方向に同期させるアルゴリズムを実装しています。これにより、各関節が独立した ArticulationBody でありながら、リンク機構が機械的に繋がっているかのように、ジョーの面を常にワークに対して垂直（平行）に保つことが可能になりました。

3. 搬送中の摩擦力と慣性のベクトル計算

「掴む」ことはできても、「運ぶ」ときには物理の壁が立ちはだかります。アームを高速に動かすと、慣性によってワークが滑り落ちてしまうのです。

ワークが滑り落ちないための条件をベクトル方程式で見ると、以下のようになります。

摩擦力のつり合い式

ワークにかかる全外力の合計 $\mathbf{F}$ は、ニュートンの運動方程式 $\mathbf{F} = m\mathbf{a}$ に従います。

$$\sum \mathbf{f}_i + m\mathbf{g} = m\mathbf{a}$$

これを摩擦力 $\mathbf{f}_i$ について解くと：

$$\sum \mathbf{f}_i = m(\mathbf{a} - \mathbf{g})$$

最大静止摩擦力の制約

滑らないためには、この必要な摩擦力が最大静止摩擦力 $\mu |\mathbf{N}_i|$ を超えてはいけません。

$$| m(\mathbf{a} - \mathbf{g}) | \le \sum \mu |\mathbf{N}_i|$$

この式から、加速（$\mathbf{a}$）が大きすぎる、あるいは握力（$\mathbf{N}$）が足りない場合に搬送が失敗することがわかります。

実装での対策：Unity物理エンジンの制約と工夫

理論上は $\mu |\mathbf{N}| \ge | m(\mathbf{a} - \mathbf{g}) |$ を満たせば滑らないはずですが、Unityの物理エンジン（特に ArticulationBody）で高速なピック＆プレイスを行う場合、実務上は以下の制約に直面します。

• 計算精度の限界: 非常に激しい移動（高い加速度 $\mathbf{a}$）が発生すると、物理エンジンの内部計算（離散的な時間ステップでの計算）が追いつかず、指先とワークの間の接触判定が微細に浮いてしまうことがあります。
• 摩擦力の瞬断: その結果、瞬間的に垂直抗力 $\mathbf{N}$ がゼロになり、十分な摩擦力を維持できずワークがスルスルと滑り落ちてしまいます。

これを解決するため、本プロジェクトでは以下の2つのアプローチを組み合わせて対策しました。

1. 摩擦係数 μ の動的調整

ワークを掴んでいる間だけ、物理マテリアルの Static Friction と Dynamic Friction を通常ではありえないほど高い値（例：5.0以上）へ動的に設定します。物理学的な厳密さには欠けますが、これにより「計算上の微細な浮き」が発生しても、わずかな接触さえあればワークを保持し続けることが可能になります。

2. SmoothStep（Ease-in/Ease-out）の導入

加速度 $m\mathbf{a}$ を物理エンジンの計算が追従できる範囲に抑えるため、関節制御の基本となる ArticulationDrive.target への値の与え方を工夫しました。

単に関節角を線形（Linear）に変化させるのではなく、各関節を動かすとき、動作をゆっくりとはじめ、ゆっくりと終える SmoothStep を適用しています。

// コルーチンやUpdate内での実装イメージ
float elapsed = 0;
while (elapsed < duration)
{
    elapsed += Time.deltaTime;
    float t = elapsed / duration;

    // 0.0〜1.0の間をS字カーブで補間
    float smoothedT = Mathf.SmoothStep(0f, 1f, t);

    // 全ての関節(ArticulationBody)のtargetへ適用
    for (int i = 0; i < joints.Length; i++)
    {
        var drive = joints[i].xDrive;
        drive.target = Mathf.Lerp(startAngles[i], targetAngles[i], smoothedT);
        joints[i].xDrive = drive;
    }
    yield return null;
}

この「S字カーブ」を介して target を更新することで、関節にかかるトルクの急変を抑え、ワークにかかる慣性力のピークを叩いています。結果として物理計算の破綻が防げ、安定してワークを運搬できるようになりました。

注意：外部負荷（Unity Recorder）による影響

物理計算の精度は、PCのリソース状況にも左右されます。特に Unity Recorder 等で動画キャプチャを行うと、描画負荷の増大により物理演算の更新（Fixed Update）が不安定になることがあります。これにより、通常なら維持されるはずの接触判定が瞬間的に「すり抜け」てしまい、搬送中にワークを保持しきれなくなる原因となります。

4. エンドエフェクターのオブジェクト指向化

Gemini Robotics-ERのドキュメント（Robotics Overview）では、AI（LLM）が setGripperState(opened) のような抽象的な命令を生成してロボットを制御します。

しかし、実際の現場では「四面体には2本指グリッパー」「円錐形には3本指グリッパー」「平らな面には吸着ハンド」といった具合に、ワークの形状に合わせてエンドエフェクターを付け替える必要があります。これらを各個別に実装すると、ロボット本体の制御ロジックが複雑化してしまいます。

そこで、インターフェースを用いたオブジェクト指向設計により、エンドエフェクターの交換可能性（プラグイン化）を実現しました。

インターフェースによる役割の分離

まず、すべてのエンドエフェクタに共通する基底インターフェース IEndEffector を定義しました。

• ToolCenterPoint (TCP): IK計算において極めて重要です。手首関節から指先（作用点）までの正確な位置と向きを持つ Transform を提供することで、IK側は「どのアタッチメントが付いていても、その先端が正確にターゲットへリーチ」できるようになります。
• CurrentDriveTarget: エンドエフェクタの主要な駆動部（グリッパーの指の間隔など）の状態を管理し、シミュレーション上での物理的なインタラクションを支えます。

次に、これを継承して「把握（グリップ）」機能に特化した IGripper を定義します。

• Open / Close の非同期化 (Task): 指の開閉動作は物理的な時間を要します。async/await（Task）を採用することで、上位の制御層から「閉じ終わるまで待機し、完了後に次の動作（持ち上げ）へ移る」といったシーケンス制御を簡潔かつ安全に記述できます。
• フォースフィードバック: LeftFingerForce / RightFingerForce プロパティを設けることで、上位層が「今どれくらいの力で対象物を握っているか」をリアルタイムに把握し、繊細な操作を可能にします。

using System.Threading.Tasks;
using UnityEngine;

/// <summary>
/// ロボットのエンドエフェクタの基本プロパティを定義します。
/// シミュレーション環境におけるエンドエフェクタの位置決めや相互作用に必要な基本情報を提供します。
/// </summary>
public interface IEndEffector
{
    /// <summary>
    /// エンドエフェクタの先端（エッジ）の Transform を取得します。
    /// これは、相互作用の正確なポイント（グリッパーの指先など）を表します。
    /// </summary>
    Transform ToolCenterPoint { get; }

    /// <summary>
    /// エンドエフェクタの主要な駆動部の現在のターゲット位置を取得します。
    /// グリッパーの場合、これは指の間のターゲット離隔距離を表すことがあります。
    /// </summary>
    float CurrentDriveTarget { get; }
}

/// <summary>
/// <see cref="IEndEffector"/> インフェースを拡張し、力覚センサや非同期の開閉コマンドなど、
/// グリッパー特有の機能を提供します。これにより、平行グリッパー機構の詳細な制御とフィードバックが可能になります。
/// </summary>
public interface IGripper : IEndEffector
{
    /// <summary>
    /// 左指の圧力センサによって現在印加されている力をニュートン（N）単位で取得します。
    /// </summary>
    float LeftFingerForce { get; }

    /// <summary>
    /// 右指の圧力センサによって現在印加されている力をニュートン（N）単位で取得します。
    /// </summary>
    float RightFingerForce { get; }

    /// <summary>
    /// グリッパーに「開く」コマンドを非同期で送信します。
    /// </summary>
    /// <param name="targetAngularVelocity">グリッパーの指が開く際の目標角速度。</param>
    /// <returns>グリッパーが完全に開ききったときに完了するタスク。</returns>
    Task Open(float targetAngularVelocity);

    /// <summary>
    /// 指定された目標の力を印加しながら、グリッパーに「閉じる」コマンドを非同期で送信します。
    /// </summary>
    /// <param name="targetForce">対象物を把握する際の目標値（ニュートン）。0に設定した場合、物理的な限界に達するまで完全に閉じます。</param>
    /// <param name="targetAngularVelocity">グリッパーの指が閉じる際の目標角速度。</param>
    /// <returns>グリッパーが対象物に接触して目標の力に達したか、あるいは完全に閉じきったときに完了するタスク。</returns>
    Task Close(float targetForce, float targetAngularVelocity);
}

実装クラス：ParallelGripper.cs の責務

このインターフェースを実装した ParallelGripper.cs では、以下の詳細な物理制御をカプセル化しています。

1. 駆動系の同期化: 1つのターゲット値に対し、平行四辺形リンクを構成する複数の ArticulationBody を一斉に動かします。

2. 力制御（Force Feedback）のループ: 単に「閉じる」だけでなく、指先の圧力センサー（PressureSensor.cs）の値を監視し、ターゲットとする握力（targetForce）に達した瞬間に isHolding 状態へ移行させます。

3. コンプライアンス（遊び）の付与: 物体を掴んだ際、ArticulationDrive の lowerLimit と upperLimit を動的に狭めることで、物理的な「粘り」を持たせ、ワークを弾き飛ばさないように工夫しています。

private void FixedUpdate()
{
    if (_isMoving)
    {
        // 1. 衝突判定と低速接近
        if (IsColliding() && _targetMasterValue == _upperLimit)
        {
            // 接触したら、衝撃を抑えるために移動速度を大幅に下げる
            float currentSpeed = _angularVelocity / slowDownDenominator;
            _currentTarget = Mathf.MoveTowards(_currentTarget, _targetMasterValue, currentSpeed * Time.fixedDeltaTime);
        }
        else
        {
            _currentTarget = Mathf.MoveTowards(_currentTarget, _targetMasterValue, _angularVelocity * Time.fixedDeltaTime);
        }

        // 2. 目標トルク(握力)への到達判定
        if (_targetForce > 0f && IsColliding())
        {
            if (LeftFingerForce > _targetForce && RightFingerForce > _targetForce)
            {
                _isMoving = false; // Close() の非同期待機を終了
                _isHolding = true; // 保持モードへ移行
            }
        }
        
        ApplyTargetToJoints(_currentTarget, applyGap: false);
    }
    else if (_isHolding)
    {
        // 3. 保持中の力制御ループ
        // 握りすぎなら少し緩め、足りなければ少し締めるフィードバック
        float adjustment = (LeftFingerForce + RightFingerForce - 2 * _targetForce) / forceReductionDenominator;
        _currentTarget -= adjustment;

        // 保持中は applyGap を true にし、物理的な「粘り」を持たせる
        ApplyTargetToJoints(_currentTarget, applyGap: true);
    }
}

private void ApplyTargetToJoints(float target, bool applyGap)
{
    foreach (var body in _allFingerBodies)
    {
        var drive = body.xDrive;
        drive.target = target;

        if (applyGap)
        {
            // ターゲット値に対してわずかな「遊び」を許容する
            // これにより、物理エンジンの計算誤差による微振動を吸収し、グリップが安定する
            drive.lowerLimit = target - minMaxGap;
            drive.upperLimit = target;
        }
        body.xDrive = drive;
    }
}

この設計によるメリット

この抽象化によって、PickAndPlace.cs（ロボット本体の脳）は以下のようなシンプルな記述で済むようになります。

// グリッパーの種類を問わず、同じコードで制御可能
IGripper gripper = endEffector.GetComponent<IGripper>();
await gripper.Open(30.0f);   // 30度/秒で開くのを待つ
await gripper.Close(5.0f, 30.0f); // 5Nの力で掴むのを待つ

今後、吸引型（Suction）ハンドを追加する場合も、IEndEffector を実装した新しいクラスを作るだけで、既存のIKロジックやAI連携ロジックを一切修正することなく、シミュレーションのバリエーションを増やすことができます。

5. 動作確認とソフトウェア構成

自作IKとパラレルグリッパーを組み合わせた「ピック＆プレイス」のテストシーケンスを実施しました。

ソフトウェア構成図

今回のシミュレータは、上位層の「プランナー」と下位層の「フィジカルコントローラー」に分けて設計しています。Geminiから届く move や setGripperState という抽象的な命令を、具体的なIK計算と ArticulationBody のトルク制御へと変換する構造です。

スクリプトの関係性

オブジェクト指向に基づき、各コンポーネントの責務を明確に分離しました。

• PickAndPlace.cs: メインコントローラー。動作シーケンスを管理し、IKの結果を各関節に反映させます。
• ParallelGripper.cs: IGripper インターフェースを実装。指先の並行維持や、圧力センサーを用いた「力制御」を担当します。
• PressureSensor.cs: ArticulationBody の jointForce や衝突判定から、グリップにかかっている荷重をリアルタイムに算出します。

動作テスト動画

実際にワークを検出し、スムーズな加速（SmoothStep）を伴って搬送する様子です。

動画では、以下のポイントを確認できます。

1. IKによる正確なアプローチ: ターゲット座標に対して、エンドエフェクターが垂直に進入している。
2. アダプティブなグリップ: ワークのサイズに合わせてジョーが停止し、一定の力（targetForce）を維持している。
3. 慣性に耐える搬送: 急停止・急旋回時も、動的摩擦制御と加速度補間によってワークが滑り落ちていない。

まとめと次回の展望

今回の検証で、自作IKと物理演算を組み合わせた安定したピック＆プレイスの基礎が整いました。Geminiとの対話型制御においても、物理的に「まともな」グリッパーがあることで、より現実に即した実験が可能になります。

今後は、構築したアーム制御系と Gemini Robotics-ER との連携を少しずつ進めていく予定です。AIからの抽象的な指示を、実際の物理シミュレーション上での「仕分け動作」へどこまで落とし込めるか、その試行錯誤の過程や動作の様子を、準備が整い次第共有できればと考えています。

本記事で紹介した制作物

以下のGitHubプロジェクトで公開します。