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

36 lines
1.8 KiB
Python

import sys
import unittest
from pathlib import Path
import numpy as np
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'scripts'))
from elbow_raise_preflight import timing_preview,check_start
class PreflightTest(unittest.TestCase):
def samples(self):
return [dict(received=i*.08,state={'joint_state':dict(name=[f'L{j}_joint' for j in range(1,8)],position=[0.]*7,header={'stamp':{'sec':i}})}) for i in range(11)]
def test_timing(self):
r=timing_preview([[0.]*7,[.005]*7,[0.]*7])
self.assertLessEqual(r['maximum_speed_rad_s'],.03)
self.assertLessEqual(r['maximum_acceleration_rad_s2'],.050000001)
self.assertFalse(r['controller_interpolation_verified'])
def test_invalid_timing(self):
for q in ([[0.]*7,[.006]*7],[[0.]*7,[float('nan')]*7]):
with self.assertRaises(ValueError):timing_preview(q)
with self.assertRaises(ValueError):timing_preview([[0.]*7,[.001]*7],speed=.1)
def test_start_match(self):
self.assertEqual(check_start(self.samples(),[0.]*7,.85)['status'],'arm_start_matches_only')
def test_drift_stale_and_wrong_side(self):
with self.assertRaises(ValueError):check_start(self.samples(),[.01]*7,.85)
with self.assertRaises(ValueError):check_start(self.samples(),[0.]*7,2)
s=self.samples();s[-1]['state']['joint_state']['name'][0]='R1_joint'
with self.assertRaises(ValueError):check_start(s,[0.]*7,.85)
def test_motion_and_frozen_timestamp(self):
s=self.samples();s[-1]['state']['joint_state']['position'][0]=.003
with self.assertRaises(ValueError):check_start(s,[0.]*7,.85)
for row in s:row['state']['joint_state']['header']['stamp']={'sec':0}
with self.assertRaises(ValueError):check_start(s,[0.]*7,.85)
if __name__=='__main__':unittest.main()