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

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)