import hashlib import json import os import sys import unittest import xml.etree.ElementTree as ET from pathlib import Path import numpy as np import torch from scipy.spatial.transform import Rotation PROJECT_ROOT = Path(__file__).resolve().parents[1] sys.path[:0] = [str(PROJECT_ROOT / "src"), str(PROJECT_ROOT)] os.environ["PSI_SKIP_VLM_MODEL"] = "1" from psi.config.transform import ActionStateTransform from psi.data.seed.motion_repr40 import encode_root40 # +测试直接声明机器人关节顺序,不再依赖已退出当前训练入口的数据转换脚本。 MUJOCO_TO_ISAACLAB = np.asarray( [0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 15, 22, 4, 10, 16, 23, 5, 11, 17, 24, 18, 25, 19, 26, 20, 27, 21, 28], dtype=np.int64, ) class Normalization40Test(unittest.TestCase): def setUp(self): self.path = PROJECT_ROOT / "config/motion_bounds_root40_v1.json" self.statistics = json.loads(self.path.read_text()) self.low = np.asarray(self.statistics["action"]["min"], dtype=np.float32) self.high = np.asarray(self.statistics["action"]["max"], dtype=np.float32) self.transform = ActionStateTransform(stat_path=str(self.path), action_norm_type="bounds", normalize_state=True, pad_action_dim=40, pad_state_dim=40) def test_static_protocol_matches_urdf_and_isaac_order(self): from train_vlm import check_root40_bounds metadata = self.statistics["meta"] urdf = PROJECT_ROOT / metadata["joint_limit_source"] kinematics_path = PROJECT_ROOT / metadata["kinematics_source"] self.assertEqual(hashlib.sha256(urdf.read_bytes()).hexdigest(), metadata["joint_limit_sha256"]) self.assertEqual(hashlib.sha256(kinematics_path.read_bytes()).hexdigest(), metadata["kinematics_sha256"]) joints = json.loads(kinematics_path.read_text())["joint_order"] expected_names = [joints[index] for index in MUJOCO_TO_ISAACLAB] self.assertEqual(metadata["feature_names"][:29], expected_names) limits = {joint.attrib["name"]: [float(joint.find("limit").attrib[key]) for key in ("lower", "upper")] for joint in ET.parse(urdf).getroot().findall("joint") if joint.attrib["type"] == "revolute"} self.assertEqual(self.statistics["action"]["min"][:29], [limits[name][0] for name in expected_names]) self.assertEqual(self.statistics["action"]["max"][:29], [limits[name][1] for name in expected_names]) self.assertEqual(self.statistics["action"], self.statistics["states"]) self.assertEqual(self.low.shape, (40,)) self.assertTrue(np.all(self.high > self.low)) self.assertEqual(check_root40_bounds(self.path), hashlib.sha256(self.path.read_bytes()).hexdigest()) def test_endpoints_and_shared_action_state_inverse(self): rng = np.random.default_rng(4080) raw = self.low + rng.random((512, 40), dtype=np.float32) * (self.high - self.low) raw = np.concatenate([self.low[None], self.high[None], raw]) normalized = self.transform({"actions": raw.copy(), "states": raw.copy()}) np.testing.assert_array_equal(normalized["actions"], normalized["states"]) np.testing.assert_array_equal(normalized["raw_actions"], raw) np.testing.assert_array_equal(normalized["actions"][:2], np.stack([-np.ones(40), np.ones(40)])) np.testing.assert_allclose(self.transform.reverse_call(normalized["actions"]), raw, atol=8e-7, rtol=0) np.testing.assert_allclose(self.transform.denormalize(normalized["actions"]), raw, atol=8e-7, rtol=0) np.testing.assert_allclose(normalized["actions"][:, 31:37], raw[:, 31:37], atol=1e-7, rtol=0) def test_absolute_height_contact_mapping_and_torch_gradient(self): raw = np.tile((self.low + self.high) / 2, (6, 1)) raw[:, 37] = [0, 0.156112, 0.8, 4, 6.4163857, 8] raw[:, 38:40] = np.arange(12).reshape(6, 2) % 2 normalized = self.transform({"actions": raw.copy(), "states": raw.copy()}) np.testing.assert_allclose(normalized["actions"][:, 37], raw[:, 37] / 4 - 1, atol=1e-7, rtol=0) np.testing.assert_array_equal(normalized["actions"][:, 38:40], raw[:, 38:40] * 2 - 1) tensor = torch.tensor(normalized["actions"], requires_grad=True) restored = self.transform.denormalize(tensor) np.testing.assert_allclose(restored.detach().numpy(), raw, atol=8e-7, rtol=0) restored.sum().backward() torch.testing.assert_close(tensor.grad, torch.tensor((self.high - self.low) / 2).expand_as(tensor)) def test_raw_encoding_stays_in_bounds_under_arbitrary_yaw_anchor(self): rng = np.random.default_rng(506) raw = np.zeros((101, 36), dtype=np.float32) raw[:, :29] = (self.low[:29] + self.high[:29]) / 2 direction = rng.uniform(-np.pi, np.pi, len(raw) - 1) delta = np.column_stack([np.cos(direction), np.sin(direction)]) * 0.119 raw[1:, 29:31] = np.cumsum(delta, axis=0) raw[:, 31] = np.linspace(0, 8, len(raw)) raw[:, 32:] = Rotation.random(len(raw), random_state=rng).as_quat()[:, [3, 0, 1, 2]] for yaw in np.linspace(-np.pi, np.pi, 25): heading = Rotation.from_euler("z", yaw).as_matrix() actions, states, _ = encode_root40(raw[1:], raw[:1], anchor=heading) self.assertLess(float(np.abs(actions[:, 29:31]).max()), 0.12) normalized = self.transform({"actions": actions.copy(), "states": states.copy()}) np.testing.assert_allclose(self.transform.denormalize(normalized["actions"]), actions, atol=8e-7, rtol=0) np.testing.assert_allclose(self.transform.denormalize(normalized["states"]), states, atol=8e-7, rtol=0) def test_tolerances_distinguish_roundoff_from_material_joint_changes(self): exact = np.asarray(self.statistics["action"]["max"], dtype=np.float64) near = exact.copy() far = exact.copy() near[9] += 5e-5 far[9] += 2e-4 self.assertFalse(np.any(near[:29] - exact[:29] > 1e-4)) self.assertTrue(np.any(far[:29] - exact[:29] > 1e-4)) source = np.stack([near, far]).astype(np.float32) unchanged = source.copy() normalized = self.transform({"actions": source, "states": source.copy()}) np.testing.assert_array_equal(source, unchanged) np.testing.assert_array_equal(normalized["raw_actions"], unchanged) self.assertTrue(np.all(normalized["actions"] <= 1)) if __name__ == "__main__": unittest.main()