File size: 8,422 Bytes
5d9f970
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
"""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())