36 lines
1.8 KiB
Python
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()
|