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

121 lines
5.8 KiB
Python

"""Synthetic geometry/path tests. No hardware, camera, or controller connection."""
import unittest
import numpy as np
from scipy.spatial.transform import Rotation
from table_edge_constraint import (TableEdgeConstraint, TableEdgeViolation,
check_joint_linear_segment)
def table(**kwargs):
return TableEdgeConstraint([0, 0, 1], [0, 0, 0], [1, 0, 0], [0, 0, 0],
diagnostic=True, **kwargs)
class TableEdgeTests(unittest.TestCase):
def test_unverified_and_missing_edges_fail_closed(self):
with self.assertRaisesRegex(ValueError, 'Unverified'):
TableEdgeConstraint([0,0,1], [0,0,0], [1,0,0], [0,0,0])
with self.assertRaises(ValueError):
TableEdgeConstraint([0,0,1], [0,0,0], None, None, diagnostic=True)
t = TableEdgeConstraint([0,0,1], [0,0,0], [1,0,0], [0,0,0],
edge_verified=True)
self.assertFalse(t.diagnostic)
def test_above_table_passes(self):
self.assertAlmostEqual(table().hull_clearance([[1,0,.1],[2,1,.2]]), .1)
def test_below_table_outside_near_edge_passes(self):
self.assertAlmostEqual(table().hull_clearance([[-.2,0,-.5],[-.1,0,-.2]]), .1)
def test_inside_forbidden_volume_fails(self):
self.assertLess(table().hull_clearance([[.2,0,-.1],[.3,1,-.2]]), 0)
def test_vertexwise_or_is_not_used_for_crossing_triangle(self):
# Every vertex is outside, but midpoint of first two vertices is IN.
vertices = np.array([[-2,0,-2],[2,0,1],[2,1,1]], dtype=float)
vertexwise_incorrect = np.minimum.reduce(np.maximum(vertices[:,2], -vertices[:,0]))
self.assertGreater(vertexwise_incorrect, 0)
midpoint = .5*(vertices[0]+vertices[1])
self.assertTrue(midpoint[0] >= 0 and midpoint[2] <= 0)
self.assertLess(table().hull_clearance(vertices), 0)
def test_negative_is_not_proof_of_intersection(self):
# Entire triangle has z=x+2: x>=0 implies z>=2, hence no intersection.
vertices = [[-3,0,-1],[-1,0,1],[1,1,3]]
self.assertLess(table().hull_clearance(vertices), 0)
def test_sideways_far_and_below_regions_are_unbounded(self):
for point in ([2,100,-.1],[100,2,-.1],[2,2,-100]):
self.assertLess(table().hull_clearance([point]), 0)
def test_sphere_radius_and_corner_conservatism(self):
self.assertAlmostEqual(table().sphere_clearance([-.2,0,-.2],.05), .15)
self.assertAlmostEqual(table().sphere_clearance([.2,0,.2],.05), .15)
self.assertLess(table().sphere_clearance([-.01,0,-.2],.05), 0)
def test_uncertainty_expands_top_and_edge(self):
t = table(uncertainty_m=.03)
self.assertAlmostEqual(t.sphere_clearance([-.1,0,-1],.02), .05)
self.assertAlmostEqual(t.hull_clearance([[1,0,.1]]), .07)
def test_rigid_transform_invariance(self):
rotation = Rotation.from_euler('xyz',[.1,.15,.7]).as_matrix()
shift = np.array([.4,-.1,.2])
t = TableEdgeConstraint(rotation @ [0,0,1], shift,
rotation @ [1,0,0], shift, diagnostic=True)
points = np.array([[-.1,0,-.2],[-.2,1,-.1]])
self.assertAlmostEqual(t.hull_clearance(points@rotation.T+shift),
table().hull_clearance(points))
def test_geometry_validation(self):
cases = [([0,0,2],[0,0,0],[1,0,0],[0,0,0]),
([0,0,-1],[0,0,0],[1,0,0],[0,0,0]),
([0,0,1],[0,0,0],[0,0,1],[0,0,0]),
([0,0,1],[0,0,0],[1,0,0],[0,0,.1]),
([0,0,1],[np.nan,0,0],[1,0,0],[0,0,0])]
for params in cases:
with self.subTest(params=params), self.assertRaises(ValueError):
TableEdgeConstraint(*params, diagnostic=True)
for kwargs in ({'units':'mm'}, {'uncertainty_m':-.01}):
with self.subTest(kwargs=kwargs), self.assertRaises(ValueError):
table(**kwargs)
for vertices in ([], [1,2,3], [[1,2,np.nan]]):
with self.subTest(vertices=vertices), self.assertRaises(ValueError):
table().hull_clearance(vertices)
with self.assertRaises(ValueError):
table().sphere_clearance([0,0,0], -.1)
def test_interval_padding_subtracted(self):
result = check_joint_linear_segment([0],[.1], lambda q:(.05,'link'),
reach_bound_m=1., margin_m=.02,
max_sample_motion_m=.02)
self.assertEqual(result['samples'],6)
self.assertAlmostEqual(result['interval_bound_m'],.01)
self.assertAlmostEqual(result['certified_clearance_lower_bound_m'],.04)
def test_between_sample_motion_bound_rejects(self):
# Endpoints .03m clear, midpoint -.02m. A one-interval checker must reject
# before pretending the endpoints certify a collision-free path.
with self.assertRaises(TableEdgeViolation):
check_joint_linear_segment([0],[.1],
lambda q:abs(q[0]-.05)-.02, reach_bound_m=1.,
margin_m=.001, max_sample_motion_m=1.)
def test_starting_collision_and_nonfinite_reject(self):
with self.assertRaises(TableEdgeViolation) as caught:
check_joint_linear_segment([0],[1], lambda q:(q[0]-.01,'L5'),
reach_bound_m=1.)
self.assertEqual(caught.exception.report['fraction'],0.)
self.assertEqual(caught.exception.report['body'],'L5')
with self.assertRaises(ValueError):
check_joint_linear_segment([0],[1], lambda q:np.nan,reach_bound_m=1.)
with self.assertRaises(ValueError):
check_joint_linear_segment([0],[np.nan], lambda q:1.,reach_bound_m=1.)
with self.assertRaises(RuntimeError):
check_joint_linear_segment([0],[1],lambda q:1.,reach_bound_m=1.,max_steps=1)
if __name__ == '__main__':
unittest.main(verbosity=2)