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