"""Guards for the xtrainer2 deployment path, from robot observation to command. The failure modes these lock down are silent: a quaternion in the wrong order, a state normalized differently than at training, or a 16/20 dimension split taken at the wrong offsets all produce plausible numbers that move the arms to the wrong pose. Each expectation is re-derived here with plain numpy rather than by calling the code under test. No GPU, no checkpoint weights and no dataset: only ``config.yaml`` and ``dataset_statistics.json``, which is all the normalization path reads. """ from __future__ import annotations import json from pathlib import Path import numpy as np import pytest pytest.importorskip("torch") pytest.importorskip("pytorch3d.transforms") import torch # noqa: E402 from pytorch3d import transforms as pt # noqa: E402 from deployment.model_server import xtrainer2_command # noqa: E402 from deployment.model_server.policy_norm_processor import PolicyNormProcessor # noqa: E402 from deployment.model_server.policy_wrapper import PolicyServerWrapper # noqa: E402 RAW_DIMS = 16 MODEL_DIMS = 20 UNNORM_KEY = "new_embodiment" # Raw layout, per arm: position(3), quaternion(4), gripper(1). POSITION_SLICES = (slice(0, 3), slice(8, 11)) GRIPPER_INDICES = (7, 15) QUATERNION_SLICES = tuple(slice(start, start + 4) for start in xtrainer2_command.QUATERNION_STARTS) def _stats(seed: int) -> dict: """A plausible 16-D statistics block, as ``save_dataset_statistics`` writes it.""" rng = np.random.default_rng(seed) q01 = rng.uniform(-1.0, -0.2, size=RAW_DIMS) q99 = q01 + rng.uniform(0.5, 2.0, size=RAW_DIMS) return { "mean": ((q01 + q99) / 2).tolist(), "std": np.full(RAW_DIMS, 0.5).tolist(), "min": (q01 - 0.1).tolist(), "max": (q99 + 0.1).tolist(), "q01": q01.tolist(), "q99": q99.tolist(), } @pytest.fixture(scope="module") def run_dir(tmp_path_factory) -> Path: """A checkpoint-shaped directory: the two files deployment actually reads.""" directory = tmp_path_factory.mktemp("xtrainer2_run") config = { "framework": { "name": "QwenGR00T", "qwenvl": {"base_vlm": "/models/florence-2-large", "vl_hidden_dim": 1024}, "action_model": {"action_dim": MODEL_DIMS, "state_dim": MODEL_DIMS, "action_horizon": 16, "hidden_size": 1024}, }, "datasets": {"vla_data": {"data_mix": "xtrainer2_pack_phone", "include_state": True}}, "trainer": {"repeated_diffusion_steps": 4}, } (directory / "config.yaml").write_text(json.dumps(config)) statistics = {UNNORM_KEY: {"action": _stats(0), "state": _stats(1)}} statistics[UNNORM_KEY]["action"]["mask"] = [True] * RAW_DIMS (directory / "dataset_statistics.json").write_text(json.dumps(statistics)) (directory / "checkpoints").mkdir() # Only the path is inspected: the normalization path never reads weights. (directory / "checkpoints" / "steps_1_pytorch_model.pt").touch() return directory @pytest.fixture(scope="module") def processor(run_dir: Path) -> PolicyNormProcessor: return PolicyNormProcessor(str(run_dir / "checkpoints" / "steps_1_pytorch_model.pt")) def _raw_frame(seed: int = 7) -> np.ndarray: """One 16-D frame with unit quaternions, in the training (wxyz) layout.""" rng = np.random.default_rng(seed) frame = np.zeros(RAW_DIMS, dtype=np.float32) for position in POSITION_SLICES: frame[position] = rng.uniform(-0.3, 0.3, size=3) for quaternion in QUATERNION_SLICES: value = rng.normal(size=4) frame[quaternion] = value / np.linalg.norm(value) for gripper in GRIPPER_INDICES: frame[gripper] = rng.uniform(0.0, 1.0) return frame def _q99(values: np.ndarray, q01: np.ndarray, q99: np.ndarray) -> np.ndarray: return np.clip(2 * (values - q01) / (q99 - q01) - 1, -2.2, 2.2) def test_raw_and_model_widths_are_the_16_to_20_expansion(processor): """Per arm, quaternion(4) -> rotation_6d(6); everything else is unchanged.""" assert processor.raw_state_dim == RAW_DIMS assert processor.raw_action_dim == RAW_DIMS assert processor.state_dim == MODEL_DIMS assert processor.action_dim == MODEL_DIMS def test_apply_state_matches_the_training_normalization(processor, run_dir): """Position and gripper are q99; the rotation becomes 6D on fixed ±1 bounds.""" statistics = json.loads((run_dir / "dataset_statistics.json").read_text())[UNNORM_KEY]["state"] q01 = np.array(statistics["q01"], dtype=np.float32) q99 = np.array(statistics["q99"], dtype=np.float32) raw = _raw_frame() normalized = processor.apply_state(raw) assert normalized.shape == (1, MODEL_DIMS) # Model layout: [pos(3), rot6d(6), grip(1)] per arm. for arm, (position, quaternion, gripper) in enumerate( zip(POSITION_SLICES, QUATERNION_SLICES, GRIPPER_INDICES) ): offset = arm * 10 expected_position = _q99(raw[position], q01[position], q99[position]) assert np.allclose(normalized[0, offset:offset + 3], expected_position, atol=1e-6) matrix = pt.quaternion_to_matrix(torch.tensor(raw[quaternion], dtype=torch.float32)) expected_rotation = pt.matrix_to_rotation_6d(matrix).numpy() # min_max over [-1, 1] is the identity, so the 6D block is stored as-is. assert np.allclose(normalized[0, offset + 3:offset + 9], expected_rotation, atol=1e-6) expected_gripper = _q99(raw[gripper], q01[gripper], q99[gripper]) assert np.allclose(normalized[0, offset + 9], expected_gripper, atol=1e-6) def test_unapply_actions_returns_raw_width_and_recovers_the_rotation(processor): """A normalized chunk comes back as 16 raw dims with a unit quaternion.""" raw = _raw_frame(seed=11) normalized_chunk = np.repeat(processor.apply_state(raw), 4, axis=0) recovered = processor.unapply_actions(normalized_chunk) assert recovered.shape == (4, RAW_DIMS) for quaternion in QUATERNION_SLICES: norms = np.linalg.norm(recovered[:, quaternion], axis=-1) assert np.allclose(norms, 1.0, atol=1e-5) # q and -q are the same rotation, so compare through the matrix. original = pt.quaternion_to_matrix(torch.tensor(raw[quaternion], dtype=torch.float32)) for step in recovered[:, quaternion]: matrix = pt.quaternion_to_matrix(torch.tensor(step, dtype=torch.float32)) assert torch.allclose(matrix, original, atol=1e-4) def test_prepare_examples_keeps_wxyz_pose_for_rel_compose(processor): model_frame = _raw_frame(seed=9) robot_frame = xtrainer2_command.model_actions_to_robot(model_frame) prepared = PolicyServerWrapper._prepare_examples( [{"image": [], "lang": "x", "state": robot_frame}], processor ) assert np.allclose(prepared[0]["_raw_state_wxyz"], model_frame, atol=1e-6) def test_server_reorders_the_incoming_robot_state(processor): """The robot speaks xyzw; the transforms were fitted on wxyz.""" model_frame = _raw_frame(seed=3) robot_frame = xtrainer2_command.model_actions_to_robot(model_frame) prepared = PolicyServerWrapper._prepare_examples( [{"image": [], "lang": "x", "state": robot_frame}], processor ) assert np.allclose(prepared[0]["state"], processor.apply_state(model_frame), atol=1e-6) def test_a_state_of_model_width_is_rejected(processor): """20 dims means the client normalized it too; that would be normalized twice.""" with pytest.raises(ValueError, match="raw"): PolicyServerWrapper._prepare_examples( [{"state": np.zeros(MODEL_DIMS, dtype=np.float32)}], processor ) def test_quaternion_reorder_is_an_involution_on_both_arms(): frame = _raw_frame(seed=5) assert np.allclose( xtrainer2_command.robot_state_to_model(xtrainer2_command.model_actions_to_robot(frame)), frame, ) robot = xtrainer2_command.model_actions_to_robot(frame) for quaternion in QUATERNION_SLICES: wxyz, xyzw = frame[quaternion], robot[quaternion] assert xyzw[3] == pytest.approx(wxyz[0]) # w moves to the end assert np.allclose(xyzw[:3], wxyz[1:]) def test_metadata_states_the_contract_the_client_verifies(processor, monkeypatch): """These fields are what the on-robot client refuses to run without.""" wrapper = PolicyServerWrapper.__new__(PolicyServerWrapper) wrapper._ckpt_path = "unused.pt" wrapper._action_chunk_size = 16 wrapper._uses_state = True wrapper._available_unnorm_keys = [UNNORM_KEY] wrapper._default_unnorm_key = UNNORM_KEY wrapper._normalized_action_clip = None monkeypatch.setattr(PolicyServerWrapper, "_get_processor", lambda self, key: processor) metadata = wrapper.metadata assert metadata["camera_order"] == [ "video.cam_high", "video.cam_left_wrist", "video.cam_right_wrist", ] assert metadata["state_dim"] == RAW_DIMS assert metadata["action_dim"] == RAW_DIMS assert metadata["model_state_dim"] == MODEL_DIMS assert metadata["model_action_dim"] == MODEL_DIMS assert metadata["robot_quaternion_format"] == "xyzw" assert metadata["model_quaternion_format"] == "wxyz" assert metadata["server_normalizes_state"] is True assert metadata["image_format"] == "HWC_RGB_uint8" def test_other_embodiments_keep_the_pass_through_behaviour(): """Only configs that opt in get server-side state handling.""" class _PlainConfig: action_keys = ["action.eef"] state_keys = ["state.eef"] class _PlainProcessor: _data_config = _PlainConfig() examples = [{"state": np.zeros(7, dtype=np.float32)}] assert PolicyServerWrapper._prepare_examples(examples, _PlainProcessor()) is examples