Files
UniversalTrain-ai/tests/test_native_pose_views.py
chenlin ceca51b0d8
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
Improve generic robot onboarding, walking controls and end-effector interaction
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
2026-11-06 10:54:58 +08:00

97 lines
4.6 KiB
Python

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