89 lines
4.7 KiB
Python
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()
|