Genesis Introduction (10) - Soft Robots
I have summarized the "Soft Robots" in "Genesis".
Previous
1. Soft Robots
"Genesis" supports "Volumetric Muscle simulation" using "MPM" and "FEM" for "Soft Robots". The following example shows a very simple "Soft Robot" with a spherical body actuated by a sine wave control signal.
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()
Most of the code is quite standard compared to instantiating a normal deformable Entity. The only useful differences are the following two.
・When instantiating the soft robot robot_mpm and robot_fem, use the materials gs.materials.MPM.Muscle and gs.materials.FEM.Muscle, respectively.
・When stepping through the simulation, use robot_mpm.set_actuation or robot_fem.set_actuation to set the muscle actuation.
By default, there is only one muscle that spans the entire body of the robot, and the muscle direction is perpendicular to the ground [0, 0, 1].
The following example shows how to simulate a worm crawling forward by setting muscle groups and directions as shown below. (The full script can be found in 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()
・When specifying the material gs.materials.MPM.Muscle, set the additional argument n_groups = 4. This means that there can be up to 4 different muscles in this robot.
・To set up the muscles, call robot.set_muscle, which takes muscle_group and muscle_direction as input. Both are the same length as n_units. n_units for MPM is the number of particles, and n_units for FEM is the number of elements. muscle_group is an array of integers from 0 to n_groups - 1, indicating which muscle group the units of the robot body belong to. muscle_direction is an array of floating-point numbers specifying the vector of the muscle direction. Since normalization is not performed, it is recommended to ensure that the input muscle_direction is already normalized.
・The way to set the muscles for this worm example is to simply divide the body into four parts: upper front, upper back, lower front, and lower back. Use lu_thresh for threshold setting between lower/upper, and fh_thresh for threshold setting between front/back.
・Now that you have 4 muscle groups, when you set the control via set_actuation, the actuation input will be an array of shape (4,).
2. Hybrid Robots
Another type of "Soft Robot" uses an internal "Rigid Body" skeleton to actuate a "Soft Body" skin, or more accurately, a "Hybrid Robot". Since the dynamics of both "Rigid Body" and "Soft Body" are already implemented, "Genesis" also supports "Hybrid Robots". The following example is a hybrid robot with a 2-link skeleton wrapped in a soft skin pushing a rigid ball.
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()
・Hybrid robots can be specified with the material gs.materials.Hybrid, which consists of gs.materials.Rigid and gs.materials.MPM.Muscle. Since the hybrid material internally reuses the muscle_group implemented for Muscle, only MPM is supported here and it must be a Muscle class.
・When controlling the robot, given that actuation is performed from the internal rigid body skeleton, there is an interface similar to a rigid body robot (control_dofs_velocity, control_dofs_force, control_dofs_position, etc.). Also, the control dimension is the same as the DoF of the internal skeleton (2 in the example above).
・The skin is determined by the shape of the internal skeleton, and the thickness determines the thickness of the skin when wrapping the skeleton.
・By default, the skin grows based on the shape of the skeleton. This is specified by the morph (urdf/simple/two_link_arm.urdf in this example). The argument func_instantiate_soft_from_rigid of gs.materials.Hybrid specifically defines how the skin grows based on the rigid body morph. genesis/engine/entities/hybrid_entity.py has a default implementation called default_func_instantiate_soft_from_rigid. You can also implement your own function.
・If the morph is a Mesh instead of a URDF, the mesh specifies the soft outer body, and the inner skeleton grows based on the shape of the skin. This is defined by func_instantiate_rigid_from_soft. There is also a default implementation called default_func_instantiate_rigid_from_soft, which basically implements skeletonization of a 3D mesh.
・The argument func_instantiate_rigid_soft_association of gs.materials.Hybrid determines how each skeleton part is associated with the skin. The default implementation finds the soft skin particles closest to the rigid skeleton parts.