File size: 6,488 Bytes
bda0250 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 | 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()
|