diff --git a/embodichain/gen_sim/task_engine/_task_program/twist_geometry_guard.py b/embodichain/gen_sim/task_engine/_task_program/twist_geometry_guard.py index 1af905baf..a01625f73 100644 --- a/embodichain/gen_sim/task_engine/_task_program/twist_geometry_guard.py +++ b/embodichain/gen_sim/task_engine/_task_program/twist_geometry_guard.py @@ -64,7 +64,7 @@ def _matrix(value: Any) -> np.ndarray: def _native_table(table: Any) -> tuple[np.ndarray, float]: if table.num_instances != 1 or table.body_type not in {"static", "kinematic"}: raise ValueError("E8 table screen requires one static/kinematic support.") - entity = table.body_data.entities[0] + entity = table._entities[0] body = entity.native().get_physical_body() shapes = table.get_collision_shapes(env_id=0) if body is None or not shapes or len(shapes) != int(body.get_shape_count()): diff --git a/tests/gen_sim/task_engine/test_twist.py b/tests/gen_sim/task_engine/test_twist.py index 8f5427791..3de610602 100644 --- a/tests/gen_sim/task_engine/test_twist.py +++ b/tests/gen_sim/task_engine/test_twist.py @@ -40,6 +40,51 @@ from embodichain.utils.utility import load_config, save_config +@pytest.mark.parametrize( + ("body_type", "shape_count", "contact_offset", "error"), + [ + ("static", 1, 0.01, None), + ("kinematic", 1, 0.01, None), + ("static", 2, 0.01, "requires all actual native table colliders"), + ("static", 1, -0.01, "contact offsets are invalid"), + ], +) +def test_native_table_reads_static_entity_and_checks_shape_and_contact( + body_type, shape_count, contact_offset, error +): + from embodichain.gen_sim.task_engine._task_program.twist_geometry_guard import ( + _native_table, + ) + + body = NS(get_shape_count=lambda: shape_count) + entity = NS( + native=lambda: NS(get_physical_body=lambda: body), + get_physical_attr=lambda: NS(contact_offset=contact_offset, rest_offset=0.0), + ) + shape = NS( + local_pose=torch.eye(4), + vertices=None, + triangles=None, + half_extents=torch.tensor([1.0, 2.0, 3.0]), + ) + table = NS( + num_instances=1, + body_type=body_type, + body_data=None if body_type == "static" else NS(entities=[entity]), + _entities=[entity], + get_collision_shapes=lambda env_id: [shape], + get_local_pose=lambda to_matrix: torch.eye(4).unsqueeze(0), + ) + if error is not None: + with pytest.raises(ValueError, match=error): + _native_table(table) + return + + bounds, contact = _native_table(table) + np.testing.assert_array_equal(bounds, [[-1, -2, -3], [1, 2, 3]]) + assert contact == 0.01 + + @pytest.mark.parametrize("mode", ["safe", "collision", "missing", "nan"]) def test_table_clearance_requires_complete_finite_geometry(monkeypatch, mode): from embodichain.gen_sim.task_engine._task_program import (