Repository: Genesis-Embodied-AI/Genesis
File: tests/test_rigid_physics.py (Python)

Code before:
        ),
        sim_options=gs.options.SimOptions(
            dt=0.01,
        ),
        profiling_options=gs.options.ProfilingOptions(show_FPS=False),
        rigid_options=gs.options.RigidOptions(
            noslip_iterations=5,
        ),
        show_viewer=show_viewer,
    )

    boxes = []
    for i in range(3):
        boxes.append(
            scene.add_entity(
                gs.morphs.Box(
                    size=(1 * scale, 1 * scale, 1 * scale),
                    pos=(i * scale, 0, 0),
                    fixed=i == 0,
                ),
            )
        )
    scene.build()
    # simulate for 20 seconds
    for i in range(2000):
        boxes[2].control_dofs_force(np.array([-20000.0 * scale**3]), np.array([0]))
        # push to -x direction
        scene.step()

    box_1_z = boxes[1].get_qpos().cpu().numpy()[2]
    # allow some small sliding due to first few frames
    # scale = 0.1 is less stable than bigger scale
    assert_allclose(box_1_z, 0.0, atol=4e-2 * scale)


@pytest.mark.required
def test_batched_aabb(tol):
    scene = gs.Scene()
    plane = scene.add_entity(
        gs.morphs.Plane(
            normal=(0, 0, 1),
            pos=(0, 0, 0),
        ),
    )
    box = scene.add_entity(
        gs.morphs.Box(
            size=(0.1, 0.1, 0.1),
            pos=(0.5, 0, 0.05),
        ),
    )
    sphere = scene.add_entity(

Review comment (performance):
Ideally you should set the friction coefficient yourself, and set the normal force so that to box does not slip (with some safety factor, i.e. `F_n = 2.0 * box_mass * g / friction_coef`, make sure that the box does not move, then decrease the normal force using `F_n = 0.5 *  box_mass * g / friction_coef` and check that it starts slipping.

Code after:
        vis_mode="collision",
    )
    cube = scene.add_entity(
        gs.morphs.Box(
            size=(0.5, 0.5, 0.5),
            pos=(1.0, 1.0, 1.0),
        ),
        vis_mode="collision",
    )
    ############# build ##########################
    scene.build()
    rigid = scene.sim.rigid_solver
    for i in range(500):
        scene.step()

    link_COM = rigid.get_links_pos(ref="link_com")
    root_COM = rigid.get_links_pos(ref="root_com")
    bunny_z = link_COM[1, 2]
    cube_z = link_COM[2, 2]
    root_bunny_z = root_COM[1, 2]
    root_cube_z = root_COM[2, 2]

    assert_allclose(bunny_z, bunny.get_links_pos(links_idx_local=[0], ref="link_com")[0, 2], atol=gs.EPS)
    assert_allclose(cube_z, cube.get_links_pos(links_idx_local=[0], ref="link_com")[0, 2], atol=gs.EPS)
    assert_allclose(root_bunny_z, bunny.get_links_pos(links_idx_local=[0], ref="root_com")[0, 2], atol=gs.EPS)
    assert_allclose(root_cube_z, cube.get_links_pos(links_idx_local=[0], ref="root_com")[0, 2], atol=gs.EPS)

    # in the old (wrong) code, their initial COM are (0,0,0)
    # but now their z are above the plane
    assert_allclose(bunny_z, 0.3424, atol=1e-3)
    assert_allclose(cube_z, 0.2499, atol=1e-3)

    # root and link COM should be the same for single link
    assert_allclose(root_bunny_z, bunny_z, atol=tol)
    assert_allclose(root_cube_z, cube_z, atol=tol)


@pytest.mark.required
@pytest.mark.parametrize("scale", [0.1, 10.0])
@pytest.mark.parametrize("backend", [gs.cpu, gs.gpu])
def test_noslip_iterations(scale, show_viewer, tol):
    scene = gs.Scene(
        sim_options=gs.options.SimOptions(
            dt=0.01,
        ),
        rigid_options=gs.options.RigidOptions(
            noslip_iterations=5,
        ),
        profiling_options=gs.options.ProfilingOptions(show_FPS=False),
        show_viewer=show_viewer,
    )