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()