Genesis 入門 (5) - インバースキネマティクス と モーションプランニング
「Genesis」の「インバースキネマティクス」と「モーションプランニング」についてまとめました。
前回
1. インバースキネマティクス と モーションプランニング
「Genesis」で「インバースキネマティクス」 (IK) と「モーションプランニング」を解析して、簡単な把持タスクを実行する方法を示します。
・インバースキネマティクス（Inverse Kinematics, IK）
エンドエフェクター (ロボットの手など) を目標位置や姿勢に動かすために、リンクと関節の動きを計算する技術。
・モーションプランニング (Motion Planning)
ロボットが与えられた環境内で、障害物を避けながら目的地に到達するための経路を計画する技術。
2. シーンの準備
シーンを作成し、ロボットアームと小さな立方体をロードしてシーンを構築し、制御ゲインを設定します。
import numpy as np
import genesis as gs
# 初期化
gs.init(backend=gs.gpu)
# シーンの作成
scene = gs.Scene(
viewer_options = gs.options.ViewerOptions(
camera_pos = (3, -1, 1.5),
camera_lookat = (0.0, 0.0, 0.5),
camera_fov = 30,
max_FPS = 60,
),
sim_options = gs.options.SimOptions(
dt = 0.01,
substeps = 4, # より安定した把持接触のため
),
show_viewer = True,
)
# シーンにエンティティを追加
plane = scene.add_entity(
gs.morphs.Plane(),
)
cube = scene.add_entity(
gs.morphs.Box(
size = (0.04, 0.04, 0.04),
pos = (0.65, 0.0, 0.02),
)
)
franka = scene.add_entity(
gs.morphs.MJCF(file='xml/franka_emika_panda/panda.xml'),
)
# シーンのビルド
scene.build()
# 自由度
motors_dof = np.arange(7)
fingers_dof = np.arange(7, 9)
# エンティティの自由度の位置ゲインを設定
franka.set_dofs_kp(
np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
)
# エンティティの自由度の速度ゲインを設定
franka.set_dofs_kv(
np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
)
# エンティティの自由度の力の範囲を設定 (安全のため)
franka.set_dofs_force_range(
np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
np.array([ 87, 87, 87, 87, 12, 12, 12, 100, 100]),
)
3. OMPLのインストール
「OMPL」は、ロボットの「モーションプランニング」のためのオープンソースライブラリです。主にロボット工学や自動化システムで、ロボットが目標地点に障害物を避けながら到達するための経路を計算する際に使用されます。インストールページの指示に従ってインストールできます。
MacのPython 3.10の場合は、ここから「ompl-1.6.0-cp310-cp310-macosx_13_0_arm64.whl」をダウンロードしてpipでインストールします。
pip install ompl-1.6.0-cp310-cp310-macosx_13_0_arm64.whl4. インバースキネマティクス と モーションプランニング の実行
4-1. エンドエフェクタをキューブ近くまで移動
「Genesis」の「インバースキネマティクス」「モーションプランニング」は非常にシンプルです。それぞれ1つの関数呼び出しで実行できます。
# エンドエフェクタのリンクの取得
end_effector = franka.get_link('hand')
# 単一の目標リンクのインバースキネマティクスを計算
qpos = franka.inverse_kinematics(
link = end_effector, # エンドエフェクタのリンク
pos = np.array([0.65, 0.0, 0.25]), # 位置
quat = np.array([0, 1, 0, 0]), # クォータニオン
)
# qpos_goalまでのパスをモーションプランニング
qpos[-2:] = 0.04
path = franka.plan_path(
qpos_goal = qpos, # 目標関節位置
num_waypoints = 200, # 持続時間2秒
)
# モーションプランを実行
for waypoint in path:
# エンティティの自由度に目標位置を設定
franka.control_dofs_position(waypoint)
scene.step()
# ステップ実行
for i in range(100):
scene.step()「IKソルビング」では、ロボットのIKソルバーにどのリンクが「エンドエフェクタ」であるかを伝え、「ターゲットポーズ」を指定します。
次に、「モーションプランナー」に「目標関節位置」 (qpos) を伝えると、計画され平滑化されたウェイポイントのリストが返されます。パスを実行した後、コントローラをさらに 100 ステップ実行します。これは、「PDコントローラ」を使用しているため、目標位置と現在位置の間にギャップがあるためです。したがって、コントローラをもう少し長く実行して、ロボットが計画された軌道の最後のウェイポイントに到達できるようにします。
2-2. エンドエフェクタを下に移動してキューブをつかみ持ち上げる
エンドエフェクタを下に移動してキューブをつかみ持ち上げます。
# リーチ
qpos = franka.inverse_kinematics(
link = end_effector,
pos = np.array([0.65, 0.0, 0.135]),
quat = np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
# ステップ実行
for i in range(100):
scene.step()
# グリップ
franka.control_dofs_position(qpos[:-2], motors_dof)
franka.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)
# ステップ実行
for i in range(100):
scene.step()
# リフト
qpos = franka.inverse_kinematics(
link=end_effector,
pos=np.array([0.65, 0.0, 0.3]),
quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
# ステップ実行
for i in range(200):
scene.step()物体を掴む際は、2つのグリッパー自由度に力制御を使用し、0.5N の把持力を適用しました。すべてがうまくいけば、物体が掴まれて持ち上げられるのがわかります。
Genesisの動作確認 (5)https://t.co/iOmGKOpvEv pic.twitter.com/o3OmUTDf3x
— 布留川英一 / Hidekazu Furukawa (@npaka123) December 21, 2024
【おまけ】 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 = (3, -1, 1.5),
camera_lookat = (0.0, 0.0, 0.5),
camera_fov = 30,
max_FPS = 60,
),
sim_options = gs.options.SimOptions(
dt = 0.01,
substeps = 4, # より安定した把持接触のため
),
show_viewer = True,
)
# シーンにエンティティを追加
plane = scene.add_entity(
gs.morphs.Plane(),
)
cube = scene.add_entity(
gs.morphs.Box(
size = (0.04, 0.04, 0.04),
pos = (0.65, 0.0, 0.02),
)
)
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):
# 自由度
motors_dof = np.arange(7)
fingers_dof = np.arange(7, 9)
# エンティティの自由度の位置ゲインを設定
franka.set_dofs_kp(
np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
)
# エンティティの自由度の速度ゲインを設定
franka.set_dofs_kv(
np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
)
# エンティティの自由度の力の範囲を設定 (安全のため)
franka.set_dofs_force_range(
np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
np.array([ 87, 87, 87, 87, 12, 12, 12, 100, 100]),
)
# エンドエフェクタのリンクの取得
end_effector = franka.get_link('hand')
# 単一の目標リンクのインバースキネマティクスを計算
qpos = franka.inverse_kinematics(
link = end_effector,
pos = np.array([0.65, 0.0, 0.25]),
quat = np.array([0, 1, 0, 0]),
)
# # qpos_goalまでのパスをモーションプランニング
qpos[-2:] = 0.04
path = franka.plan_path(
qpos_goal = qpos,
num_waypoints = 200, # 2s duration
)
# モーションプランを実行
for waypoint in path:
franka.control_dofs_position(waypoint)
scene.step()
# ステップ実行
for i in range(100):
scene.step()
# リーチ
qpos = franka.inverse_kinematics(
link = end_effector,
pos = np.array([0.65, 0.0, 0.135]),
quat = np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
for i in range(100):
scene.step()
# グリップ
franka.control_dofs_position(qpos[:-2], motors_dof)
franka.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)
# ステップ実行
for i in range(100):
scene.step()
# リフト
qpos = franka.inverse_kinematics(
link=end_effector,
pos=np.array([0.65, 0.0, 0.3]),
quat=np.array([0, 1, 0, 0]),
)
franka.control_dofs_position(qpos[:-2], motors_dof)
# ステップ実行
for i in range(200):
scene.step()
if __name__ == "__main__":
main()
