42 lines
1.7 KiB
Python
42 lines
1.7 KiB
Python
"""Offline regression tests: no hardware imports or movement."""
|
|
import copy
|
|
import json
|
|
import unittest
|
|
from pathlib import Path
|
|
from hand_table_check import check_candidate
|
|
|
|
|
|
class HandTableTests(unittest.TestCase):
|
|
def setUp(self):
|
|
scene = json.loads((Path(__file__).resolve().parents[1] / 'scene/a7l20_scene.json').read_text())
|
|
names = [n.removeprefix('left_hand_') for n in scene['hands']['left']['active_joints']]
|
|
self.pose = dict(wrist_pos=[0, 0, 1], wrist_quat_wxyz=[1, 0, 0, 0],
|
|
approach=[0, 0, -1], hand_pre=dict.fromkeys(names, 0.),
|
|
hand_qpos=dict.fromkeys(names, 0.))
|
|
|
|
def test_clear_hand(self):
|
|
self.assertGreater(check_candidate(self.pose, 'left', 0)['closure_gap_m'], .005)
|
|
|
|
def test_table_collision(self):
|
|
self.pose['wrist_pos'][2] = -1
|
|
with self.assertRaises(RuntimeError):
|
|
check_candidate(self.pose, 'left', 0)
|
|
|
|
def test_approach_collision(self):
|
|
# End pose is clear but the start of the approach is below the table.
|
|
self.pose['approach'] = [0, 0, 1]
|
|
with self.assertRaises(RuntimeError):
|
|
check_candidate(self.pose, 'left', 0, clearance=2)
|
|
|
|
def test_invalid_inputs(self):
|
|
for field, value in [('approach', [0,0,0]), ('wrist_pos', [float('nan'),0,1]),
|
|
('hand_pre', {}), ('wrist_quat_wxyz', [0,0,0,0])]:
|
|
with self.subTest(field=field):
|
|
pose = copy.deepcopy(self.pose); pose[field] = value
|
|
with self.assertRaises((ValueError, RuntimeError)):
|
|
check_candidate(pose, 'left', 0)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|