"""Native tensor matrix views must support entity poses without copying physics buffers.""" from dataclasses import replace from xml.etree import ElementTree as ET import numpy as np import pytest import torch from mjrl.backend.native_sim import _NativeData @pytest.mark.parametrize('name', ['xmat', 'site_xmat', 'geom_xmat']) def test_native_matrix_view_shares_the_original_flat_buffer(name): raw = np.arange(36, dtype=np.float32).reshape(2, 2, 9) data = _NativeData({name: raw}, 2) held = getattr(data, name) assert held.shape == (2, 2, 3, 3) assert data.numpy(name) is raw and raw.shape == (2, 2, 9) raw[1, 0, 5] = 123. assert held[1, 0, 1, 2].item() == 123. held[0, 1, 2, 0] = 456. assert raw[0, 1, 6] == 456. # Attribute assignment also writes the original gather/scatter allocation. setattr(data, name, torch.full_like(held, 7.)) np.testing.assert_array_equal(raw, np.full_like(raw, 7.)) def test_native_entity_site_and_geom_poses_match_rotated_mujoco_states(neutral_robot, tmp_path): from mjlab.envs import ManagerBasedRlEnv from mjlab.sim import get_simulation_cls, set_simulation_cls from mjlab.utils.lab_api.math import matrix_from_quat from mjrl.backend import native_sim from mjrl.backend.resolve import resolve from mjrl.backend.select import use_backend from tasks.templates.Biped_Template.env_cfg import env_cfg tree = ET.parse(neutral_robot.model_path) base = tree.find(".//body[@name='base']") ET.SubElement(base, 'site', name='offset_site', pos='.11 -.03 .06', euler='.4 -.2 .3') tree.find(".//geom[@name='trunk']").set('euler', '-.3 .2 -.1') path = tmp_path / 'rotated.xml' tree.write(path) robot_spec = replace(neutral_robot, model_path=path) saved_class, saved_threads = get_simulation_cls(), native_sim._DEFAULT_NTHREAD env = None try: use_backend(resolve(backend='native', device='cpu', num_envs=2, cpu_threads=1)) cfg = env_cfg(robot_spec, play=True) cfg.scene.num_envs = 2 env = ManagerBasedRlEnv(cfg, device='cpu') robot, sim = env.scene['robot'], env.sim site_views = sim.data.site_xmat # Keep a reference across native forwards. old_site_matrices = site_views.clone() for phase in (0., .25): sim.data.qpos[:, :3] = torch.tensor([[.2, -.1, .7], [-.4, .3, .9]]) for world, angle in enumerate((.6 + phase, -.8 - phase)): sim.data.qpos[world, 3:7] = torch.tensor( [np.cos(angle / 2), 0., 0., np.sin(angle / 2)], dtype=torch.float32) sim.data.qpos[world, 7:] = torch.tensor([.2 + phase, -.3]) sim.forward() assert not torch.allclose(site_views, old_site_matrices) old_site_matrices = site_views.clone() for kind in ('site', 'geom'): indices = getattr(robot.data.indexing, f'{kind}_ids').cpu().numpy() poses = getattr(robot.data, f'{kind}_pose_w') matrices = matrix_from_quat(poses[..., 3:]).cpu().numpy() for world in range(2): _, native = sim.env_mjdata(world) np.testing.assert_allclose( poses[world, :, :3].cpu().numpy(), getattr(native, f'{kind}_xpos')[indices], atol=1e-6, rtol=0) np.testing.assert_allclose( matrices[world], getattr(native, f'{kind}_xmat')[indices].reshape(-1, 3, 3), atol=1e-6, rtol=0) # Distinct worlds and nonidentity matrices make order and # orientation errors observable, not just a shape assertion. assert not np.allclose(matrices[0], matrices[1]) assert not np.allclose(matrices[0], np.eye(3)) np.testing.assert_array_equal( getattr(robot.data, f'{kind}_pos_w').cpu().numpy(), poses[..., :3].cpu().numpy()) # Raycast consumers keep accepting the same matrices via view. assert sim.data.xmat[:, 1].view(-1, 3, 3).shape == (2, 3, 3) # Regression control: restoring the old flattened tensor layout makes # the public pose accessor reject the flattened rotation matrices. sim.data._t['site_xmat'] = site_views.reshape(2, -1, 9) with pytest.raises(torch.jit.Error, match='Invalid rotation matrix shape'): _ = robot.data.site_pos_w sim.data._t['site_xmat'] = site_views finally: if env is not None: env.close() set_simulation_cls(saved_class) native_sim._DEFAULT_NTHREAD = saved_threads