code-HFD / tests /test_root40_production.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
5d9f970 verified
Raw History Blame Contribute Delete
8.42 kB
"""Physical invariants of the production Root40 codec and RTC integration."""
from pathlib import Path
from types import SimpleNamespace
import json
import numpy as np
import pytest
import torch
from scipy.spatial.transform import Rotation
from psi.data.seed.motion_repr40 import (
build_root40_action, build_root40_state, choose_heading_anchor,
recover_seed_abs_from_root40_action, rot6d_to_matrix,
)
from psi.config.transform import SeedPosttrainRepackTransform, ActionStateTransform
from psi.runtime.motion_rollout import decode_action_chunk, next_rtc_prefix, clamp_generated_actions
ROOT = Path(__file__).resolve().parents[1]
def poses(n=35):
t = np.arange(n, dtype=np.float32)
result = np.zeros((n, 36), np.float32)
result[:, :29] = np.sin(t[:, None] * .07) * .1
result[:, 29] = t * .003
result[:, 30] = np.sin(t * .04) * .04
result[:, 31] = .75 + .01 * np.sin(t * .1)
quat = Rotation.from_euler('xyz', np.stack((.1*np.sin(t*.03), .15*np.cos(t*.02), t*.04), -1)).as_quat()
result[:, 32:36] = quat[:, [3,0,1,2]]
return result
def same_poses(a, b):
np.testing.assert_allclose(a[:, :32], b[:, :32], atol=2e-6)
dots = np.abs(np.sum(a[:, 32:] * b[:, 32:], -1))
np.testing.assert_allclose(dots, 1, atol=2e-6)
def field():
return ActionStateTransform(stat_path=str(ROOT/'assets/stats/seed_all_g1_root40_bounds.json'),
action_norm_type='bounds', pad_action_dim=40, pad_state_dim=40)
def norm(raw, f):
return (raw-np.asarray(f.action_min))/(np.asarray(f.action_max)-f.action_min)*2-1
def test_roundtrip_with_turn_roll_pitch_and_source_reset():
p = poses()
raw = build_root40_action(p[1:31], p[:1])
same_poses(recover_seed_abs_from_root40_action(raw, p[0]), p[1:31])
state = build_root40_state(p[:1])
np.testing.assert_array_equal(state[0,29:31], 0)
moved = p.copy(); moved[:,29:31] += [10,-20]
same_poses(recover_seed_abs_from_root40_action(build_root40_action(moved[1:31],moved[:1]),moved[0]),moved[1:31])
@pytest.mark.parametrize('executed', [1, 10, 20])
def test_rtc_reanchoring_preserves_world_poses_and_contacts(executed):
p, f = poses(), field()
raw = build_root40_action(p[1:31],p[:1])
raw[:,38:] = np.random.default_rng(12).uniform(size=(30,2))
encoded = norm(raw,f)
prefix = next_rtc_prefix(encoded,p[0],p[executed],f,executed_frames=executed)
same_poses(decode_action_chunk(prefix,p[executed],f),p[executed+1:executed+11])
np.testing.assert_allclose(f.denormalize(prefix)[:,38:],raw[executed:executed+10,38:],atol=1e-6)
def test_rebased_prefix_outside_box_is_not_clipped():
p, f = poses(), field()
p[:,32:] = [1,0,0,0]
p[20:,32:] = Rotation.from_euler('z',-45,degrees=True).as_quat()[[3,0,1,2]]
p[21:,29:31] = p[20,29:31] + np.arange(1,len(p)-20)[:,None] * (np.asarray(f.action_max[29:31])*.99)
raw = build_root40_action(p[1:31],p[:1])
prefix = next_rtc_prefix(norm(raw,f),p[0],p[20],f)
assert np.abs(prefix[:,29:31]).max() > 1
bounded = clamp_generated_actions(torch.zeros(1,30,40),torch.from_numpy(prefix[None]))
np.testing.assert_array_equal(bounded[0,:10].numpy(),prefix)
same_poses(decode_action_chunk(prefix,p[20],f),p[21:31])
def test_repack_history_uses_current_anchor_and_preserves_padding():
p = poses()
repack = SeedPosttrainRepackTransform(textop_actions=True,root40_actions=True,
action_chunk_size=30,pad_action_dim=40,pad_state_dim=40)
item = repack(dict(actions=p[3:33],states=p[2:3],action_mask=np.r_[np.ones(25),np.zeros(5)],
smoothness_context_actions=p[1:3],smoothness_context_reference_states=p[:1]))
assert item['actions'].shape == (30,40) and item['states'].shape == (1,40)
assert item['actions_mask'][-5:].sum() == 0
np.testing.assert_allclose(item['smoothness_context_actions'],
build_root40_action(p[1:3],p[:1],choose_heading_anchor(p[2:3])),atol=1e-6)
f = field()
transformed = f(item)
assert np.isfinite(transformed['actions']).all()
def test_degenerate_rotation_fallback_is_valid_and_bad_state_rejected():
rotations = rot6d_to_matrix(np.zeros((3,6)))
np.testing.assert_allclose(rotations @ rotations.transpose(0,2,1),np.broadcast_to(np.eye(3),(3,3,3)),atol=1e-6)
np.testing.assert_allclose(np.linalg.det(rotations),1,atol=1e-6)
with pytest.raises(ValueError,match='quaternion'):
build_root40_state(np.zeros((1,36)))
def test_smoothness_zero_gt_and_nonzero_differentiable_seam():
from psi.trainers.posttrain_full_duplex import PosttrainFullDuplexTrainer
target = torch.randn(2,30,40)
mask = torch.ones_like(target)
loss = PosttrainFullDuplexTrainer._clean_action_smoothness_losses_full(target,target,mask)
assert all(x.sum() == 0 for x in loss.values())
prediction = target.clone(); prediction[:,10,:38] += .2; prediction.requires_grad_()
loss = PosttrainFullDuplexTrainer._clean_action_smoothness_losses_full(prediction,target,mask)
total = sum(x.mean() for x in loss.values())
assert total > 0
total.backward()
assert torch.isfinite(prediction.grad).all() and prediction.grad[:,10].abs().sum() > 0
def test_old_checkpoint_cannot_be_relabelled_as_root40(tmp_path):
from psi.hfd_training_launcher import resolve
checkpoint = tmp_path/'ckpt_1'; checkpoint.mkdir()
(tmp_path/'run_config.json').write_text(json.dumps({'model':{'action_dim':39}}))
with pytest.raises(ValueError,match='39D/40D'):
resolve(ROOT,{'HFD_RESUME_FROM_CHECKPOINT':str(checkpoint)})
def test_normalization_and_representation_contract():
from psi.hfd_training_launcher import resolve
from psi.models.relative_instruction_memory import architecture_contract
o,_ = resolve(ROOT,{})
assert o['model.action-representation'] == 'root40_v1'
assert o['model.action-dim'] == o['model.odim'] == 40
assert architecture_contract(SimpleNamespace()) == {}
assert architecture_contract(SimpleNamespace(action_representation='root40_v1'))['motion_representation_v1']['height_index'] == 37
with pytest.raises(ValueError,match='normalization'):
resolve(ROOT,{},['--data.transform.field.action-norm-type=bounds_q99'])
def test_group_weights_preserve_mass_and_height_channel():
from psi.trainers.posttrain_v1 import PosttrainV1Trainer
weights = SimpleNamespace(joint_abs=1.,root_delta_xy=2.,root_rot5_delta=3.,root_height=4.,foot_contact=5.)
fake = SimpleNamespace(model_cfg=SimpleNamespace(action_dim=39),
train_cfg=SimpleNamespace(loss_group_weight=weights),device=torch.device('cpu'))
old = PosttrainV1Trainer._build_group_loss_weight(fake)
fake.model_cfg.action_dim = 40
new = PosttrainV1Trainer._build_group_loss_weight(fake)
for a,b in [(slice(0,29),slice(0,29)),(slice(29,31),slice(29,31)),
(slice(31,36),slice(31,37)),(slice(36,37),slice(37,38)),(slice(37,39),slice(38,40))]:
torch.testing.assert_close(old[a].sum()/39,new[b].sum()/40)
logs = PosttrainV1Trainer._action_group_losses(fake,torch.arange(40.).float())
assert logs['loss_group/root_height'] == 37
assert logs['loss_group/foot_contact'] == 38.5
def test_full_duplex_control_condition_with_root40_backward():
from psi.models.full_duplex_psi0 import FullDuplexPsi0Model
from psi.config.model_full_duplex_psi0 import FullDuplexPsi0ModelConfig
config = FullDuplexPsi0ModelConfig(action_representation='root40_v1',action_dim=40,odim=40,
action_input_projection_fp32=True,streaming_qwen_enabled=False,
n_conditions=0,hidden_dim=48,num_blocks=1,dim_feedforward=64,nhead=4,
view_feature_dim=8,use_future_progress_embedding=False,
streaming_memory_dim=16,streaming_memory_layers=1,streaming_memory_heads=4)
model = FullDuplexPsi0Model(config,None)
condition = model.encode_stream_input(source_ids=['a'],kv_cache=model.begin_source(['a']),
update_semantic_cache=True,vlm_hidden_states=torch.randn(1,5,8),attention_mask=torch.ones(1,5,dtype=torch.long))
action = model.forward_action_conditioned(condition=condition,action_samples=torch.randn(1,30,40),
known_action_prefix=None,states=torch.randn(1,1,40),timestep=torch.full((1,30),500.))
assert action.shape == (1,30,40)
(action.square().mean()+condition.control_logits.square().mean()).backward()
assert all(p.grad is None or torch.isfinite(p.grad).all() for p in model.parameters())