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

73 lines
3.8 KiB
Python

import sys
import tempfile
import unittest
from pathlib import Path
from unittest.mock import Mock
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'scripts'))
from trajectory_supervisor import Trajectory,Supervisor
from joint_follow_adapter import ReferenceGate,SdkFollowBackend,ControlLease,native_state_observation,bind_sdk
class AdapterTest(unittest.TestCase):
def setUp(self):self.t=Trajectory([[0.]*7,[.005]*7])
def test_reference_stream(self):
gate=ReferenceGate(self.t)
for t in (0,.02,.04):self.assertEqual(len(gate.accept(t,self.t.sample(t)['position'])),7)
def test_bad_packet_latches(self):
gate=ReferenceGate(self.t)
with self.assertRaises(ValueError):gate.accept(0,[float('nan')]*7)
with self.assertRaises(ValueError):gate.accept(0,[0.]*7)
def test_jump_and_unplanned_reference(self):
for time,q in [(.1,self.t.sample(.1)['position']),(.02,[.1]*7)]:
gate=ReferenceGate(self.t);gate.accept(0,[0.]*7)
with self.assertRaises(ValueError):gate.accept(time,q)
def test_no_live_connection(self):
backend=SdkFollowBackend()
with self.assertRaises(RuntimeError):backend.connect_live()
with self.assertRaises(RuntimeError):backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
self.assertFalse(backend.request_controlled_stop()['stopped'])
def test_mock_sdk_abi_and_left_side(self):
fake=Mock();fake.lbot_joint_follow.return_value=True;bind_sdk(fake)
backend=SdkFollowBackend();backend.library=fake;backend.handle=123
r=backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
self.assertFalse(r['arrival_confirmed'])
self.assertEqual(fake.lbot_joint_follow.call_args.args[1],0)
self.assertEqual(list(fake.lbot_joint_follow.call_args.args[2]),[0.]*7)
fake.lbot_init.assert_not_called()
def test_send_failure_no_retry(self):
backend=SdkFollowBackend();backend.handle=123;backend.library=Mock()
backend.library.lbot_joint_follow.return_value=False
for _ in range(2):
with self.assertRaises(RuntimeError):backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
self.assertEqual(backend.library.lbot_joint_follow.call_count,1)
def test_lease(self):
with tempfile.TemporaryDirectory() as d:
path=Path(d)/'lock'
with ControlLease(path):
with self.assertRaises(RuntimeError):
with ControlLease(path):pass
with ControlLease(path):pass
def test_state_mapping(self):
message={'joint_state':dict(name=[f'L{i}_joint' for i in range(1,8)],position=[0.]*7,velocity=[0.]*7,header={'stamp':{'sec':1}})}
self.assertEqual(native_state_observation(message,1.,1)['seq'],1)
message['joint_state']['name'][0]='R1_joint'
with self.assertRaises(ValueError):native_state_observation(message,1.,1)
def test_supervisor_to_mock_follow_end_to_end(self):
backend=SdkFollowBackend();backend.handle=123;backend.library=Mock()
backend.library.lbot_joint_follow.return_value=True
gate=ReferenceGate(self.t);supervisor=Supervisor(self.t)
for step in range(100):
now=step*.02;ref=self.t.sample(max(0,now-.02))
observation=dict(seq=step,received=now,position=ref['position'],velocity=ref['velocity'])
actions=supervisor.tick(now,observation,acknowledgement=True if step==1 else None)
for action in actions:
if action['action']=='reference_only' and action['elapsed_s']<=self.t.duration:
backend.send_checked(gate,action['elapsed_s'],action['position'])
self.assertEqual(supervisor.state,'DONE')
self.assertGreater(backend.library.lbot_joint_follow.call_count,10)
backend.library.lbot_init.assert_not_called()
if __name__=='__main__':unittest.main()