7f39abb613
Add declarative single-leg import, fixed-anchor benchmark, masked shared-policy PPO prototype, regression tests, and consolidated documentation.
184 lines
8.3 KiB
Python
184 lines
8.3 KiB
Python
"""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)
|