Genesis 入門 (3) - ロボットの操作

2024年12月20日 18:27

「Genesis」のロボットの操作についてまとめました。

・Control Your Robot

前回

1. ロボットの操作

ロボットアームは「制御力」(actuation force) を与えないと、重力で落下します。Genesisには、「目標関節位置」または「速度」を入力として受け取る「PDコントローラ」が組み込まれています。また、各関節に直接「トルク/力」を設定することもできます。

2. 関節と自由度

2-1. 関節と自由度

ロボットシミュレーションの文脈では、「関節」(joint)と「自由度」(dof : degree-of-freedom) は関連しているものの異なる概念です。

・関節 (joint)
ロボットの2つのリンク（剛体）を接続し、相対的な動きを可能にする機構。動きを特定の種類（回転、並進など）に限定。
・自由度 (dof : degree-of-freedom)
動力学的または運動学的に独立して制御可能な動きの数。ロボットが環境内で取れる可能な動きの幅を示す。

今回扱うFrankaアームは、アームに7つの回転関節、グリッパーに2つの並進関節を持っています。すべての関節は1自由度のため、9自由度の関節体となっています。より一般的なケースでは、フリージョイント (6自由度) やボールジョイント (3自由度) のように、複数の自由度を持つ関節タイプも存在します。一般的に、各自由度はモーターとして考えることができ、独立制御できます。

2-2. 関節名を自由度のインデックスにマッピング

どの関節（自由度）を制御するかを知るために、URDF/MJCFファイルで（ユーザーとして）定義した関節名をシミュレータ内の実際の自由度インデックスにマッピングする必要があります。

# 関節名を自由度のインデックスにマッピング
jnt_names = [
    'joint1',
    'joint2',
    'joint3',
    'joint4',
    'joint5',
    'joint6',
    'joint7',
    'finger_joint1',
    'finger_joint2',
]
dofs_idx = [franka.get_joint(name).dof_idx_local for name in jnt_names]

「joint.dof_idx_local」で、ロボットエンティティ自体に対する自由度のローカルインデックスを取得しています。「joint.dof_idx」で、シーン内の各関節のグローバル自由度インデックスにアクセスすることもできます。

2-3. 各自由度の制御ゲインの設定

「制御ゲイン」 (Gain) は、制御システムにおいて、入力と出力の関係を調整するための係数です。制御量（トルクや力）を目標値と現在の値の差（偏差）に比例して計算します。偏差が大きい場合、大きな力で目標値に近づけようとし、偏差が小さくなるにつれて、出力する力も小さくなります。

通常、これらの情報は「MJCF」または「URDF」から解析されますが、手動でチューニングするか、オンラインで十分にチューニングされた値を参照することを推奨します。

# エンティティの自由度の位置ゲインを設定
franka.set_dofs_kp(
    kp             = np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
    dofs_idx_local = dofs_idx,
)

# エンティティの自由度の速度ゲインを設定
franka.set_dofs_kv(
    kv             = np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
    dofs_idx_local = dofs_idx,
)

# エンティティの自由度の力の範囲を設定 (安全のため)
franka.set_dofs_force_range(
    lower          = np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
    upper          = np.array([ 87,  87,  87,  87,  12,  12,  12,  100,  100]),
    dofs_idx_local = dofs_idx,
)

これらのAPIは、設定する実際の値と、対応する自由度のインデックスを受け取ります。ほとんどの制御関連APIはこの規則に従っています。

3. ハードリセットによるロボット操作

物理的に現実的な「PDコントローラ」を使用する代わりに、ロボットの構成を手動で設定してみます。これらのAPIは物理法則に従わずにロボットの状態を突然変更することができます。

# ハードリセット
for i in range(150):
    if i < 50:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]), dofs_idx)
    elif i < 100:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]), dofs_idx)
    else:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]), dofs_idx)

    scene.step()

ビューアーをオンにしている場合、50ステップごとにロボットの状態が変化するのが見えます。

4. PDコントローラによるロボット操作

組み込みの「PDコントローラ」を使ってロボットを制御してみます。「Genesis」のAPI設計は構造化されたパターンに従っています。set_dofs_positionを使って自由度の位置を直接設定しました。ここでは、set_*をcontrol_*に変更して、対応する制御器APIを使用します。以下で、ロボットを制御するさまざまな方法を示します。

# PDコントローラ
for i in range(1250):
    if i == 0:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]),
            dofs_idx,
        )
    elif i == 250:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]),
            dofs_idx,
        )
    elif i == 500:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
            dofs_idx,
        )
    elif i == 750:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])[1:],
            dofs_idx[1:],
        )
        # エンティティの自由度の目標速度を設定
        franka.control_dofs_velocity(
            np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0])[:1],
            dofs_idx[:1],
        )
    elif i == 1000:
        # エンティティの自由度の目標制御力を設定
        franka.control_dofs_force(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
            dofs_idx,
        )

    # 制御コマンドを元に計算された制御力
    print('control force:', franka.get_dofs_control_force(dofs_idx))

    # 実際に受ける力
    print('internal force:', franka.get_dofs_force(dofs_idx))

    scene.step()

・ステップ0から500まで
すべての自由度に対して位置制御を使用し、ロボットを3つの目標位置に順次移動させています。control_* APIでは、目標値が一度設定されると、内部に保存されるため、目標が同じである限り、以降のステップで繰り返しコマンドをシミュレーションに送る必要はありません。
・ステップ750
異なる自由度に対して異なる制御が可能であることを示しています：最初の自由度（dof 0）には速度コマンドを送り、残りは位置制御コマンドに従います
・ステップ1000
トルク（力）制御に切り替え、すべての自由度にゼロ力コマンドを送ると、ロボットは再び重力によって床に落下します。

各ステップの最後に、get_dofs_control_force() と get_dofs_force() の2種類の力を出力します。

・get_dofs_control_force()
PDコントローラによって適用される力を返します。位置制御や速度制御の場合、これは目標コマンドと制御ゲインを使用して計算します。力（トルク）制御の場合、これは入力制御コマンドと同じになります。
・get_dofs_force()
各自由度が実際に経験する力を返します。これはPDコントローラによって適用される力と、衝突力やコリオリ力などの他の内部力の組み合わせになります。

・衝突力 (Impact Force)
物体同士が接触する際に生じる力。短い時間で大きな力が発生するのが特徴。
・コリオリ力 (Coriolis Force)
回転する参照フレーム内で観測される見かけの力の一種で、物体が移動する際に、その動きが曲げられるように見える原因となる。

Genesisの動作確認 (3)https://t.co/BVNlo2UsXA pic.twitter.com/Oa8gofc5UZ
— 布留川英一 / Hidekazu Furukawa (@npaka123) December 21, 2024

5. 完全なコード

以下は、完全なコードです。

import numpy as np
import genesis as gs

# 初期化
gs.init(backend=gs.gpu)

# シーンの作成
scene = gs.Scene(
    viewer_options = gs.options.ViewerOptions(
        camera_pos    = (0, -3.5, 2.5),
        camera_lookat = (0.0, 0.0, 0.5),
        camera_fov    = 30,
        res           = (960, 640),
        max_FPS       = 60,
    ),
    sim_options = gs.options.SimOptions(
        dt = 0.01,
    ),
    show_viewer = True,
)

# シーンにエンティティを追加
plane = scene.add_entity(
    gs.morphs.Plane(),
)
franka = scene.add_entity(
    gs.morphs.MJCF(
        file  = 'xml/franka_emika_panda/panda.xml',
    ),
)

# シーンのビルド
scene.build()

# 関節名を自由度のインデックスにマッピング
jnt_names = [
    'joint1',
    'joint2',
    'joint3',
    'joint4',
    'joint5',
    'joint6',
    'joint7',
    'finger_joint1',
    'finger_joint2',
]
dofs_idx = [franka.get_joint(name).dof_idx_local for name in jnt_names]

# エンティティの自由度の位置ゲインを設定
franka.set_dofs_kp(
    kp             = np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
    dofs_idx_local = dofs_idx,
)

# エンティティの自由度の速度ゲインを設定
franka.set_dofs_kv(
    kv             = np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
    dofs_idx_local = dofs_idx,
)

# エンティティの自由度の力の範囲を設定 (安全のため)
franka.set_dofs_force_range(
    lower          = np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
    upper          = np.array([ 87,  87,  87,  87,  12,  12,  12,  100,  100]),
    dofs_idx_local = dofs_idx,
)

# ハードリセット
for i in range(150):
    if i < 50:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]), dofs_idx)
    elif i < 100:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]), dofs_idx)
    else:
        # エンティティの自由度の位置を設定
        franka.set_dofs_position(np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]), dofs_idx)

    scene.step()

# PDコントロール
for i in range(1250):
    if i == 0:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]),
            dofs_idx,
        )
    elif i == 250:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]),
            dofs_idx,
        )
    elif i == 500:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
            dofs_idx,
        )
    elif i == 750:
        # エンティティの自由度の目標位置を設定
        franka.control_dofs_position(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])[1:],
            dofs_idx[1:],
        )
        # エンティティの自由度の目標速度を設定
        franka.control_dofs_velocity(
            np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0])[:1],
            dofs_idx[:1],
        )
    elif i == 1000:
        # エンティティの自由度の目標制御力を設定
        franka.control_dofs_force(
            np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
            dofs_idx,
        )

    # 制御コマンドを元に計算された制御力
    print('control force:', franka.get_dofs_control_force(dofs_idx))

    # 実際に受ける力
    print('internal force:', franka.get_dofs_force(dofs_idx))

    scene.step()

【おまけ】 Mac版のコード

import genesis as gs
import numpy as np

def main():
    # 初期化
    gs.init(backend=gs.cpu)

    # シーンの作成
    scene = gs.Scene(
        viewer_options = gs.options.ViewerOptions(
            camera_pos    = (0, -3.5, 2.5),
            camera_lookat = (0.0, 0.0, 0.5),
            camera_fov    = 30,
            res           = (960, 640),
            max_FPS       = 60,
        ),
        sim_options = gs.options.SimOptions(
            dt = 0.01,
        ),
        show_viewer = True,
    )

    # シーンにエンティティを追加
    plane = scene.add_entity(
        gs.morphs.Plane(),
    )
    franka = scene.add_entity(
        gs.morphs.MJCF(
            file  = 'xml/franka_emika_panda/panda.xml',
        ),
    )

    # シーンのビルド
    scene.build()

    # スレッドの開始
    gs.tools.run_in_another_thread(fn=run_sim, args=(scene, franka))

    # シミュレータの開始
    scene.viewer.start()


def run_sim(scene, franka):
    # 関節名を自由度のインデックスにマッピング
    jnt_names = [
        'joint1',
        'joint2',
        'joint3',
        'joint4',
        'joint5',
        'joint6',
        'joint7',
        'finger_joint1',
        'finger_joint2',
    ]
    dofs_idx = [franka.get_joint(name).dof_idx_local for name in jnt_names]

    # エンティティの自由度の位置ゲインを設定
    franka.set_dofs_kp(
        kp             = np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
        dofs_idx_local = dofs_idx,
    )

    # エンティティの自由度の速度ゲインを設定
    franka.set_dofs_kv(
        kv             = np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
        dofs_idx_local = dofs_idx,
    )

    # エンティティの自由度の力の範囲を設定 (安全のため)
    franka.set_dofs_force_range(
        lower          = np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
        upper          = np.array([ 87,  87,  87,  87,  12,  12,  12,  100,  100]),
        dofs_idx_local = dofs_idx,
    )

    # ハードリセット
    for i in range(150):
        # エンティティの自由度の位置を設定
        if i < 50:
            franka.set_dofs_position(np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]), dofs_idx)
        # エンティティの自由度の位置を設定
        elif i < 100:
            franka.set_dofs_position(np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]), dofs_idx)
        # エンティティの自由度の位置を設定
        else:
            franka.set_dofs_position(np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]), dofs_idx)

        scene.step()

    # PDコントロール
    for i in range(1250):
        if i == 0:
            # エンティティの自由度の目標位置を設定
            franka.control_dofs_position(
                np.array([1, 1, 0, 0, 0, 0, 0, 0.04, 0.04]),
                dofs_idx,
            )
        elif i == 250:
            # エンティティの自由度の目標位置を設定
            franka.control_dofs_position(
                np.array([-1, 0.8, 1, -2, 1, 0.5, -0.5, 0.04, 0.04]),
                dofs_idx,
            )
        elif i == 500:
            # エンティティの自由度の目標位置を設定
            franka.control_dofs_position(
                np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
                dofs_idx,
            )
        elif i == 750:
            # エンティティの自由度の目標位置を設定
            franka.control_dofs_position(
                np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])[1:],
                dofs_idx[1:],
            )
            # エンティティの自由度の目標速度を設定
            franka.control_dofs_velocity(
                np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0])[:1],
                dofs_idx[:1],
            )
        elif i == 1000:
            # エンティティの自由度の目標制御力を設定
            franka.control_dofs_force(
                np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]),
                dofs_idx,
            )

        # 制御コマンドを元に計算された制御力
        print('control force:', franka.get_dofs_control_force(dofs_idx))

        # 実際に受ける力
        print('internal force:', franka.get_dofs_force(dofs_idx))

        scene.step()


if __name__ == "__main__":
    main()