ceca51b0d8
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.
97 lines
4.6 KiB
Python
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
|