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