ReTime / real_robot /source /tests /test_xtrainer2_policy_server_contract.py
mmmmmcyaaa's picture
Add Xtrainer2 real robot source and Chinese handoff guide
8101696 verified
Raw History Blame Contribute Delete
9.81 kB
"""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