Files
a7l20-hug-sim/tests/test_left_hand_fist_restore.py

175 lines
8.7 KiB
Python

"""Offline fake CAN tests; never open real hardware."""
import contextlib
import importlib.util
import io
import json
import sys
from pathlib import Path
import tempfile
from types import SimpleNamespace
import unittest
from unittest.mock import patch
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'scripts'))
spec = importlib.util.spec_from_file_location('fist', Path(__file__).resolve().parents[1]/'scripts/left_hand_fist_restore.py')
fist = importlib.util.module_from_spec(spec)
spec.loader.exec_module(fist)
INITIAL = [[247,76,253,0,0,253], [54,0,171,0,0,240],
[12,0,157,0,0,253], [37,0,163,0,0,253], [93,0,168,0,0,250]]
def message(**kw):
return SimpleNamespace(is_rx=True, is_error_frame=False, is_remote_frame=False, **kw)
class Bus:
def __init__(self, blocked=False, lag=False):
self.rows = [r.copy() for r in INITIAL]
self.queue, self.commands = [], []
self.blocked = blocked
self.lag = lag
self.targets = [r.copy() for r in INITIAL]
def __enter__(self): return self
def __exit__(self, *args): pass
def send(self, m, timeout=None):
assert m.arbitration_id == 0x28 and not m.is_extended_id
prop = m.data[0]
if 0x49 <= prop <= 0x4d or 0x59 <= prop <= 0x5d:
assert len(m.data)==1
raw=[2,0,2,0,0,2] if prop<0x50 else [0]*6
self.queue.append(message(arbitration_id=0x28,is_extended_id=False,data=[prop]+raw))
return
assert 0x41 <= prop <= 0x45
i = prop-0x41
if len(m.data) == 1:
if self.lag:
self.rows[i] = [a+max(-2,min(2,b-a)) for a,b in zip(self.rows[i],self.targets[i])]
self.queue.append(message(arbitration_id=0x28, is_extended_id=False, data=[prop]+self.rows[i]))
else:
assert len(m.data) == 7
self.commands.append(m)
self.targets[i] = list(m.data[1:])
if not self.blocked and not self.lag: self.rows[i] = list(m.data[1:])
def recv(self, timeout=0):
return self.queue.pop(0) if self.queue else None
class TestFist(unittest.TestCase):
def setUp(self): self.lookup=fist.LeftLookup()
def test_target_preserves_nonflexion(self):
target = fist.close_target(INITIAL,self.lookup)
for i in range(5):
for j in range(6):
if j not in (2,5): self.assertEqual(target[i][j], INITIAL[i][j])
else: self.assertLessEqual(target[i][j], INITIAL[i][j])
# Middle side swing byte 12 is preserved, NOT clipped through URDF limits.
self.assertEqual(target[2][0],12)
def test_invalid_input(self):
with self.assertRaises(ValueError): fist.close_target([[0]*6]*4,self.lookup)
with self.assertRaises(ValueError): fist.close_target([[999]*6]*5,self.lookup)
def test_default_no_can(self):
with patch('sys.argv',['fist']), contextlib.redirect_stdout(io.StringIO()):
self.assertEqual(fist.main(),0)
def run_fake(self, blocked, lag=False, open_only=False):
bus=Bus(blocked,lag)
fake=SimpleNamespace(Message=message,Bus=lambda **kwargs:bus)
with tempfile.TemporaryDirectory() as tmp:
root=Path(tmp); (root/'data').mkdir()
with patch.dict('sys.modules', {'can':fake}), patch.object(fist,'ROOT',root), \
patch.object(fist.time,'sleep',lambda _:None), \
patch('sys.argv',['fist','--execute']+(['--open'] if open_only else [])), \
contextlib.redirect_stdout(io.StringIO()), contextlib.redirect_stderr(io.StringIO()):
result=fist.main()
report=json.loads(next((root/'data').glob('left_hand_*/result.json')).read_text())
report['_events']=[json.loads(line) for line in next((root/'data').glob('left_hand_*/events.jsonl')).read_text().splitlines()]
self.assertEqual(next((root/'data').glob('left_hand_*/calibration_used.json')).read_bytes(),self.lookup.raw)
return result,report,bus
def test_close_and_restore(self):
result,report,bus=self.run_fake(False)
self.assertEqual(result,0); self.assertTrue(report['restored'])
self.assertEqual(bus.rows,INITIAL)
self.assertEqual(report['device_status_before']['fault_raw'],[[0]*6]*5)
previous=[r.copy() for r in INITIAL]
for m in bus.commands:
i=m.data[0]-0x41
self.assertLessEqual(max(abs(a-b) for a,b in zip(previous[i],m.data[1:])),2)
previous[i]=list(m.data[1:])
def test_open_only_never_closes_or_restores(self):
result,report,bus=self.run_fake(False,open_only=True)
self.assertEqual(result,0)
self.assertEqual(report['status'],'opened')
self.assertTrue(report['opened'])
previous=[r.copy() for r in INITIAL]
for m in bus.commands:
i=m.data[0]-0x41
for slot in range(6):
if slot in (2,5):
self.assertGreaterEqual(m.data[1+slot],previous[i][slot])
self.assertLessEqual(m.data[1+slot]-previous[i][slot],2)
else: self.assertEqual(m.data[1+slot],INITIAL[i][slot])
previous[i]=list(m.data[1:])
self.assertEqual(bus.rows,fist.open_target(INITIAL,self.lookup))
phases={e['context']['phase'] for e in report['_events'] if e['event']=='command_sent'}
self.assertEqual(phases,{'open'})
def test_blocked_stops_without_false_restore(self):
result,report,bus=self.run_fake(True)
self.assertEqual(result,1); self.assertFalse(report['restored'])
self.assertTrue(report['hold_command_sent'])
self.assertIn('fault_raw',report['device_status_after_failure'])
self.assertLess(len(bus.commands),160*5)
self.assertEqual(report['failure']['reason'],'tracking_error')
self.assertEqual(report['failure']['details']['context']['phase'],'close')
self.assertTrue(report['failure']['details']['offending_joints'])
self.assertTrue(any(e['event']=='fault' for e in report['_events']))
def test_realistic_lag_is_logged_not_silently_accepted(self):
result,report,bus=self.run_fake(False,lag=True)
self.assertEqual(result,1)
detail=report['failure']['details']
self.assertIn('actual_raw',detail); self.assertIn('command_raw',detail)
worst=detail['offending_joints'][0]
self.assertIn('error_deg',worst); self.assertIn('joint',worst)
self.assertGreater(abs(worst['error_u8']),18)
self.assertFalse(report['restored'])
def test_json_replaces_linear_pip_mapping(self):
self.assertAlmostEqual(self.lookup.angle('index_pip',31),1.54800055)
self.assertEqual(self.lookup.command('index_pip',.95),130)
target=fist.close_target(INITIAL,self.lookup)
self.assertEqual(target[1][5],130)
def test_motor_names_follow_json_not_legacy_thumb_swap(self):
desc=self.lookup.describe(INITIAL)
self.assertEqual(desc['thumb_cmc_yaw']['raw_u8'],247)
self.assertEqual(desc['thumb_cmc_roll']['raw_u8'],76)
self.assertEqual(desc['thumb_ip']['raw_u8'],253)
self.assertTrue(desc['thumb_ip']['passive_estimate'])
def test_no_out_of_range_clipping_or_passive_commands(self):
with self.assertRaises(ValueError): self.lookup.command('index_pip',100)
with self.assertRaises(ValueError): self.lookup.command('index_dip',.5)
def test_reject_wrong_side_and_malformed_table(self):
for kind in ('side','length','motor'):
d=json.loads(self.lookup.raw)
if kind=='side': d['side']='right'
if kind=='length': d['joints']['index_pip']['angle_rad'].pop()
if kind=='motor': d['joints']['thumb_cmc_roll']['motor_index']=10
with tempfile.TemporaryDirectory() as tmp:
p=Path(tmp)/'bad.json';p.write_text(json.dumps(d))
with self.assertRaises(ValueError): fist.LeftLookup(p)
def test_timeout_retains_property_and_partial_feedback(self):
class Missing(Bus):
def send(self,m,timeout=None):
if m.data==[0x43]: return
super().send(m,timeout)
with self.assertRaises(fist.MotionFault) as cm:
fist.read_all(Missing(),SimpleNamespace(Message=message))
self.assertEqual(cm.exception.details['can_property'],0x43)
self.assertEqual(len(cm.exception.details['partial_feedback']),2)
def test_every_active_table_point_round_trips_in_angle(self):
from g20_left_lookup import EXPECTED
for name in EXPECTED:
for raw in range(256):
angle=self.lookup.angle(name,raw)
recovered=self.lookup.command(name,angle,near=raw)
self.assertEqual(recovered,raw)
if __name__ == '__main__': unittest.main()