diff --git a/genesis/engine/couplers/legacy_coupler.py b/genesis/engine/couplers/legacy_coupler.py index 499ff51da6..64e2090784 100644 --- a/genesis/engine/couplers/legacy_coupler.py +++ b/genesis/engine/couplers/legacy_coupler.py @@ -192,12 +192,14 @@ def _func_collide_with_rigid_geom_robust( pos_world, vel, mass, + pressure, normal_prev, geom_idx, batch_idx, geoms_state: array_class.GeomsState, geoms_info: array_class.GeomsInfo, links_state: array_class.LinksState, + links_info: array_class.LinksInfo, rigid_info: array_class.RigidInfo, sdf_info: array_class.SDFInfo, collider_static_config: qd.template(), @@ -221,6 +223,27 @@ def _func_collide_with_rigid_geom_robust( pos_world, vel, mass, normal_rigid, influence, geom_idx, batch_idx, geoms_info, links_state, rigid_info ) + # Static fluid pressure pushes on the geom even at rest, where the velocity-gated collision response above + # transfers nothing; this is what makes submerged geoms buoyant. Mirroring the particle pressure across the + # surface and integrating the symmetric pressure force over the truncated kernel support yields the factor + # 2 * sigma(signed_dist), with sigma the kernel plane integral (see cubic_kernel_plane_integral in + # sph_solver.py). Sigma integrates to 1/2 across the support band, so a covering particle layer transmits + # exactly p per unit area: Archimedes buoyancy with no tuning constant. Fixed links are exempt: they cannot + # respond to the force, and the reaction is a conservative stiff kick that keeps fluid resting on them + # ringing forever, pumped by the acoustic pressure fluctuations of the fluid. + link_idx = geoms_info.link_idx[geom_idx] + I_l = [link_idx, batch_idx] if qd.static(self.rigid_solver._options.batch_links_info) else link_idx + if signed_dist < self.sph_solver._support_radius and pressure > 0 and not links_info.is_fixed[I_l]: + pressure_force = ( + -2.0 + * pressure + * self.sph_solver._particle_volume + * self.sph_solver.cubic_kernel_plane_integral(signed_dist) + * normal_rigid + ) + self.rigid_solver._func_apply_coupling_force(link_idx, batch_idx, pos_world, pressure_force, links_state) + vel = vel - pressure_force * (rigid_info.substep_dt[None] / mass) + # attraction force # if 0.001 < signed_dist < 0.01: # vel = vel - normal_rigid * 0.1 * signed_dist @@ -646,6 +669,7 @@ def sph_rigid( geoms_state: array_class.GeomsState, geoms_info: array_class.GeomsInfo, links_state: array_class.LinksState, + links_info: array_class.LinksInfo, rigid_info: array_class.RigidInfo, sdf_info: array_class.SDFInfo, collider_static_config: qd.template(), @@ -661,12 +685,14 @@ def sph_rigid( self.sph_solver.particles_reordered[i_p, i_b].pos, self.sph_solver.particles_reordered[i_p, i_b].vel, self.sph_solver.particles_info_reordered[i_p, i_b].mass, + self.sph_solver.particles_reordered[i_p, i_b].p, self.sph_rigid_normal_reordered[i_p, i_g, i_b], i_g, i_b, geoms_state, geoms_info, links_state, + links_info, rigid_info, sdf_info, collider_static_config, @@ -878,6 +904,7 @@ def couple(self, f): self.rigid_solver.dyn_state.geoms, self.rigid_solver.dyn_info.geoms, self.rigid_solver.dyn_state.links, + self.rigid_solver.dyn_info.links, self.rigid_solver.rigid_info, self.rigid_solver.collider._sdf._sdf_info, self.rigid_solver.collider._collider_static_config, diff --git a/genesis/engine/solvers/rigid/abd/misc.py b/genesis/engine/solvers/rigid/abd/misc.py index a753383eed..dc2caf74ae 100644 --- a/genesis/engine/solvers/rigid/abd/misc.py +++ b/genesis/engine/solvers/rigid/abd/misc.py @@ -778,6 +778,39 @@ def func_apply_coupling_force(link_idx, env_idx, pos, force, links_state: array_ links_state.cfrc_coupling_vel[link_idx, env_idx] -= force +@qd.kernel +def kernel_wakeup_coupled_links( + dyn_state: array_class.DynState, + constraint_state: array_class.ConstraintState, + dyn_info: array_class.DynInfo, + rigid_info: array_class.RigidInfo, + rigid_config: qd.template(), +): + """ + Wake the hibernated islands of links holding a pending coupling force. + + Couplers accumulate forces into cfrc_coupling_* directly, bypassing the wake-aware public force API, while + forward dynamics only consumes the forces of awake entities before clearing the field. Waking the receiving + islands before the forces are consumed keeps hibernated links responsive to the other solvers and preserves + the momentum exchange whose opposite half has already been applied on the coupled side. + """ + qd.loop_config(serialize=rigid_config.para_level < gs.PARA_LEVEL.PARTIAL) + for i_l, i_b in qd.ndrange(dyn_state.links.is_hibernated.shape[0], dyn_state.links.is_hibernated.shape[1]): + if dyn_state.links.is_hibernated[i_l, i_b] and ( + dyn_state.links.cfrc_coupling_vel[i_l, i_b].norm_sqr() > 0 + or dyn_state.links.cfrc_coupling_ang[i_l, i_b].norm_sqr() > 0 + ): + func_wakeup_island( + constraint_state.island.links_island_idx[i_l, i_b], + i_b, + dyn_state, + constraint_state, + dyn_info, + rigid_info, + rigid_config, + ) + + @qd.func def func_apply_link_external_force( link_idx, env_idx, force, dyn_state: array_class.DynState, ref: qd.template(), local: qd.template() diff --git a/genesis/engine/solvers/rigid/rigid_solver.py b/genesis/engine/solvers/rigid/rigid_solver.py index 6ea5327508..b85a1dee2b 100644 --- a/genesis/engine/solvers/rigid/rigid_solver.py +++ b/genesis/engine/solvers/rigid/rigid_solver.py @@ -46,6 +46,7 @@ func_write_and_read_field_if, kernel_init_invweight, kernel_init_meaninertia, + kernel_wakeup_coupled_links, kernel_init_dof_fields, kernel_reset_hibernation, kernel_init_link_fields, @@ -1199,6 +1200,17 @@ def substep(self, f): if self._requires_grad and f == 0: kernel_save_adjoint_cache(f, self.dyn_state, self._rigid_adjoint_cache, self.rigid_info, self.rigid_config) + # Coupling forces from the previous coupling phase may target hibernated links (see + # kernel_wakeup_coupled_links in abd/misc.py). They can only exist when another solver is active. + if self._use_hibernation and len(self.sim.active_solvers) > 1: + kernel_wakeup_coupled_links( + self.dyn_state, + self.constraint_solver.constraint_state, + self.dyn_info, + self.rigid_info, + self.rigid_config, + ) + kernel_step_1( self.dyn_state, self.constraint_solver.constraint_state, diff --git a/genesis/engine/solvers/sph_solver.py b/genesis/engine/solvers/sph_solver.py index c2e6fe2a4f..6f16414703 100644 --- a/genesis/engine/solvers/sph_solver.py +++ b/genesis/engine/solvers/sph_solver.py @@ -421,6 +421,10 @@ def _kernel_compute_DFSPH_factor(self, f: qd.i32): factor = 0.0 self.particles_reordered[i_p, i_b].dfsph_factor = factor + # Reset the implicit pressure accumulated by the density solve (see + # _kernel_density_solve_iteration). + self.particles_reordered[i_p, i_b].p = 0.0 + @qd.func def _task_compute_density_time_derivative(self, i, j, ret: qd.template(), i_b): v_i = self.particles_reordered[i, i_b].vel @@ -599,6 +603,15 @@ def _kernel_density_solve_iteration(self): ) self.particles_reordered[i_p, i_b].vel = self.particles_reordered[i_p, i_b].vel + ret.dv + # The stiffness k_i is the pressure this Jacobi iteration applies to cancel the current density + # error, up to the -1/rho0 scaling carried by dfsph_factor, so summing -k_i * rho0 over the + # iterations of the density solve yields the implicit pressure field that enforces + # incompressibility over the substep. This exposes in particles.p the same physical quantity + # that the equation of state provides under WCSPH. The field is zeroed once per substep in + # _kernel_compute_DFSPH_factor. + rho0 = self.particles_info_reordered[i_p, i_b].rho + self.particles_reordered[i_p, i_b].p = self.particles_reordered[i_p, i_b].p - k_i * rho0 + def _density_solve_iteration(self): self._kernel_density_solve_iteration() self._kernel_compute_density_star() @@ -673,6 +686,28 @@ def cubic_kernel_derivative(self, r): res = -6.0 * k * (1.0 - q) ** 2 * grad_q return res + @qd.func + def cubic_kernel_plane_integral(self, dist): + """ + Integral of the cubic spline smoothing kernel over the plane at distance dist from its center. + + Closed form of 2 * pi * int_{|dist|}^{h} W(r) r dr. Its own integral along the plane normal equals + half the kernel normalization: int_{0}^{h} of this quantity over dist is exactly 1/2, which makes + forces weighted by it integrate to a per-unit-area magnitude over a covering layer of particles. + """ + res = gs.qd_float(0.0) + h = self._support_radius + q = qd.abs(dist) / h + if q <= 1.0: + q2 = q**2 + q3 = q2 * q + if q <= 0.5: + res = 0.0875 - (1.2 * q3 * q2 - 1.5 * q2**2 + 0.5 * q2) + else: + res = 0.1 - (q2 - 2.0 * q3 + 1.5 * q2**2 - 0.4 * q3 * q2) + res = res * 16.0 / h + return res + # ------------------------------------------------------------------------------------ # ------------------------------------ stepping -------------------------------------- # ------------------------------------------------------------------------------------ diff --git a/tests/coupling/test_sph_rigid.py b/tests/coupling/test_sph_rigid.py new file mode 100644 index 0000000000..8d4b5e1de9 --- /dev/null +++ b/tests/coupling/test_sph_rigid.py @@ -0,0 +1,147 @@ +import pytest + +import genesis as gs +from genesis.utils.misc import qd_to_numpy + + +@pytest.mark.required +@pytest.mark.parametrize("n_envs", [0, 2]) +@pytest.mark.parametrize("pressure_solver", ["WCSPH", "DFSPH"]) +def test_rigid_flotation_follows_density_ratio(n_envs, pressure_solver, show_viewer): + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=1e-2, + substeps=10, + gravity=(0.0, 0.0, -9.81), + ), + sph_options=gs.options.SPHOptions( + lower_bound=(-0.25, -0.25, 0.0), + upper_bound=(0.25, 0.25, 1.0), + particle_size=0.02, + pressure_solver=pressure_solver, + ), + viewer_options=gs.options.ViewerOptions( + camera_pos=(1.2, 0.0, 0.7), + camera_lookat=(0.0, 0.0, 0.3), + camera_fov=40, + ), + show_viewer=show_viewer, + ) + scene.add_entity( + morph=gs.morphs.Plane(), + ) + scene.add_entity( + morph=gs.morphs.Box( + pos=(0.0, 0.0, 0.25), + size=(0.5, 0.5, 0.5), + ), + material=gs.materials.SPH.Liquid( + sampler="regular", + ), + ) + light_ball = scene.add_entity( + morph=gs.morphs.Sphere( + pos=(-0.12, 0.0, 0.25), + radius=0.06, + ), + material=gs.materials.Rigid( + rho=200.0, + ), + ) + heavy_ball = scene.add_entity( + morph=gs.morphs.Sphere( + pos=(0.12, 0.0, 0.25), + radius=0.06, + ), + material=gs.materials.Rigid( + rho=1500.0, + ), + ) + scene.build(n_envs=n_envs) + + for _ in range(50): + scene.step() + + # The static fluid pressure must push the light ball (rho well below the fluid rest density) up toward the + # surface, while the heavy ball (rho well above) must keep sinking: flotation discriminates on the density + # ratio, which is Archimedes' principle. + light_z = light_ball.get_pos()[..., 2] + heavy_z = heavy_ball.get_pos()[..., 2] + assert (light_z > 0.28).all(), f"Light ball must rise under buoyancy, got z={light_z}" + assert (heavy_z < 0.22).all(), f"Heavy ball must sink, got z={heavy_z}" + + +@pytest.mark.required +@pytest.mark.parametrize("n_envs", [0, 2]) +def test_coupling_force_wakes_hibernated_link(n_envs, show_viewer): + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=1e-2, + substeps=10, + gravity=(0.0, 0.0, -9.81), + ), + rigid_options=gs.options.RigidOptions( + use_hibernation=True, + ), + sph_options=gs.options.SPHOptions( + lower_bound=(-0.55, -0.25, 0.0), + upper_bound=(0.55, 0.25, 1.0), + particle_size=0.02, + ), + viewer_options=gs.options.ViewerOptions( + camera_pos=(2.0, -1.5, 1.0), + camera_lookat=(0.0, 0.0, 0.2), + camera_fov=40, + ), + show_viewer=show_viewer, + ) + scene.add_entity( + morph=gs.morphs.Plane(), + ) + liquid = scene.add_entity( + morph=gs.morphs.Box( + pos=(-0.35, 0.0, 0.15), + size=(0.3, 0.4, 0.3), + ), + material=gs.materials.SPH.Liquid( + sampler="regular", + ), + ) + ball = scene.add_entity( + morph=gs.morphs.Sphere( + pos=(0.35, 0.0, 0.06), + radius=0.06, + ), + material=gs.materials.Rigid( + rho=200.0, + ), + ) + # The island partition, which hibernation builds on, only engages when the scene holds at least two free + # bodies; this bystander rests dry on the floor for the whole scenario. + scene.add_entity( + morph=gs.morphs.Sphere( + pos=(0.0, 0.0, 0.06), + radius=0.06, + ), + material=gs.materials.Rigid( + rho=2000.0, + ), + ) + scene.build(n_envs=n_envs) + + for _ in range(15): + scene.step() + + # The dry resting ball must have hibernated, otherwise the scenario would validate flotation instead of the + # wake-up on coupling forces. + is_ball_hibernated = qd_to_numpy(scene.rigid_solver.dyn_state.links.is_hibernated, transpose=True) + assert is_ball_hibernated[..., ball.links[0].idx].all() + + # Teleporting the fluid over the hibernated ball must wake it through the coupling forces alone: there is no + # awake body around to collide with, so with the ball left asleep the fluid pressure would be silently + # discarded and the ball would stay frozen on the floor instead of floating up. + liquid.set_position((0.35, 0.0, 0.15)) + for _ in range(40): + scene.step() + ball_z = ball.get_pos()[..., 2] + assert (ball_z > 0.15).all(), f"Submerged ball must wake up and float, got z={ball_z}" diff --git a/tests/particles/test_sph.py b/tests/particles/test_sph.py index 9f49ccaa83..f6c7b750a7 100644 --- a/tests/particles/test_sph.py +++ b/tests/particles/test_sph.py @@ -10,6 +10,7 @@ import numpy as np import genesis as gs +from genesis.utils.misc import qd_to_numpy def create_sph_scene(show_viewer, particle_size=0.01, pressure_solver="WCSPH"): @@ -273,3 +274,63 @@ def test_dfsph_simulation_builds_and_runs(show_viewer): pos = liquid.get_particles_pos() assert pos.shape[0] == scene.sph_solver.n_particles + + +@pytest.mark.required +@pytest.mark.parametrize("pressure_solver", ["WCSPH", "DFSPH"]) +def test_pressure_carries_column_weight(show_viewer, pressure_solver): + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=1e-2, + substeps=10, + gravity=(0.0, 0.0, -9.81), + ), + sph_options=gs.options.SPHOptions( + lower_bound=(-0.15, -0.15, 0.0), + upper_bound=(0.15, 0.15, 1.0), + particle_size=0.02, + pressure_solver=pressure_solver, + ), + viewer_options=gs.options.ViewerOptions( + camera_pos=(0.9, 0.0, 0.5), + camera_lookat=(0.0, 0.0, 0.25), + camera_fov=40, + ), + show_viewer=show_viewer, + ) + scene.add_entity( + morph=gs.morphs.Plane(), + ) + scene.add_entity( + morph=gs.morphs.Box( + pos=(0.0, 0.0, 0.25), + size=(0.3, 0.3, 0.5), + ), + material=gs.materials.SPH.Liquid( + sampler="regular", + ), + ) + scene.build() + + for _ in range(100): + scene.step() + + sph_solver = scene.sph_solver + pressures = qd_to_numpy(sph_solver.particles_reordered.p, transpose=True)[0] + heights = qd_to_numpy(sph_solver.particles_reordered.pos, transpose=True)[0, :, 2] + + # The pressure at the bottom of a settled column carries the weight of the whole column, which is exactly + # the particle count times the particle weight spread over the column footprint. The tolerance absorbs the + # residual sloshing of the column and two boundary effects that pull in opposite directions: the wall + # pressure force from the floor relieves part of the compression that the WCSPH equation of state reads, + # while the positive-part clamp of the DFSPH density solve rectifies velocity noise into extra pressure. + weight_per_area = sph_solver.n_particles * sph_solver.particles_info[0].mass * 9.81 / (0.3 * 0.3) + p_bottom = pressures[heights < 0.05].mean() + assert 0.4 * weight_per_area < p_bottom < 1.5 * weight_per_area + + # The pressure must decrease with height, following the hydrostatic gradient. The factors are lower bounds + # on the ratio of the hydrostatic depths at the band centers, with slack for pressure fluctuations. + p_middle = pressures[(heights >= 0.2) & (heights < 0.25)].mean() + p_top = pressures[heights >= 0.3].mean() + assert p_bottom > 1.5 * p_middle + assert p_middle > 2.0 * p_top