322 lines
16 KiB
Python
322 lines
16 KiB
Python
"""Independent synthetic tests for offline table-contact escape.
|
|
|
|
No controller, camera, ROS or CAN imports. Affine examples have zero curvature;
|
|
nonlinear adversarial examples provide a reach bound covering both their first
|
|
derivatives and second directional derivatives over the tested joint domain.
|
|
These are mathematical test geometries, not robot motion proposals.
|
|
"""
|
|
|
|
import unittest
|
|
|
|
import numpy as np
|
|
|
|
from table_contact_escape import (ContactEscapeChecker, EscapePolicy,
|
|
EscapeRejected, plan_escape)
|
|
from table_edge_constraint import TableEdgeConstraint, TableEdgeViolation
|
|
|
|
|
|
def table(uncertainty=0.):
|
|
return TableEdgeConstraint([0, 0, 1], [0, 0, 0],
|
|
[1, 0, 0], [0, 0, 0],
|
|
diagnostic=True, uncertainty_m=uncertainty)
|
|
|
|
|
|
class FunctionGeometry:
|
|
def __init__(self, callback, *, names=('hand',), nq=1, reach=1.,
|
|
lo=None, hi=None):
|
|
self.callback = callback
|
|
self.body_names = tuple(names)
|
|
self.lo = np.full(nq, -1.) if lo is None else np.array(lo, dtype=float)
|
|
self.hi = np.full(nq, 1.) if hi is None else np.array(hi, dtype=float)
|
|
self.reach = reach
|
|
|
|
def sample(self, q):
|
|
return self.callback(np.asarray(q, dtype=float))
|
|
|
|
|
|
def affine(bodies, *, nq=1, reach=1.):
|
|
"""Map body name to (constant Nx3 points, constant Nx3xnq Jacobian)."""
|
|
prepared = {name: (np.asarray(p, float), np.asarray(j, float))
|
|
for name, (p, j) in bodies.items()}
|
|
|
|
def sample(q):
|
|
return {name: {'points': p + np.einsum('nij,j->ni', j, q),
|
|
'jacobian': j.copy()}
|
|
for name, (p, j) in prepared.items()}
|
|
|
|
return FunctionGeometry(sample, names=prepared, nq=nq, reach=reach)
|
|
|
|
|
|
def point(z=0., *, dz=1., x=.5, y=0.):
|
|
return ([[x, y, z]], [[[0.], [0.], [dz]]])
|
|
|
|
|
|
class ContactEscapeTests(unittest.TestCase):
|
|
def checker(self, geometry=None, **kwargs):
|
|
return ContactEscapeChecker(
|
|
geometry if geometry is not None else affine({'hand': point()}),
|
|
table(), **kwargs)
|
|
|
|
def test_zero_contact_outward_segment_passes(self):
|
|
checker = self.checker()
|
|
initial = checker.bind([0.])
|
|
self.assertEqual(initial['status'], 'escape_required')
|
|
self.assertEqual(initial['minimum_clearance_m'], 0.)
|
|
report = checker.check_segment([0.], [.01])
|
|
self.assertEqual(report['status'], 'continuous_table_segment_pass')
|
|
self.assertGreaterEqual(report['minimum_certified_bound_m'], 0.)
|
|
self.assertAlmostEqual(report['endpoint_body_gaps_m']['hand'], .01)
|
|
|
|
def test_zero_contact_downward_rejected(self):
|
|
with self.assertRaises(EscapeRejected):
|
|
self.checker().check_segment([0.], [-.001])
|
|
|
|
def test_negative_initial_geometry_never_waived_as_contact(self):
|
|
checker = self.checker(affine({'hand': point(-.001)}))
|
|
initial = checker.bind([0.])
|
|
self.assertEqual(initial['status'], 'unresolved_initial_geometry')
|
|
self.assertTrue(initial['negative_bound_is_not_proof_of_physical_collision'])
|
|
with self.assertRaisesRegex(EscapeRejected, 'unresolved_initial_geometry'):
|
|
checker.check_segment([0.], [.01])
|
|
report = plan_escape(checker, [0.])
|
|
self.assertEqual(report['status'], 'unresolved_initial_geometry')
|
|
self.assertFalse(report['executable'])
|
|
|
|
def test_normally_clear_body_must_keep_twenty_mm(self):
|
|
checker = self.checker(affine({'elbow': point(.025, dz=-1.)}))
|
|
self.assertEqual(checker.bind([0.])['status'], 'normal_clearance')
|
|
with self.assertRaisesRegex(EscapeRejected, 'normal_margin_lost'):
|
|
checker.check_segment([0.], [.01])
|
|
|
|
def test_hand_rises_but_near_table_elbow_descends_rejected(self):
|
|
checker = self.checker(affine({
|
|
'hand': point(), 'elbow': point(.01, dz=-1.)}))
|
|
with self.assertRaises(EscapeRejected) as caught:
|
|
checker.check_segment([0.], [.005])
|
|
self.assertEqual(caught.exception.report['body'], 'elbow')
|
|
|
|
def test_two_dof_lift_must_check_every_body(self):
|
|
hand = ([[.5, 0., 0.]], [[[0., 0.], [0., 0.], [1., 0.]]])
|
|
elbow = ([[.2, 0., .005]], [[[0., 0.], [0., 0.], [0., 1.]]])
|
|
checker = self.checker(affine({'hand': hand, 'elbow': elbow}, nq=2))
|
|
with self.assertRaises(EscapeRejected) as caught:
|
|
checker.check_segment([0., 0.], [.005, -.002])
|
|
self.assertEqual(caught.exception.report['body'], 'elbow')
|
|
valid = checker.check_segment([0., 0.], [.005, .005])
|
|
self.assertEqual(valid['status'], 'continuous_table_segment_pass')
|
|
|
|
def test_clear_endpoints_cannot_hide_midpoint_penetration(self):
|
|
# z(0)=z(.01)=.03, z(.005)=-.0325. z''=5000, |z'|<=75
|
|
# on [-.01,.02]. A single interval must use its curvature bound.
|
|
def sample(q):
|
|
x = q[0]
|
|
return {'hand': {'points': np.array([[.5, 0., .03-2500*x*(.01-x)]]),
|
|
'jacobian': np.array([[[0.], [0.], [-25.+5000*x]]])}}
|
|
geometry = FunctionGeometry(sample, reach=5000., lo=[-.01], hi=[.02])
|
|
checker = self.checker(geometry, policy=EscapePolicy(max_sample_motion_m=100.))
|
|
self.assertGreater(geometry.sample(np.array([0.]))['hand']['points'][0, 2], .02)
|
|
self.assertGreater(geometry.sample(np.array([.01]))['hand']['points'][0, 2], .02)
|
|
self.assertLess(geometry.sample(np.array([.005]))['hand']['points'][0, 2], 0.)
|
|
with self.assertRaisesRegex(EscapeRejected, 'normal_margin_lost'):
|
|
checker.check_segment([0.], [.01])
|
|
|
|
def test_observed_span_midpoint_exit_rejected_with_safe_endpoints(self):
|
|
# y(0)=y(.01)=0 but y(.005)=.02 exceeds the observed .015.
|
|
# |y''|=1600, |y'|<=24 on the stated domain. Far outside the
|
|
# near edge keeps x separation safe despite the curvature padding.
|
|
def sample(q):
|
|
x = q[0]
|
|
return {'hand': {'points': np.array([[-1., 800*x*(.01-x), -.1]]),
|
|
'jacobian': np.array([[[0.], [8.-1600*x], [0.]]])}}
|
|
geometry = FunctionGeometry(sample, reach=1600., lo=[-.01], hi=[.02])
|
|
checker = self.checker(geometry, observed_y=[-.015, .015],
|
|
policy=EscapePolicy(max_sample_motion_m=20.))
|
|
self.assertEqual(checker.bind([0.])['bodies'][0]['plane'], 'near_edge')
|
|
with self.assertRaisesRegex(EscapeRejected, 'path_leaves_observed_edge_span'):
|
|
checker.check_segment([0.], [.01])
|
|
|
|
def test_below_top_outside_edge_requires_observed_span(self):
|
|
geometry = affine({'hand': point(-.05, x=-.03, dz=0.)})
|
|
self.assertEqual(self.checker(geometry).bind([0.])['status'],
|
|
'unresolved_initial_geometry')
|
|
report = self.checker(geometry, observed_y=[-.1, .1]).bind([0.])
|
|
self.assertEqual(report['status'], 'normal_clearance')
|
|
self.assertEqual(report['bodies'][0]['plane'], 'near_edge')
|
|
|
|
def test_convex_hull_crossing_corner_not_vertexwise_or(self):
|
|
points = np.array([[-.03, 0., -.03], [.03, 0., .01], [.03, .01, .01]])
|
|
self.assertTrue(np.all(np.maximum(points[:, 2], -points[:, 0]) > 0.))
|
|
midpoint = .5*(points[0]+points[1])
|
|
self.assertGreaterEqual(midpoint[0], 0.)
|
|
self.assertLess(midpoint[2], 0.)
|
|
geometry = affine({'hand': (points, np.zeros((3, 3, 1)))})
|
|
checker = self.checker(geometry, observed_y=[-.1, .1])
|
|
self.assertEqual(checker.bind([0.])['status'], 'unresolved_initial_geometry')
|
|
with self.assertRaisesRegex(EscapeRejected, 'unresolved_initial_geometry'):
|
|
checker.check_path([[0.], [.001]])
|
|
|
|
def test_cross_segment_near_contact_backtracking_rejected(self):
|
|
checker = self.checker()
|
|
checker.check_segment([0.], [.009])
|
|
with self.assertRaises(EscapeRejected):
|
|
checker.check_segment([.009], [.008])
|
|
with self.assertRaises(EscapeRejected):
|
|
self.checker().check_path([[0.], [.009], [.008], [.016], [.024]])
|
|
|
|
def test_path_without_normal_margin_restoration_rejected(self):
|
|
with self.assertRaisesRegex(EscapeRejected, 'normal_margin_not_restored'):
|
|
self.checker().check_path([[0.], [.005], [.01]])
|
|
with self.assertRaisesRegex(EscapeRejected, 'normal_margin_not_restored'):
|
|
self.checker().check_path([[0.]])
|
|
|
|
def test_complete_contact_escape_path_is_offline_only(self):
|
|
result = self.checker().check_path([[0.], [.009], [.018], [.025]])
|
|
self.assertEqual(result['status'], 'escape_path_table_verified_offline')
|
|
self.assertGreaterEqual(result['endpoint_body_gaps_m']['hand'], .02)
|
|
self.assertFalse(result['executable'])
|
|
self.assertFalse(result['motion_sent'])
|
|
self.assertEqual(result['next_phase'], 'normal_motion_requires_separate_path_validation')
|
|
|
|
def test_uncertainty_is_not_a_penetration_allowance(self):
|
|
uncertain = table(.005)
|
|
checker = ContactEscapeChecker(affine({'hand': point(.005)}), uncertain)
|
|
self.assertEqual(checker.bind([0.])['minimum_clearance_m'], 0.)
|
|
checker.check_segment([0.], [.005])
|
|
below = ContactEscapeChecker(affine({'hand': point()}), uncertain)
|
|
self.assertEqual(below.bind([0.])['status'], 'unresolved_initial_geometry')
|
|
|
|
def test_nonfinite_wrong_size_and_limits_rejected(self):
|
|
for q in ([np.nan], [np.inf], [], [0., 0.], [1.01], [-1.01]):
|
|
with self.subTest(q=q), self.assertRaises(ValueError):
|
|
self.checker().bind(q)
|
|
for end in ([np.nan], [0., 0.], [1.01]):
|
|
with self.subTest(end=end), self.assertRaises(ValueError):
|
|
self.checker().check_segment([0.], end)
|
|
for path in ([], [0., .01], [[0.], [np.nan]], [[0., 0.]]):
|
|
with self.subTest(path=path), self.assertRaises(ValueError):
|
|
self.checker().check_path(path)
|
|
|
|
def test_invalid_geometry_and_nonfinite_jacobian_rejected(self):
|
|
invalid = [
|
|
{'points': np.array([.5, 0., 0.]), 'jacobian': np.zeros((1, 3, 1))},
|
|
{'points': np.zeros((0, 3)), 'jacobian': np.zeros((0, 3, 1))},
|
|
{'points': np.array([[.5, 0., np.nan]]), 'jacobian': np.zeros((1, 3, 1))},
|
|
{'points': np.array([[.5, 0., 0.]]), 'jacobian': np.zeros((1, 2, 1))},
|
|
{'points': np.array([[.5, 0., 0.]]), 'jacobian': np.full((1, 3, 1), np.nan)},
|
|
]
|
|
for row in invalid:
|
|
with self.subTest(row=row), self.assertRaises(ValueError):
|
|
self.checker(FunctionGeometry(lambda q: {'hand': row})).bind([0.])
|
|
for bodies in ({}, {'other': {'points': np.zeros((1, 3)),
|
|
'jacobian': np.zeros((1, 3, 1))}}):
|
|
with self.subTest(bodies=bodies), self.assertRaises(ValueError):
|
|
self.checker(FunctionGeometry(lambda q: bodies)).bind([0.])
|
|
|
|
def test_geometry_shape_changes_fail_closed(self):
|
|
def sample(q):
|
|
n = 1 if q[0] == 0. else 2
|
|
return {'hand': {'points': np.tile([.5, 0., .03], (n, 1)),
|
|
'jacobian': np.zeros((n, 3, 1))}}
|
|
checker = self.checker(FunctionGeometry(sample))
|
|
checker.bind([0.])
|
|
with self.assertRaisesRegex(ValueError, 'Geometry changed'):
|
|
checker.check_segment([0.], [.001])
|
|
|
|
def test_invalid_policy_and_bounds(self):
|
|
for kwargs in ({'margin_m': .005}, {'margin_m': np.nan},
|
|
{'max_joint_step_rad': .03}, {'speed_rad_s': .06},
|
|
{'acceleration_rad_s2': .11}, {'max_steps': True},
|
|
{'max_interval_checks': 0}, {'max_sample_motion_m': 0.}):
|
|
with self.subTest(kwargs=kwargs), self.assertRaises(ValueError):
|
|
EscapePolicy(**kwargs)
|
|
for attr, value in (('lo', [np.nan]), ('hi', [-1.]), ('reach', 0.),
|
|
('reach', np.inf), ('hi', [1., 2.])):
|
|
geometry = affine({'hand': point()})
|
|
setattr(geometry, attr, value)
|
|
with self.subTest(attr=attr, value=value), self.assertRaises(ValueError):
|
|
self.checker(geometry)
|
|
|
|
def test_large_step_and_path_budget_are_not_bypassed(self):
|
|
with self.assertRaisesRegex(EscapeRejected, 'escape_step_too_large'):
|
|
self.checker().check_segment([0.], [.011])
|
|
checker = self.checker(policy=EscapePolicy(max_steps=2))
|
|
with self.assertRaisesRegex(EscapeRejected, 'escape_path_budget_exceeded'):
|
|
checker.check_path([[0.], [.009], [.018], [.025]])
|
|
|
|
def test_plan_from_contact_restores_clearance_and_never_authorizes_motion(self):
|
|
checker = self.checker()
|
|
report = plan_escape(checker, [0.])
|
|
self.assertEqual(report['status'], 'escape_found_offline')
|
|
for key in ('executable', 'hardware_connected', 'motion_sent'):
|
|
self.assertFalse(report[key])
|
|
self.assertGreaterEqual(min(report['verification']['endpoint_body_gaps_m'].values()), .02)
|
|
waypoints = np.array(report['waypoints_rad'])
|
|
self.assertTrue(np.all(np.diff(waypoints[:, 0]) > 0.))
|
|
self.assertLessEqual(np.max(np.abs(np.diff(waypoints, axis=0))), .01 + 1e-10)
|
|
self.assertLessEqual(report['speed_ceiling_rad_s'], .05)
|
|
self.assertLessEqual(report['acceleration_ceiling_rad_s2'], .1)
|
|
self.assertEqual(checker.check_path(waypoints)['status'], 'escape_path_table_verified_offline')
|
|
|
|
def test_no_outward_direction_is_not_global_unreachability(self):
|
|
checker = self.checker(affine({'hand': point(), 'elbow': point(dz=-1.)}))
|
|
report = plan_escape(checker, [0.])
|
|
self.assertEqual(report['status'], 'no_certified_escape_found')
|
|
self.assertIn('not global unreachability', report['reason'])
|
|
self.assertTrue(report['partial_waypoints_are_not_a_motion_plan'])
|
|
self.assertFalse(report['executable'])
|
|
self.assertEqual(report['waypoints_rad'], [[0.]])
|
|
|
|
def test_already_clear_plan_is_still_offline_only(self):
|
|
checker = self.checker(affine({'hand': point(.03)}))
|
|
report = plan_escape(checker, [0.])
|
|
self.assertEqual(report['status'], 'normal_clearance')
|
|
self.assertEqual(report['steps'], 0)
|
|
self.assertFalse(report['executable'])
|
|
|
|
def test_normal_phase_passes_after_contact_escape_restores_margin(self):
|
|
checker = self.checker()
|
|
escape = checker.check_path([[0.], [.009], [.018], [.025]])
|
|
self.assertGreaterEqual(escape['endpoint_body_gaps_m']['hand'], .02)
|
|
gap, body = checker.normal_clearance([.025])
|
|
self.assertEqual(body, 'hand')
|
|
self.assertAlmostEqual(gap, .025)
|
|
normal = checker.check_normal_segment([.025], [.04])
|
|
self.assertGreaterEqual(normal['certified_clearance_lower_bound_m'], .02)
|
|
self.assertEqual(normal['interpolation'], 'joint_linear_offline_only')
|
|
|
|
def test_normal_phase_does_not_reuse_contact_exemption(self):
|
|
checker = self.checker()
|
|
checker.check_path([[0.], [.009], [.018], [.025]])
|
|
with self.assertRaises(TableEdgeViolation):
|
|
checker.check_normal_segment([.025], [.015])
|
|
# Normal checking must also reject a renewed zero-contact start.
|
|
with self.assertRaises(TableEdgeViolation):
|
|
checker.check_normal_segment([0.], [.005])
|
|
|
|
def test_normal_near_edge_branch_continuously_limits_unknown_span(self):
|
|
geometry = affine({'hand': (
|
|
[[-.1, 0., -.02]], [[[0.], [1.], [0.]]])})
|
|
checker = self.checker(geometry, observed_y=[-.1, .1])
|
|
self.assertAlmostEqual(checker.normal_clearance([0.])[0], .1)
|
|
checker.check_normal_segment([0.], [.05])
|
|
# Staying outside the near edge does not license travelling below
|
|
# the table near an unobserved side: y-span clearance is also required.
|
|
self.assertAlmostEqual(checker.normal_clearance([.09])[0], .01)
|
|
with self.assertRaises(TableEdgeViolation):
|
|
checker.check_normal_segment([0.], [.09])
|
|
epsilon = 1e-6
|
|
left = checker.normal_clearance([.1-epsilon])[0]
|
|
right = checker.normal_clearance([.1+epsilon])[0]
|
|
self.assertAlmostEqual(left, epsilon)
|
|
self.assertAlmostEqual(right, -epsilon)
|
|
self.assertLessEqual(abs(right-left), 2*epsilon+1e-12)
|
|
# Without any observed edge span, the top-only fallback rejects it.
|
|
unobserved = self.checker(geometry)
|
|
self.assertAlmostEqual(unobserved.normal_clearance([0.])[0], -.02)
|
|
with self.assertRaises(TableEdgeViolation):
|
|
unobserved.check_normal_segment([0.], [.005])
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main(verbosity=2)
|