121 lines
5.8 KiB
Python
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)
|