Skip to content

Commit aaebb7f

Browse files
Add regression test for multi-body sensor flat-buffer ordering
Existing tests are all single-body per ContactSensor; pattern-major vs. env-major flat-buffer indexing collapses to the same address when there is only one body, so they couldn't catch the kernel formula bug fixed in the previous commit. test_multi_body_per_sensor_indexing exercises a single ContactSensor with prim_path='{ENV_REGEX_NS}/Cube_.*' that resolves to two cubes per env (one on the ground, one floating). After the scene settles, the on-ground cube reports a non-zero net force and the floating cube reports zero. If the kernel indexing is wrong, the floating cube picks up a phantom contact force from another env's grounded cube and the assertion fires with a diagnostic message pointing at the pattern-major / env-major mismatch. Verified to fail (sum-abs ~3.0) on the pre-fix kernel and pass after re-applying sensor*num_envs+env.
1 parent 6234c25 commit aaebb7f

1 file changed

Lines changed: 64 additions & 0 deletions

File tree

source/isaaclab_ovphysx/test/sensors/test_contact_sensor.py

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -459,6 +459,70 @@ def test_no_contact_reporting():
459459
assert fm2.torch.sum().item() == 0.0
460460

461461

462+
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
463+
@pytest.mark.parametrize("num_envs", [1, 3])
464+
@pytest.mark.isaacsim_ci
465+
def test_multi_body_per_sensor_indexing(device, num_envs):
466+
"""Ground-truth body-index check for a single sensor that resolves to two bodies.
467+
468+
OVPhysX :class:`ContactBinding` returns sensors in **pattern-major** order
469+
(``[env_0/body_0, env_1/body_0, …, env_0/body_1, env_1/body_1, …]``),
470+
whereas the inherited PhysX kernel formula assumes env-major
471+
(``[env_0/body_0, env_0/body_1, …, env_1/body_0, …]``). Single-body
472+
sensors don't disambiguate the two layouts, so this test exercises the
473+
multi-body discovery path with one cube on the ground and one floating
474+
above it. After the scene settles, only the bottom cube should report a
475+
non-zero net force. An env-major bug would attribute that force to the
476+
wrong (env, body) slot — caught here.
477+
"""
478+
with _ovphysx_sim_context(device=device, dt=_SIM_DT, add_lighting=True) as sim:
479+
scene_cfg = ContactSensorSceneCfg(num_envs=num_envs, env_spacing=2.0, lazy_sensor_update=False)
480+
scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground")
481+
# -- Cube_low: on the ground, will report contact forces
482+
scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_low")
483+
scene_cfg.shape.init_state.pos = (0.0, 0.0, 0.25)
484+
# -- Cube_high: floating well above the ground, should remain in air
485+
scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_high")
486+
scene_cfg.shape_2.init_state.pos = (0.0, 1.5, 3.0)
487+
# Single ContactSensor that matches BOTH cubes via a regex glob.
488+
scene_cfg.contact_sensor = ContactSensorCfg(
489+
prim_path="{ENV_REGEX_NS}/Cube_.*",
490+
track_pose=False,
491+
debug_vis=False,
492+
update_period=0.0,
493+
filter_prim_paths_expr=[],
494+
)
495+
scene = InteractiveScene(scene_cfg)
496+
sim.reset()
497+
contact_sensor: ContactSensor = scene["contact_sensor"]
498+
499+
# Sanity: the sensor discovered exactly two bodies, one per cube.
500+
assert contact_sensor.body_names is not None
501+
assert sorted(contact_sensor.body_names) == ["Cube_high", "Cube_low"]
502+
low_idx = contact_sensor.body_names.index("Cube_low")
503+
high_idx = contact_sensor.body_names.index("Cube_high")
504+
505+
# Let physics settle and accumulate stable contacts on Cube_low.
506+
scene.reset()
507+
for _ in range(200):
508+
_perform_sim_step(sim, scene, _SIM_DT)
509+
510+
# Net force readout: shape (num_envs, num_sensors=2, 3) after .torch.
511+
net_forces = contact_sensor.data.net_forces_w.torch
512+
assert net_forces.shape == (num_envs, 2, 3)
513+
low_force_mag = net_forces[:, low_idx, :].abs().sum().item()
514+
high_force_mag = net_forces[:, high_idx, :].abs().sum().item()
515+
# Cube_low rests on the ground: non-zero contact force per env.
516+
assert low_force_mag > 0.0, "Cube_low (on ground) should report contact force"
517+
# Cube_high floats: net force is zero (no contact).
518+
assert high_force_mag == 0.0, (
519+
f"Cube_high (in air) should report zero contact force, got sum-abs={high_force_mag:.6f}."
520+
" A non-zero value here usually means body indices are scrambled —"
521+
" e.g. a Cube_low contact was attributed to Cube_high because the kernel"
522+
" assumed env-major instead of pattern-major flat-buffer layout."
523+
)
524+
525+
462526
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
463527
@pytest.mark.isaacsim_ci
464528
def test_sensor_print(device):

0 commit comments

Comments
 (0)