๐Ÿ› Soft Robots#

Volumetric muscle simulation#

Genesis supports volumetric muscle simulation using MPM and FEM for soft robots. In the following example, we demonstrate an extremely simple soft robot with a sphere body, actuated by a sine-wave control signal.

import numpy as np
import genesis as gs


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

########################## create a scene ##########################
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,
    ),
)

########################## entities ##########################
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',
    ),
)

########################## build ##########################
scene.build()

########################## run ##########################
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()

This is what you will see:

Most of the code is pretty standard compared to instantiating regular deformable entities. There are only two small differences that do the trick:

  • When instantiating soft robots robot_mpm and robot_fem, we use materials gs.materials.MPM.Muscle and gs.materials.FEM.Muscle respectively.

  • When stepping the simulation, we use robot_mpm.set_actuation or robot_fem.set_actuation to set the actuation of the muscle.

By default, there is only one muscle that spans the entire robot body with the muscle direction perpendicular to the ground [0, 0, 1].

In the next example, we show how to simulate a worm crawling forward by setting muscle groups and directions, as shown in the following. (The full script can be found in tutorials/advanced_worm.py.)

########################## entities ##########################
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,
    ),
)

########################## set muscle ##########################
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)

########################## run ##########################
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()

This is what you will see:

Several things that worth noticing in this code snippet:

  • When specifying the material gs.materials.MPM.Muscle, we set an additional argument n_groups = 4, which means there can be at most 4 different muscles in this robot.

  • We can set the muscle by calling robot.set_muscle, which takes muscle_group and muscle_direction as inputs. Both have the same length as n_units, where in MPM n_units is the number of particles while in FEM n_units is the number of elements. muscle_group is an array of integer ranging from 0 to n_groups - 1, indicating which muscle group an unit of the robot body belongs to. muscle_direction is an array of floating-point numbers that specify vectors for muscle direction. Note that we donโ€™t do normalization and thus you may want to make sure the input muscle_direction is already normalized.

  • How we set the muscle of this worm example is simply breaking the body into four parts: upper fore, upper hind, lower fore, and lower hind body, using lu_thresh for thresholding between lower/upper and fh_thresh for thresholding between fore/hind.

  • Now given four muscle groups, when setting the control via set_actuation, the actuation input is thus an array of shape (4,).

Hybrid (rigid-and-soft) robot#

Another type of soft robot is using rigid-bodied inner skeleton to actuatuate soft-bodied outer skin, or more precisely speaking, hybrid robot. With both rigid-bodied and soft-bodied dynamics implemented already, Genesis also supports hybrid robot. The following example is a hybrid robot with a two-link skeleton wrapped by soft skin pushing a rigid ball.

import numpy as np
import genesis as gs


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

######################## create a scene ##########################
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,
    ),
)

########################## entities ##########################
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),
)

########################## build ##########################
scene.build()

########################## run ##########################
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()

This is what you will see:

  • You can specify hybrid robot with the material gs.materials.Hybrid, which consists of gs.materials.Rigid and gs.materials.MPM.Muscle. Note that only MPM is supported here and it must be the Muscle class since the hybrid material reuses internally the muscle_group implemented for Muscle.

  • When controlling the robot, given the actuation being from the inner rigid-bodied skeleton, there is a similar interface to rigid-bodied robot, e.g., control_dofs_velocity, control_dofs_force, control_dofs_position. Also, the control dimension is the same as the DoFs of the inner skeleton (in the above example, 2).

  • The skin is determined by the shape of the inner skeleton, where thickness determines the skin thickness when wrapping the skeleton.

  • By default, we grow skin based on the shape of the skeleton, which is specified by morph (in this example, the urdf/simple/two_link_arm.urdf). The argument func_instantiate_soft_from_rigid of gs.materials.Hybrid defines concretely how skin should grow based on the rigid-bodied morph. There is a default implementation default_func_instantiate_soft_from_rigid in genesis/engine/entities/hybrid_entity.py. You can also implement your own function.

  • When morph is Mesh instead of URDF, the mesh specifies the soft outer body and the inner skeleton is grown based on the skin shape. This is defined by func_instantiate_rigid_from_soft. There is also a default implementation default_func_instantiate_rigid_from_soft, which basically implements skeletonization of 3D meshes.

  • The argument func_instantiate_rigid_soft_association of gs.materials.Hybrid determines how each skeletal part is associated with skin. The default implementation is to find the closest particles of the soft skin to the rigid skeletal parts.