"""Masking, variable DoF, equivalent token permutations, FK, and PPO semantics.""" import copy import pytest import torch from SkillFactory.legs.policy import LegActorCritic, compute_gae, load_policy, ppo_update, save_policy from SkillFactory.legs.tokens import FEATURE_SCHEMA, LegKinematics, pad_joint_observations, static_tokens from SkillFactory.legs.urdf import extract_leg, forward from test_leg_assets import fixture_spec def batch(b=3, j=6, e=1): result = { name: torch.randn(b, j if name.startswith("joint") else e, len(FEATURE_SCHEMA[name])) for name in ("joint_description", "joint_observation", "effector_description", "effector_observation") } result.update( joint_mask=torch.tensor([[True, True, False, False, False, False]]).expand(b, -1).clone(), effector_mask=torch.ones(b, e, dtype=torch.bool), context=torch.randn(b, len(FEATURE_SCHEMA["context"])), goal=torch.randn(b, 3), ) return result def test_padding_and_missing_feet_do_not_affect_active_policy(): torch.manual_seed(3) model = LegActorCritic() original = batch() action, logp, value = model.act(original, deterministic=True) changed = copy.deepcopy(original) changed["joint_description"][:, 2:] = torch.nan changed["joint_observation"][:, 2:] = torch.nan other, other_log, other_value = model.act(changed, deterministic=True) assert torch.equal(action, other) and torch.equal(logp, other_log) and torch.equal(value, other_value) assert (action[:, 2:] == 0).all() original["effector_mask"][:] = False original["effector_description"][:] = torch.nan original["effector_observation"][:] = torch.nan assert all(torch.isfinite(t).all() for t in model.act(original)) def test_permutation_equivariance_and_no_inactive_gradients(): torch.manual_seed(42) model = LegActorCritic() original = batch() original["joint_description"].requires_grad_() original["joint_observation"].requires_grad_() permutation = torch.tensor([3, 1, 5, 0, 4, 2]) shuffled = {k: v[:, permutation] if k.startswith("joint") else v for k, v in original.items()} a, _, v = model.act(original, deterministic=True) b, _, w = model.act(shuffled, deterministic=True) torch.testing.assert_close(b, a[:, permutation], atol=1e-6, rtol=1e-6) torch.testing.assert_close(v, w, atol=1e-6, rtol=1e-6) (a.sum() + v.sum()).backward() assert original["joint_description"].grad[:, 2:].abs().max() == 0 assert original["joint_observation"].grad[:, 2:].abs().max() == 0 assert original["joint_observation"].grad[:, :2].abs().sum() > 0 distribution, _ = model.distribution_value(original) _, entropy, _ = model.evaluate(original, a) torch.testing.assert_close(entropy, distribution.entropy()[:, :2].sum(dim=-1)) @pytest.mark.parametrize("n", [1, 2, 4]) def test_actual_fk_and_shared_network_across_declared_dof(tmp_path, n): spec = fixture_spec(tmp_path, n) bundle = extract_leg(spec, tmp_path / "asset") static = static_tokens(bundle, 6) q = torch.linspace(-0.2, 0.3, n, dtype=torch.float32)[None, :].expand(3, -1) fk = LegKinematics(bundle) actual = fk.foot_position(q)[0] expected, _ = forward(bundle["tree"], dict(zip(bundle["joint_order"], q[0].tolist()))) torch.testing.assert_close(actual, torch.tensor([expected["toe"][i][3] for i in range(3)]), atol=1e-6, rtol=1e-6) tokens = {k: v.expand(3, *v.shape[1:]) for k, v in static.items()} tokens.update( joint_observation=pad_joint_observations(q, q * 0, q * 0, bundle, 6), effector_observation=torch.zeros(3, 1, 11), context=torch.zeros(3, len(FEATURE_SCHEMA["context"])), goal=torch.zeros(3, 3), ) model = LegActorCritic() action, _, _ = model.act(tokens) assert action.shape == (3, 6) and (action[:, n:] == 0).all() assert static["joint_mask"].sum() == n with pytest.raises(ValueError): static_tokens(bundle, n - 1) def test_inertia_and_com_are_in_anchor_nominal_frame(tmp_path): import xml.etree.ElementTree as ET spec = fixture_spec(tmp_path, 1) tree = ET.parse(tmp_path / "robot.urdf") item = tree.getroot().find("link[@name='segment0']/inertial") ET.SubElement(item, "origin", xyz="0.01 0 0", rpy="0 0 1.5707963267948966") item.find("inertia").set("iyy", "0.002") item.find("inertia").set("izz", "0.003") tree.write(tmp_path / "robot.urdf") bundle = extract_leg(spec, tmp_path / "asset") description = static_tokens(bundle, 3)["joint_description"][0, 0] scale = bundle["known_mass_scale_kg"] * bundle["length_scale_m"] ** 2 torch.testing.assert_close( description[[15, 18, 20]] * scale, torch.tensor([0.002, 0.001, 0.003]), atol=1e-7, rtol=1e-6 ) torch.testing.assert_close( description[21:24] * bundle["length_scale_m"], torch.tensor([0.01, 0.0, 0.0]), atol=1e-7, rtol=1e-6 ) def test_checkpoint_reload_is_exact(tmp_path): model = LegActorCritic().eval() observation = batch() save_policy(model, tmp_path / "policy.pt", {"test": True}) loaded = load_policy(tmp_path / "policy.pt").eval() assert all(torch.equal(a, b) for a, b in zip(model.act(observation, True), loaded.act(observation, True))) def test_gae_bootstraps_timeout_not_reset_state(): rewards = torch.tensor([[1.0, 1.0], [1.0, 1.0]]) values = torch.zeros_like(rewards) # terminal first env bootstrap=0, timeout second env terminal critic=10 next_values = torch.tensor([[0.0, 0.0], [0.0, 10.0]]) done = torch.tensor([[False, False], [True, True]]) advantage, returns = compute_gae(rewards, values, next_values, done, gamma=0.9, lam=1.0) torch.testing.assert_close(returns, torch.tensor([[1.9, 10.0], [1.0, 10.0]])) assert torch.equal(returns, advantage) def test_ppo_mixed_masks_updates_actor_without_rollout_gradients(): torch.manual_seed(1) model = LegActorCritic(hidden=16, latent=8) observation = batch(b=4) observation["joint_mask"][2:, 2] = True action, logp, value = model.act(observation) data = { "observation": {key: item[None].repeat(3, *([1] * item.ndim)) for key, item in observation.items()}, "actions": action[None].expand(3, -1, -1), "log_prob": logp[None].expand(3, -1), "values": value[None].expand(3, -1), "rewards": torch.randn(3, 4), "next_values": value[None].expand(3, -1), "done": torch.zeros(3, 4, dtype=torch.bool), } before = model.mean[-1].weight.detach().clone() stats = ppo_update(model, torch.optim.Adam(model.parameters(), lr=1e-3), data, epochs=2, minibatch=5) assert not torch.equal(before, model.mean[-1].weight) assert all(torch.isfinite(torch.tensor(v)) for v in stats.values()) advantage, _ = compute_gae(data["rewards"], data["values"], data["next_values"], data["done"]) assert not advantage.requires_grad with pytest.raises(ValueError, match="positive"): ppo_update(model, torch.optim.Adam(model.parameters()), data, epochs=0) data["log_prob"] = torch.full((3, 4), torch.nan) frozen = {key: item.clone() for key, item in model.state_dict().items()} with pytest.raises(ValueError, match="finite"): ppo_update(model, torch.optim.Adam(model.parameters()), data) assert all(torch.equal(model.state_dict()[key], item) for key, item in frozen.items()) @pytest.mark.parametrize("empty", [True, False]) def test_gae_rejects_empty_or_nonfinite_inputs(empty): reward = torch.zeros(0 if empty else 1, 2) if not empty: reward[0, 0] = torch.nan with pytest.raises(ValueError): compute_gae(reward, reward, reward, torch.zeros_like(reward, dtype=torch.bool)) def test_float32_contract_ignores_global_default_dtype(tmp_path): spec = fixture_spec(tmp_path, 2) bundle = extract_leg(spec, tmp_path / "asset") old = torch.get_default_dtype() try: torch.set_default_dtype(torch.float64) static = static_tokens(bundle, 4) q = torch.zeros(2, 2, dtype=torch.float32) assert static["joint_description"].dtype == torch.float32 assert static["effector_description"].dtype == torch.float32 assert LegKinematics(bundle).foot_position(q).dtype == torch.float32 assert pad_joint_observations(q, q, q, bundle, 4).dtype == torch.float32 assert all(p.dtype == torch.float32 for p in LegActorCritic().parameters()) finally: torch.set_default_dtype(old)