Genesis 入門 (6) - 柔軟なIKと並列IK

npaka

2024年12月21日 08:01

「Genesis」の「柔軟なIK」と「並列IK」についてまとめました。

・Advanced and Parallel IK

前回

1. IKの高度な使い方

「Genesis」の「IKソルバー」には、多くの強力な機能があります。この例では、より柔軟なターゲットポーズを受け入れるように「IKソルバー」を構成する方法と、バッチ設定でロボットを解決する方法を説明します。

2. 複数のエンドエフェクタリンクを含むIK

この例では、ロボットグリッパーの左指と右指を2つの別々のターゲットリンクとして使用します。また、各リンクのターゲットポーズとして完全な 6-DoF ポーズを使用する代わりに、それらの位置とZ軸の方向のみを考慮して解決します。

import numpy as np

import genesis as gs

# 初期化
gs.init(seed=0, precision='32', logging_level='debug')

# シーンの作成
scene = gs.Scene(
    viewer_options= gs.options.ViewerOptions(
        camera_pos=(2.0, -2, 1.5),
        camera_lookat=(0.0, 0.0, 0.0),
        camera_fov=40,
    ),
    rigid_options=gs.options.RigidOptions(
        enable_joint_limit=False,
        enable_collision=False,
    ),
)

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

# 視覚化のための2つのターゲッリンク
target_left = scene.add_entity(
    gs.morphs.Mesh(
        file='meshes/axis.obj',
        scale=0.1,
    ),
    surface=gs.surfaces.Default(color=(1, 0.5, 0.5, 1)),
)
target_right = scene.add_entity(
    gs.morphs.Mesh(
        file='meshes/axis.obj',
        scale=0.1,
    ),
    surface=gs.surfaces.Default(color=(0.5, 1.0, 0.5, 1)),
)

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

target_quat = np.array([0, 1, 0, 0])
center = np.array([0.4, -0.2, 0.25])
r = 0.1

left_finger = robot.get_link('left_finger')
right_finger = robot.get_link('right_finger')

for i in range(0, 2000):
    target_pos_left = center + np.array([np.cos(i/360*np.pi), np.sin(i/360*np.pi), 0]) * r
    target_pos_right = target_pos_left + np.array([0.0, 0.03, 0])

    target_left.set_qpos(np.concatenate([target_pos_left, target_quat]))
    target_right.set_qpos(np.concatenate([target_pos_right, target_quat]))
    
    q = robot.inverse_kinematics_multilink(
        links    = [left_finger, right_finger], # リンクのリスト
        poss     = [target_pos_left, target_pos_right], # 位置のリスト
        quats    = [target_quat, target_quat], # 方向のリスト
        rot_mask = [False, False, True], # Z軸の方向のみを制限
    )

    # IKは視覚化を目的としているため、scene.step()は呼ばず、状態とビジュアライザーの更新のみ行っている
    # 実アプリでは、robot.control_dofs_position()とscene.step()を使用する必要がある
    robot.set_dofs_position(q)
    scene.visualizer.update()

表示される内容は次のとおりです。

・複数のターゲットリンクを考慮してIKを解決するには robot.inverse_kinematics_multilink() を使用します。ターゲットリンクとターゲット位置とターゲット方向のリストを渡します。

・rot_mask を使用して、制限しない軸の方向をマスクします。今回は、両方の指を下向きにしたいので、Z軸は下向きにする必要があります。ただし、水平面での回転を制限することにはあまり関心がありません。同様に、x/y/z 軸に沿った位置をマスクできる pos_mask もあります。

・今回は、物理法則が考慮されていないため、ロボットの位置と2つのターゲットリンクを設定した後は、scene.step() を介して物理シミュレーションを呼び出す必要はありません。代わりに、scene.visualizer.update() を呼び出してビジュアライザーを更新し、ビューア (およびカメラがある場合はカメラ) の変更を反映させるだけです。

・ターゲットリンクの状態を設定するために set_qpos を使用します。qpos は、一般化座標におけるエンティティの構成を表します。単一のアームの場合、qpos は dofs_position と同一であり、すべてのジョイント (回転 + 直動) に 1自由度しかありません。フリージョイントを介してワールドに接続されたフリーメッシュの場合、このジョイントには6自由度 (3つの並進 + 3つの回転) がありますが、一般化座標 q は7ベクトルであり、基本的に xyz並進 + wxyzクォータニオンであるため、qpos は dofs_position とは異なります。set_qpos() と set_dofs_position() の両方を使用して状態を設定できますが、ここでは目的のクォータニオンがわかっているため、qpos を計算する方が簡単です。この違いは回転の表現方法から生じ、回転は 3ベクトル (3つの軸の周りの回転) または 4ベクトル (wxyzクォータニオン) のいずれかで表現できます。

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

3. 並列シミュレーション用のIK

「Genesis」では、バッチ環境でもIKを解決できます。16個の並列環境を生成し、ロボットのエンドエフェクタをそれぞれ異なる角速度で回転させてみます。

import numpy as np
import genesis as gs

# 初期化
gs.init()

# シーンの作成
scene = gs.Scene(
    viewer_options= gs.options.ViewerOptions(
        camera_pos    = (0.0, -2, 1.5),
        camera_lookat = (0.0, 0.0, 0.5),
        camera_fov    = 40,
        max_FPS       = 200,
    ),
    rigid_options=gs.options.RigidOptions(
        enable_joint_limit = False,
    ),
)

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

# シーンのビルド
n_envs = 16
scene.build(n_envs=n_envs, env_spacing=(1.0, 1.0))

target_quat = np.tile(np.array([0, 1, 0, 0]), [n_envs, 1]) # 下向き
center = np.tile(np.array([0.4, -0.2, 0.25]), [n_envs, 1])
angular_speed = np.random.uniform(-10, 10, n_envs)
r = 0.1

ee_link = robot.get_link('hand')

for i in range(0, 1000):
    target_pos = np.zeros([n_envs, 3])
    target_pos[:, 0] = center[:, 0] + np.cos(i/360*np.pi*angular_speed) * r
    target_pos[:, 1] = center[:, 1] + np.sin(i/360*np.pi*angular_speed) * r
    target_pos[:, 2] = center[:, 2]
    target_q = np.hstack([target_pos, target_quat])

    q = robot.inverse_kinematics(
        link     = ee_link,
        pos      = target_pos,
        quat     = target_quat,
        rot_mask = [False, False, True], # デモ目的: Z軸の方向のみを制限
    )

    robot.set_qpos(q)
    scene.step()

並列環境を扱う場合、ターゲットポーズ変数に追加のバッチ次元を挿入することを確認するだけです。

【おまけ】Macのコード

import genesis as gs
import numpy as np

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

    # シーンの作成
    scene = gs.Scene(
        viewer_options= gs.options.ViewerOptions(
            camera_pos=(2.0, -2, 1.5),
            camera_lookat=(0.0, 0.0, 0.0),
            camera_fov=40,
        ),
        rigid_options=gs.options.RigidOptions(
            enable_joint_limit=False,
            enable_collision=False,
        ),
    )

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

    # 視覚化のための2つのターゲッリンク
    target_left = scene.add_entity(
        gs.morphs.Mesh(
            file='meshes/axis.obj',
            scale=0.1,
        ),
        surface=gs.surfaces.Default(color=(1, 0.5, 0.5, 1)),
    )
    target_right = scene.add_entity(
        gs.morphs.Mesh(
            file='meshes/axis.obj',
            scale=0.1,
        ),
        surface=gs.surfaces.Default(color=(0.5, 1.0, 0.5, 1)),
    )

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

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

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


def run_sim(scene, robot, target_left, target_right):
    target_quat = np.array([0, 1, 0, 0])
    center = np.array([0.4, -0.2, 0.25])
    r = 0.1

    left_finger = robot.get_link('left_finger')
    right_finger = robot.get_link('right_finger')

    for i in range(0, 2000):
        target_pos_left = center + np.array([np.cos(i/9*np.pi), np.sin(i/9*np.pi), 0]) * r
        target_pos_right = target_pos_left + np.array([0.0, 0.03, 0])

        target_left.set_qpos(np.concatenate([target_pos_left, target_quat]))
        target_right.set_qpos(np.concatenate([target_pos_right, target_quat]))
        
        q = robot.inverse_kinematics_multilink(
            links    = [left_finger, right_finger], # リンクのリスト
            poss     = [target_pos_left, target_pos_right], # 位置のリスト
            quats    = [target_quat, target_quat], # 方向のリスト
            rot_mask = [False, False, True], # Z軸の方向のみを制限
        )

        # IKは視覚化を目的としているため、scene.step() は呼ばず、状態とビジュアライザーの更新のみ行っている
        # 実アプリでは、robot.control_dofs_position()とscene.step()を使用する必要がある
        robot.set_dofs_position(q)
        scene.visualizer.update()


if __name__ == "__main__":
    main()

import genesis as gs
import numpy as np

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

    # シーンの作成
    scene = gs.Scene(
        viewer_options= gs.options.ViewerOptions(
            camera_pos    = (0.0, -2, 1.5),
            camera_lookat = (0.0, 0.0, 0.5),
            camera_fov    = 40,
            max_FPS       = 200,
        ),
        rigid_options=gs.options.RigidOptions(
            enable_joint_limit = False,
        ),
    )

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

    # シーンのビルド
    n_envs = 16
    scene.build(n_envs=n_envs, env_spacing=(1.0, 1.0))

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

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


def run_sim(scene, robot, n_envs):
    target_quat = np.tile(np.array([0, 1, 0, 0]), [n_envs, 1]) # 下向き
    center = np.tile(np.array([0.4, -0.2, 0.25]), [n_envs, 1])
    angular_speed = np.random.uniform(-10, 10, n_envs)
    r = 0.1

    ee_link = robot.get_link('hand')

    for i in range(0, 1000):
        target_pos = np.zeros([n_envs, 3])
        target_pos[:, 0] = center[:, 0] + np.cos(i/360*np.pi*angular_speed) * r
        target_pos[:, 1] = center[:, 1] + np.sin(i/360*np.pi*angular_speed) * r
        target_pos[:, 2] = center[:, 2]
        target_q = np.hstack([target_pos, target_quat])

        q = robot.inverse_kinematics(
            link     = ee_link,
            pos      = target_pos,
            quat     = target_quat,
            rot_mask = [False, False, True], # デモ目的: Z軸の方向のみを制限
        )

        robot.set_qpos(q)
        scene.step()


if __name__ == "__main__":
    main()