Genesis 入門 (10) - ソフトロボット
「Genesis」の「ソフトロボット」についてまとめました。
前回
1. ソフトロボット
「Genesis」は、「ソフトロボット」の「MPM」と「FEM」を使用した「Volumetric Muscle シミュレーション」をサポートしています。次の例では、正弦波制御信号によって作動する球体ボディを備えた非常にシンプルな「ソフトロボット」を示します。
import numpy as np
import genesis as gs
# 初期化
gs.init(seed=0, precision='32', logging_level='debug')
# シーンの作成
dt = 5e-4
scene = gs.Scene(
sim_options=gs.options.SimOptions(
substeps=10,
gravity=(0, 0, 0),
),
viewer_options= gs.options.ViewerOptions(
camera_pos=(1.5, 0, 0.8),
camera_lookat=(0.0, 0.0, 0.0),
camera_fov=40,
),
mpm_options=gs.options.MPMOptions(
dt=dt,
lower_bound=(-1.0, -1.0, -0.2),
upper_bound=( 1.0, 1.0, 1.0),
),
fem_options=gs.options.FEMOptions(
dt=dt,
damping=45.,
),
vis_options=gs.options.VisOptions(
show_world_frame=False,
),
)
# シーンにエンティティを追加
scene.add_entity(morph=gs.morphs.Plane())
E, nu = 3.e4, 0.45
rho = 1000.
robot_mpm = scene.add_entity(
morph=gs.morphs.Sphere(
pos=(0.5, 0.2, 0.3),
radius=0.1,
),
material=gs.materials.MPM.Muscle(
E=E,
nu=nu,
rho=rho,
model='neohooken',
),
)
robot_fem = scene.add_entity(
morph=gs.morphs.Sphere(
pos=(0.5, -0.2, 0.3),
radius=0.1,
),
material=gs.materials.FEM.Muscle(
E=E,
nu=nu,
rho=rho,
model='stable_neohooken',
),
)
# シーンのビルド
scene.build()
# 実行
scene.reset()
for i in range(1000):
actu = np.array([0.2 * (0.5 + np.sin(0.01 * np.pi * i))])
robot_mpm.set_actuation(actu)
robot_fem.set_actuation(actu)
scene.step()
コードの大部分は、通常の変形可能なEntityをインスタンス化するのと比べるとかなり標準的です。 役立つのは、次の2つの違いだけです。
・ソフトロボット robot_mpm と robot_fem をインスタンス化するときは、それぞれ gs.materials.MPM.Muscle と gs.materials.FEM.Muscle というマテリアルを使用します。
・シミュレーションをステップ実行するときは、robot_mpm.set_actuation または robot_fem.set_actuation を使用して筋肉のアクチュエーションを設定します。
デフォルトでは、ロボットのボディ全体に広がる筋肉は1つだけであり、筋肉の方向は地面に対して垂直です [0, 0, 1]。
次の例では、次に示すように筋肉のグループと方向を設定して、前方に這うワームをシミュレートする方法を示します。(完全なスクリプトは tutorials/advanced_worm.py にあります。)
# シーンにエンティティを追加
worm = scene.add_entity(
morph=gs.morphs.Mesh(
file='meshes/worm/worm.obj',
pos=(0.3, 0.3, 0.001),
scale=0.1,
euler=(90, 0, 0),
),
material=gs.materials.MPM.Muscle(
E=5e5,
nu=0.45,
rho=10000.,
model='neohooken',
n_groups=4,
),
)
# 筋肉の指定
def set_muscle_by_pos(robot):
if isinstance(robot.material, gs.materials.MPM.Muscle):
pos = robot.get_state().pos
n_units = robot.n_particles
elif isinstance(robot.material, gs.materials.FEM.Muscle):
pos = robot.get_state().pos[robot.get_el2v()].mean(1)
n_units = robot.n_elements
else:
raise NotImplementedError
pos = pos.cpu().numpy()
pos_max, pos_min = pos.max(0), pos.min(0)
pos_range = pos_max - pos_min
lu_thresh, fh_thresh = 0.3, 0.6
muscle_group = np.zeros((n_units,), dtype=int)
mask_upper = pos[:, 2] > (pos_min[2] + pos_range[2] * lu_thresh)
mask_fore = pos[:, 1] < (pos_min[1] + pos_range[1] * fh_thresh)
muscle_group[ mask_upper & mask_fore] = 0 # upper fore body
muscle_group[ mask_upper & ~mask_fore] = 1 # upper hind body
muscle_group[~mask_upper & mask_fore] = 2 # lower fore body
muscle_group[~mask_upper & ~mask_fore] = 3 # lower hind body
muscle_direction = np.array([[0, 1, 0]] * n_units, dtype=float)
robot.set_muscle(
muscle_group=muscle_group,
muscle_direction=muscle_direction,
)
set_muscle_by_pos(worm)
# 実行
scene.reset()
for i in range(1000):
actu = np.array([0, 0, 0, 1. * (0.5 + np.sin(0.005 * np.pi * i))])
worm.set_actuation(actu)
scene.step()
・マテリアル gs.materials.MPM.Muscle を指定するときに、追加の引数 n_groups = 4 を設定します。これは、このロボットには最大4つの異なる筋肉が存在する可能性があることを意味します。
・筋肉を設定するには、muscle_group と Muscle_direction を入力として受け取る robot.set_muscle を呼び出します。どちらも n_units と同じ長さです。MPM の n_units は粒子の数で、FEM の n_units は要素の数です。muscle_group は 0 から n_groups - 1 までの整数の配列で、ロボット本体のユニットがどの筋肉グループに属するかを示します。muscle_direction は、筋肉の方向のベクトルを指定する浮動小数点数の配列です。正規化は行わないため、入力の Muscle_direction がすでに正規化されていることを確認することをお勧めします。
・このワームの例の筋肉を設定する方法は、単に体を4つの部分に分割することです。上部前部、上部後部、下部前部、下部後部です。下部/上部間のしきい値設定には lu_thresh を使用し、前部/後部間のしきい値設定には fh_thresh を使用します。
・これで4つの筋肉グループが与えられ、set_actuation を介してコントロールを設定すると、アクチュエーション入力は形状 (4,) の配列になります。
2. ハイブリッドロボット
別タイプの「ソフトロボット」は、「リジッドボディ」の内部骨格を使用して「ソフトボディ」の外皮を作動させるもので、より正確に言えば「ハイブリッドロボット」です。「リジッドボディ」と「ソフトボディ」の両方のダイナミクスがすでに実装されているため、「Genesis」は「ハイブリッドロボット」もサポートしています。次の例は、2リンクの骨格をソフトスキンで包んでリジッドボールを押しているハイブリッドロボットです。
import numpy as np
import genesis as gs
# 初期化
gs.init(seed=0, precision='32', logging_level='debug')
# シーンの作成
dt = 3e-3
scene = gs.Scene(
sim_options=gs.options.SimOptions(
substeps=10,
),
viewer_options= gs.options.ViewerOptions(
camera_pos=(1.5, 1.3, 0.5),
camera_lookat=(0.0, 0.0, 0.0),
camera_fov=40,
),
rigid_options=gs.options.RigidOptions(
dt=dt,
gravity=(0, 0, -9.8),
enable_collision=True,
enable_self_collision=False,
),
mpm_options=gs.options.MPMOptions(
dt=dt,
lower_bound=( 0.0, 0.0, -0.2),
upper_bound=( 1.0, 1.0, 1.0),
gravity=(0, 0, 0), # mimic gravity compensation
enable_CPIC=True,
),
vis_options=gs.options.VisOptions(
show_world_frame=True,
visualize_mpm_boundary=False,
),
)
# シーンにエンティティを追加
scene.add_entity(morph=gs.morphs.Plane())
robot = scene.add_entity(
morph=gs.morphs.URDF(
file="urdf/simple/two_link_arm.urdf",
pos=(0.5, 0.5, 0.3),
euler=(0.0, 0.0, 0.0),
scale=0.2,
fixed=True,
),
material=gs.materials.Hybrid(
mat_rigid=gs.materials.Rigid(
gravity_compensation=1.,
),
mat_soft=gs.materials.MPM.Muscle( # to allow setting group
E=1e4,
nu=0.45,
rho=1000.,
model='neohooken',
),
thickness=0.05,
damping=1000.,
func_instantiate_rigid_from_soft=None,
func_instantiate_soft_from_rigid=None,
func_instantiate_rigid_soft_association=None,
),
)
ball = scene.add_entity(
morph=gs.morphs.Sphere(
pos=(0.8, 0.6, 0.1),
radius=0.1,
),
material=gs.materials.Rigid(rho=1000, friction=0.5),
)
# シーンのビルド
scene.build()
# 実行
scene.reset()
for i in range(1000):
dofs_ctrl = np.array([
1. * np.sin(2 * np.pi * i * 0.001),
] * robot.n_dofs)
robot.control_dofs_velocity(dofs_ctrl)
scene.step()
・ハイブリッドロボットは、gs.materials.Rigid と gs.materials.MPM.Muscle で構成されるマテリアル gs.materials.Hybrid で指定できます。ハイブリッドマテリアルは Muscle 用に実装された Muscle_group を内部的に再利用するため、ここでは MPM のみがサポートされ、Muscle クラスである必要があります。
・ロボットを制御する場合、アクチュエーションが内部のリジッドボディスケルトンから行われることを考えると、リジッドボディロボットと同様のインターフェイス (control_dofs_velocity、control_dofs_force、control_dofs_position など) があります。また、制御ディメンションは内部スケルトンの DoF と同じです (上記の例では 2)。
・スキンは内部スケルトンの形状によって決定され、厚みはスケルトンを包むときのスキンの厚さを決定します。
・デフォルトでは、スキンはスケルトンの形状に基づいて成長します。これは morph (この例では urdf/simple/two_link_arm.urdf) によって指定されます。 gs.materials.Hybrid の引数 func_instantiate_soft_from_rigid は、剛体モーフに基づいてスキンがどのように成長するかを具体的に定義します。genesis/engine/entities/hybrid_entity.py には、default_func_instantiate_soft_from_rigid というデフォルトの実装があります。独自の関数を実装することもできます。
・モーフが URDF ではなく Mesh の場合、メッシュは柔らかい外側のボディを指定し、内側のスケルトンはスキンの形状に基づいて成長します。これは func_instantiate_rigid_from_soft によって定義されます。また、default_func_instantiate_rigid_from_soft というデフォルトの実装もあり、これは基本的に 3D メッシュのスケルトン化を実装します。
・gs.materials.Hybrid の引数 func_instantiate_rigid_soft_association は、各スケルトンパーツがスキンとどのように関連付けられるかを決定します。デフォルトの実装では、硬い骨格部分に最も近い柔らかい皮膚の粒子を見つけます。
