code-Movens-Zero_train / tests /test_normalization40.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
bda0250 verified
Raw History Blame Contribute Delete
6.49 kB
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
# <modify>+测试直接声明机器人关节顺序,不再依赖已退出当前训练入口的数据转换脚本。
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()