[MISC] Add get_particles_pos method to sph (#1871)

Author: YilingQiao

genesis/engine/entities/sph_entity.py

@@ -183,3 +183,11 @@ def get_particles_active(self, envs_idx=None, *, unsafe=False):
         if self._scene.n_envs == 0:
             actives = actives.squeeze(0)
         return actives
+
+    def get_particles_pos(self, envs_idx=None, *, unsafe=False):
+        envs_idx = self._scene._sanitize_envs_idx(envs_idx, unsafe=unsafe)
+        poss = torch.empty((len(envs_idx), self.n_particles, 3), dtype=gs.tc_float, device=gs.device)
+        self.solver._kernel_get_particles_pos(self._particle_start, self.n_particles, envs_idx, poss)
+        if self._scene.n_envs == 0:
+            poss = poss.squeeze(0)
+        return poss

tests/test_deformable_physics.py

@@ -186,10 +186,28 @@ def test_deformable_parallel(show_viewer):
     )
     scene.build(n_envs=2)
 
+    init_mpm_cube_pos = mpm_cube.get_particles_pos()
+    init_cloth_pos = cloth.get_particles_pos()
+    init_water_pos = water.get_particles_pos()
+
     scene.get_state()
     for i in range(1500):
         scene.step()
 
+    final_mpm_cube_pos = mpm_cube.get_particles_pos()
+    final_cloth_pos = cloth.get_particles_pos()
+    final_water_pos = water.get_particles_pos()
+
+    # check if the positions are changed
+    assert (init_mpm_cube_pos - final_mpm_cube_pos).abs().sum() > 0.1
+    assert (init_cloth_pos - final_cloth_pos).abs().sum() > 0.1
+    assert (init_water_pos - final_water_pos).abs().sum() > 0.1
+
+    # check if the particles are above the ground
+    assert final_mpm_cube_pos[..., 2].min() > -1e-5
+    assert final_cloth_pos[..., 2].min() > -1e-5
+    assert final_water_pos[..., 2].min() > -1e-5
+
     assert_allclose(cloth.get_particles_vel(), 0.0, atol=1e-5)
     assert_allclose(mpm_cube.get_particles_vel(), 0.0, atol=1e-4)
     assert_allclose(entity_fem._solver.get_state(0).vel, 0, atol=1e-3)