How to solve the collision?

[kataoka-d]

Hello everyone,

I am currently conducting research on a small humanoid robot using Genesis-world. I have a question regarding a collision issue I've encountered.

As shown in the attached photo, my robot's feet are sinking into the heightmap terrain. I have searched through the documentation, but I haven't been able to find a clear solution to this problem.

Could you please advise me on which parameters I should adjust to resolve this sinking/penetration issue?

Specifically, I would like to know if I should focus on:

Surface properties (stiffness/damping)

Collision shape definitions (URDF settings)

Solver options (GJK vs MPR)

Terrain scaling parameters

Any help or guidance would be greatly appreciated. Thank you in advance!

[duburcqa]

try enabling visualization of the collision geometries. It would help to pinpoint the root cause. it may be the physics, or just the geometries that are not what you would expect.

[kataoka-d]

I visualized the collision between the terrain and the robot using rigid_options. Is this set up as you advised?

After further investigation, I’ve found that the feet don't sink across the entire terrain, but only in specific areas. Could there be an issue with the data structure or the shape of the terrain I'm generating?

My apologies if I'm way off base here.

[duburcqa]

How the terrain was generated? Could you share a reproduction script? There must be a bug somewhere, uneven ground is not thoroughly tested.

[kataoka-d]

custom_terrain = True

    if custom_terrain:
        terrain_type = 'slope'
        match terrain_type:
            case 'plane':
                hf = np.zeros((9,9), dtype=np.int16)
                horizontal_scale = 1.0
                vertical_scale = 0.1
                terrain_pos = (-4.5, -4.5, 0)

            

            case 'slope':
                hf= np.array([[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
                            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,2,2,1,0,0,0,0,1,2,2,1,0,0],
                            [0,0,1,1,1,1,0,0,0,0,1,1,1,1,0,0],
                            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
                            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]],dtype=np.int16)
                horizontal_scale = 1.0
                vertical_scale =  0.069812
                terrain_pos = (-3.5, -3.5, 0)
        

    
        self.ground = self.scene.add_entity(
            morph=gs.morphs.Terrain(
                pos=terrain_pos,
                height_field=hf,
                horizontal_scale=horizontal_scale,
                vertical_scale=vertical_scale,
            ),
            surface=gs.surfaces.Collision(
                color=(0.2, 0.8, 0.2), 
                vis_mode='collision',
            ),
        )
    else:
        self.ground = self.scene.add_entity(
            morph = gs.morphs.URDF(
                file="urdf/plane/plane.urdf",
                fixed=True,
            ),
            surface = gs.options.surfaces.Collision(
                vis_mode = 'visual',
            ),
        )

    # add robot
    self.robot = self.scene.add_entity(
        gs.morphs.URDF(
            file="urdf/khr3hv/urdf/khr_12dof.urdf",
            pos=self.env_cfg["base_init_pos"],
            quat=self.env_cfg["base_init_quat"],
        ),
        surface=gs.surfaces.Collision(
            color=(1.0, 0.2, 0.2), 
            vis_mode='collision',
        ),
    )

I generated height field like this.
In our experiments, slope terrain was used, and foot penetration was observed only at specific locations on the slope.

[duburcqa]

Ok I wrote this repro. This looks garbage indeed.

"""Parallel-env repro: hundreds of feet, one per env, placed on a grid across the user's heightfield.

All envs render into a single frame, so we see every foot's final resting position simultaneously.
"""

import numpy as np
import torch

import genesis as gs


def main():
    gs.init(backend=gs.cpu, logging_level="warning")

    hf = np.array(
        [
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 2, 2, 1, 0, 0, 0, 0, 1, 2, 2, 1, 0, 0],
            [0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        ],
        dtype=np.int16,
    )
    hsc = 1.0
    vsc = 0.069812
    terrain_pos = (-3.5, -3.5, 0.0)

    # Focus the grid on pyramid 0: rx in [1, 6], cy in [1, 6]. 20 x 20 = 400 feet.
    grid_n = 20
    rxs = np.linspace(1.0, 6.0, grid_n)
    cys = np.linspace(1.0, 6.0, grid_n)
    rr, cc = np.meshgrid(rxs, cys, indexing="ij")
    pts_local = np.stack([rr.ravel(), cc.ravel()], axis=-1)
    n_envs = pts_local.shape[0]

    foot_size = (0.12, 0.06, 0.025)

    scene = gs.Scene(
        sim_options=gs.options.SimOptions(dt=0.005),
        rigid_options=gs.options.RigidOptions(box_box_detection=False),
        show_viewer=False,
        renderer=gs.renderers.Rasterizer(),
    )

    scene.add_entity(
        morph=gs.morphs.Terrain(
            pos=terrain_pos,
            height_field=hf,
            horizontal_scale=hsc,
            vertical_scale=vsc,
        ),
        surface=gs.surfaces.Default(color=(0.55, 0.78, 0.55)),
    )

    foot = scene.add_entity(
        morph=gs.morphs.Box(size=foot_size, pos=(0.0, 0.0, 0.5)),
        surface=gs.surfaces.Default(color=(0.95, 0.25, 0.25)),
    )

    cam = scene.add_camera(
        res=(1280, 720),
        pos=(-2.4, -2.4, 0.22),
        lookat=(0.5, 0.5, 0.10),
        fov=26,
    )

    scene.build(n_envs=n_envs)

    # Place each env's foot at its grid point, dropped just above the local surface.
    xs_world = pts_local[:, 0] * hsc + terrain_pos[0]
    ys_world = pts_local[:, 1] * hsc + terrain_pos[1]
    # Drop height: a bit above the plateau top (0.14) to avoid initial overlap.
    zs_world = np.full(n_envs, 0.4, dtype=np.float32)

    init_pos = torch.from_numpy(np.stack([xs_world, ys_world, zs_world], axis=-1).astype(np.float32))
    foot.set_pos(init_pos)

    cam.start_recording()
    n_steps = int(2.5 / 0.005)
    for k in range(n_steps):
        scene.step()
        if k % 4 == 0:
            cam.render()
    out_path = "/Users/alexis.duburcq/workspace/src/genesis/_repro_terrain_2781_parallel.mp4"
    cam.stop_recording(save_to_filename=out_path, fps=50)

    # Print penetration heatmap-style: for each grid cell, surf_z vs bottom_z.
    final_pos = foot.get_pos().cpu().numpy()
    print(f"\nGrid {grid_n}x{grid_n}, foot size {foot_size}")
    print(f"{'rx':>5} {'cy':>5} {'surf_z':>8} {'bot_z':>8} {'pen':>8}")
    worst = []
    for k in range(n_envs):
        rx, cy = pts_local[k]
        z = float(final_pos[k, 2])
        bot = z - foot_size[2] / 2
        r0, r1 = int(np.floor(rx)), min(int(np.ceil(rx)), hf.shape[0] - 1)
        c0, c1 = int(np.floor(cy)), min(int(np.ceil(cy)), hf.shape[1] - 1)
        surf = max(hf[r0, c0], hf[r1, c0], hf[r0, c1], hf[r1, c1]) * vsc
        pen = surf - bot
        worst.append((pen, rx, cy, surf, bot))
    worst.sort(reverse=True)
    print("Top 10 most-penetrated positions:")
    for pen, rx, cy, surf, bot in worst[:10]:
        print(f"{rx:5.2f} {cy:5.2f} {surf:8.4f} {bot:+8.4f} {pen:+8.4f}")
    print(f"\nVideo: {out_path}")

if __name__ == "__main__":
    main()

_repro_terrain_2781_parallel.mp4

[duburcqa]

Fixed!

[kataoka-d]

Thank you!
Do I need to reinstall the latest source to get the fix?

[duburcqa]

yes!

[duburcqa]

Let me know if it does not work for some reason and I will re-open this discussion.

[kataoka-d]

I have just checked height field now.
The terrain bug fixed!
Thank you.