Files
a7l20-hug-sim/scripts/test_hand_table_check.py

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()