Files
SkillFactory/tests/test_four_skills_safety.py

89 lines
4.7 KiB
Python

"""Synthetic safety fault injection, never native physical acceptance."""
import ast
from pathlib import Path
from types import SimpleNamespace as NS
import pytest
import torch
ROOT = Path(__file__).resolve().parents[1]
PATH = ROOT / 'source/SkillFactory/SkillFactory/tasks/manager_based/four_skills/mdp.py'
tree = ast.parse(PATH.read_text())
method = next(n for c in tree.body if isinstance(c, ast.ClassDef) and c.name == 'FourSkillState'
for n in c.body if isinstance(n, ast.FunctionDef) and n.name == 'safety')
from SkillFactory.legs.contact_cycle.task import phase_failures, phase_objective
namespace = {'torch': torch, 'phase_failures': phase_failures}
exec(compile(ast.fix_missing_locations(ast.Module(body=[method], type_ignores=[])), str(PATH), 'exec'), namespace)
def fake_state():
torque = torch.zeros(1, 3)
q = torch.zeros(1, 3)
return NS(root=lambda: (torch.tensor([[0., 0., .3]]), torch.tensor([[0., 0., 0., 1.]]),
torch.zeros(1, 3), torch.zeros(1, 3)),
foot_center=lambda: torch.tensor([[0., 0., .02]]), bundle={'foot': {'radius': .02}},
robot=NS(data=NS(applied_torque=NS(torch=torque), joint_vel=NS(torch=torch.zeros(1, 3)))),
joints=[0, 1, 2], effort=torch.ones(3) * 20, speed=torch.ones(3) * 20,
lower=torch.ones(3) * -2, upper=torch.ones(3) * 2, q=lambda: q,
raw_sensors=lambda: torch.zeros(1, 18), contact=lambda: torch.tensor([[0., 0., 50.]]),
guide=torch.zeros(1, 2, 6), auxiliary=torch.zeros(1, 2, 6), external=torch.zeros(1, 2, 6),
base_origin=torch.tensor([[0., 0., .3]]), acquisition=NS(failed=torch.tensor([False])),
separation=NS(premature=torch.tensor([False])), weight=torch.tensor([50.]),
reference=lambda: (torch.tensor([0]), torch.tensor([0.]), None, None, None),
controller=NS(q_ik=torch.zeros(1, 3), action=torch.zeros(1, 3)),
residual_request=torch.zeros(1, 3), tilt=lambda: torch.zeros(1))
@pytest.mark.parametrize('bad', [float('nan'), float('inf'), -float('inf')])
def test_nonfinite_torque_fails_closed_not_nan_comparison_bypass(bad):
s = fake_state()
assert not any(bool(x.any()) for x in namespace['safety'](s).values())
s.robot.data.applied_torque.torch[0, 1] = bad
assert namespace['safety'](s)['nonfinite'].tolist() == [True]
def test_original_actuator_collision_and_guide_limits_preserved():
s = fake_state()
s.robot.data.applied_torque.torch[0, 0] = 21
assert namespace['safety'](s)['effort_velocity'].item()
s = fake_state()
s.guide[0, 0, 2] = .001
assert namespace['safety'](s)['vertical_guide'].item()
s = fake_state()
s.foot_center = lambda: torch.tensor([[0., 0., .0179]])
assert namespace['safety'](s)['penetration'].item()
def test_brief_ready_cannot_disable_requested_hold_watchdog():
method = next(n for n in tree.body if isinstance(n, ast.FunctionDef) and n.name == 'hold_watchdog')
ns = {'torch': torch}
exec(compile(ast.fix_missing_locations(ast.Module(body=[method], type_ignores=[])), str(PATH), 'exec'), ns)
completed = torch.tensor([False])
deadline = torch.tensor([4.])
required = torch.tensor([.4])
expired = torch.tensor([False])
# Even granting brief ready intervals directly, no0.4s hold can complete.
for tick in range(1, 2501):
ready = torch.tensor([tick % 100 < 26])
held = torch.tensor([.002 * (tick % 100) if bool(ready[0]) else 0.])
completed, now_expired = ns['hold_watchdog'](ready, held, required, completed,
torch.tensor([tick * .002]), deadline)
expired |= now_expired
assert expired.item() and not completed.item()
# A genuinely completed hold stays complete during indefinite endpoint hold;
# the separate existing bad-hold rule still owns later physical failures.
completed, expired = ns['hold_watchdog'](torch.tensor([True]), torch.tensor([.5]), required,
torch.tensor([False]), torch.tensor([3.]), deadline)
assert completed.item() and not expired.item()
completed, expired = ns['hold_watchdog'](torch.tensor([False]), torch.tensor([0.]), required,
completed, torch.tensor([100.]), deadline)
assert completed.item() and not expired.item()
def test_phase_reward_does_not_pay_for_early_lift_during_unload():
def score(gap):
return phase_objective(torch.tensor([1]), torch.tensor([.5]), torch.tensor([gap]),
torch.tensor([gap]), torch.tensor([25.]), torch.tensor([25.]), torch.zeros(1),
torch.zeros(1), torch.tensor([50.]))[0]
assert score(0.).item() > score(.01).item()