[BUG FIX] Fix missing buoyancy on rigid bodies submerged in SPH fluid.

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,

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()

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,

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 --------------------------------------
     # ------------------------------------------------------------------------------------

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}"