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

Code before:
    scene = gs.Scene(
        show_viewer=show_viewer,
    )
    hand = scene.add_entity(
        gs.morphs.URDF(
            file="urdf/panda_bullet/hand.urdf",
            fixed=True,
        ),
    )
    scene.build()
    assert scene.rigid_solver.n_equalities == 1

    qvel = scene.rigid_solver.dofs_state.vel.to_numpy()
    qvel[-1] = 1
    scene.rigid_solver.dofs_state.vel.from_numpy(qvel)
    for i in range(200):
        scene.step()

    gs_qpos = scene.rigid_solver.qpos.to_numpy()[:, 0]
    assert_allclose(gs_qpos[-1], gs_qpos[-2], tol=tol)


@pytest.mark.required
@pytest.mark.parametrize("backend", [gs.cpu])
def test_jacobian_arbitrary_point(tmp_path, show_viewer, tol):
    urdf_root = _build_multi_pendulum(n=1)
    urdf_file = tmp_path / "pendulum.urdf"
    ET.ElementTree(urdf_root).write(urdf_file)

    scene = gs.Scene(show_viewer=show_viewer, show_FPS=False)
    ent = scene.add_entity(gs.morphs.URDF(file=str(urdf_file), fixed=True))
    scene.build()

    angle = 0.7
    ent.set_qpos(np.array([angle], dtype=np.float32))
    scene.step()

    link = ent.get_link("PendulumArm_0")

    p_local = np.array([0.05, -0.02, 0.12], dtype=np.float32)
    J_o = ent.get_jacobian(link).cpu().numpy()
    J_p = ent.get_jacobian(link, p_local).cpu().numpy()

    c, s = np.cos(angle), np.sin(angle)
    Rx = np.array([[1, 0, 0], [0, c, -s], [0, s, c]], dtype=np.float32)
    r_world = Rx @ p_local
    r_cross = np.array(
        [[0, -r_world[2], r_world[1]], [r_world[2], 0, -r_world[0]], [-r_world[1], r_world[0], 0]], dtype=np.float32
    )

    lin_o, ang_o = J_o[:3, 0], J_o[3:, 0]

Review comment (suggestion):
You should rather do this:

```python
@pytest.mark.required
@pytest.mark.merge_fixed_links(False)
@pytest.mark.parametrize("model_name", ["pendulum"])
def test_pendulum_links_acc(gs_sim, tol):
     [...]
```

`gs_sim` is the same as `scene.sim`. You can access entities by doing `gs_sim.entities`.

Code after:
    for i, key in enumerate(((False, False), (False, True))):
        qpos = np.concatenate(
            ((0.0, (i - 1.5) * POS_OFFSET, 0.0), tuple(WOLRD_QUAT / np.linalg.norm(WOLRD_QUAT)), door_angle)
        )
        entities[key].set_qpos(qpos)
    for i, key in enumerate(((True, False), (True, True))):
        entities[key].set_pos(np.array([0.0, 0.0, 0.0]), relative=True)
        entities[key].set_quat(np.array([1.0, 0.0, 0.0, 0.0]), relative=True)
        entities[key].set_qpos(door_angle)
    if show_viewer:
        scene.visualizer.update()
    _check_entity_positions(relative=True, tol=gs.EPS)

    # Add dof damping to stabilitze the physics
    for key in ((False, False), (False, True), (True, False), (True, True)):
        entities[key].set_dofs_damping(entities[key].get_dofs_damping() + DOOR_JOINT_DAMPING)

    # Make sure that the dynamics of the door is the same in all cases
    door_vel = np.array([-0.2])
    entities[(False, False)].set_dofs_velocity(door_vel, 6)
    entities[(False, True)].set_dofs_velocity(door_vel, 6)
    entities[(True, False)].set_dofs_velocity(door_vel)
    entities[(True, True)].set_dofs_velocity(door_vel)
    link_1 = np.array([entities[(True, True)].link_start], dtype=gs.np_int)
    for key in ((False, False), (False, True)):
        link_2 = entities[key].link_start
        scene.rigid_solver.add_weld_constraint(link_1, link_2)

    for i in range(2000):
        scene.step()
        door_pos_all = (
            entities[(False, False)].get_dofs_position(6),
            entities[(False, True)].get_dofs_position(6),
            entities[(True, False)].get_dofs_position(0),
            entities[(True, True)].get_dofs_position(0),
        )
        door_pos_diff = np.diff(torch.concatenate(door_pos_all))
        assert_allclose(door_pos_diff, 0, tol=5e-3)
    assert_allclose(scene.rigid_solver.dofs_state.vel.to_numpy(), 0.0, tol=1e-3)
    _check_entity_positions(relative=True, tol=2e-3)


@pytest.mark.required
@pytest.mark.parametrize("backend", [gs.cpu])
def test_urdf_mimic(show_viewer, tol):
    # create and build the scene
    scene = gs.Scene(
        show_viewer=show_viewer,
    )
    hand = scene.add_entity(
        gs.morphs.URDF(