330 lines
18 KiB
Python
330 lines
18 KiB
Python
"""Simulated native motion/observation adapters only; never open a display."""
|
|
import copy
|
|
import math
|
|
from pathlib import Path
|
|
import sys
|
|
import unittest
|
|
|
|
sys.path.insert(0, str(Path(__file__).resolve().parents[1]/'tools'))
|
|
import prison_approach_proof as policy
|
|
|
|
|
|
class Busy(ValueError): pass
|
|
|
|
|
|
class Scene:
|
|
def __init__(self):
|
|
self.now=2.5; self.next_sample=2.6
|
|
self.geometry={'nonce':'test', 'basis':'registered-prison-lock-native-interaction',
|
|
'prison':[100.,0.,100.], 'gate':[100.,0.,94.], 'gate_forward':[0.,0.,1.],
|
|
'context':[102.7,1.2,94.], 'radius':2.}
|
|
self.position=[98.5,0.,98.5];self.angle=0.;self.speed=0.;self.held=False
|
|
self.key_events=[];self.turns=[];self.notifications=[];self.focus=True
|
|
self.gain=.085;self.acceleration=8.;self.braking=8.;self.move_scale=1.
|
|
self.guard_cost=.22;self.ignore_turn=False;self.wrong_turn=False
|
|
self.turn_delay=0.;self.pending_turns=[]
|
|
self.stale=False;self.no_movement=False;self.changed_geometry=False
|
|
self.context_lost=False;self.fail_down=False;self.coast_events=[]
|
|
self.sample=self.snapshot();self.history=[copy.deepcopy(self.sample)];self.last_read=None
|
|
def clock(self):return self.now
|
|
def snapshot(self):
|
|
r=math.radians(self.angle)
|
|
geometry=copy.deepcopy(self.geometry)
|
|
if self.changed_geometry:geometry['context'][0]+=.1
|
|
return {'sample':int(round((2000+self.now*1000)/100)), 'real_ms':int(round(2000+self.now*1000)),
|
|
'received_monotonic':self.now-1 if self.stale else self.now,
|
|
'origin':list(self.position), 'direction':[math.sin(r),0.,math.cos(r)],
|
|
'body_forward':[math.sin(r),0.,math.cos(r)], 'input':[0.,0.,1. if self.held else 0.],
|
|
'forward':1. if self.held else 0.,'velocity':[self.speed*math.sin(r),0.,self.speed*math.cos(r)],
|
|
'inventory':False,'phase':2 if self.context_lost else 1,'door_state':0.,'door_control':0.,'geometry':geometry}
|
|
def sleep(self,seconds):
|
|
end=self.now+seconds
|
|
while self.now < end-1e-9:
|
|
step=min(.005,end-self.now)
|
|
target=3. if self.held and not self.no_movement else 0.
|
|
rate=self.acceleration if target>self.speed else self.braking
|
|
self.speed=max(target,self.speed-rate*step) if target<self.speed else min(target,self.speed+rate*step)
|
|
r=math.radians(self.angle)
|
|
self.position[0]+=self.move_scale*self.speed*math.sin(r)*step
|
|
self.position[2]+=self.move_scale*self.speed*math.cos(r)*step
|
|
self.now+=step
|
|
while self.pending_turns and self.pending_turns[0][0] <= self.now:
|
|
self.angle+=self.pending_turns.pop(0)[1]
|
|
if self.now >= self.next_sample-1e-8:
|
|
self.sample=self.snapshot();self.history.append(copy.deepcopy(self.sample));self.next_sample+=.1
|
|
def check(self):
|
|
if not self.focus:raise policy.Refused('focus lost')
|
|
self.sleep(self.guard_cost)
|
|
def require_idle(self):
|
|
if self.held:raise Busy('key held')
|
|
def send(self,key,down):
|
|
if key != 'W':raise AssertionError('Only W permitted')
|
|
if down and self.speed>.05:self.coast_events.append(self.speed)
|
|
self.key_events.append((key,down,self.now))
|
|
self.held=down
|
|
if down and self.fail_down:raise OSError('uncertain native press')
|
|
def move(self,dx,dy,guard):
|
|
if dy != 0 or not 1<=abs(dx)<=128:raise AssertionError('motion cap')
|
|
guard.check();guard.check()
|
|
self.turns.append((dx,dy,self.now))
|
|
if not self.ignore_turn:
|
|
change=dx*self.gain*(-1 if self.wrong_turn else 1)
|
|
if self.turn_delay:self.pending_turns.append((self.now+self.turn_delay,change))
|
|
else:self.angle+=change
|
|
def observe(self):
|
|
if self.last_read is None:
|
|
batch=[self.sample]
|
|
else:
|
|
batch=[s for s in self.history if s['sample'] > self.last_read] or [self.sample]
|
|
self.last_read=batch[-1]['sample']
|
|
return copy.deepcopy(batch)
|
|
def run(self,report=None,calibration=None):
|
|
self.report={} if report is None else report
|
|
angle=math.radians(24*.085)
|
|
calibration=calibration or {'nonce':'test','completed':True,'pixels':24,
|
|
'baseline_direction':[0.,0.,1.], 'response_direction':[math.sin(angle),0.,math.cos(angle)],
|
|
'ready_real_ms':2000,'ready_monotonic':0.,'observer_start_real_ms':0}
|
|
return policy.perform_approach(self,self,self,self.observe,self.geometry,calibration,self.report,
|
|
clock=self.clock,sleep=self.sleep,notify=self.notifications.append,key_state_busy=Busy)
|
|
|
|
|
|
class ApproachTests(unittest.TestCase):
|
|
def test_measured_heading_and_coasting_route_stops_inside_without_interaction(self):
|
|
scene=Scene();result=scene.run()
|
|
self.assertTrue(result['completed'])
|
|
target,_=policy.target_for(scene.geometry)
|
|
self.assertLessEqual(policy.horizontal(scene.position,target),.75)
|
|
self.assertLessEqual(scene.speed,.05);self.assertFalse(scene.held)
|
|
self.assertFalse(scene.coast_events)
|
|
self.assertLess(scene.now,35);self.assertLessEqual(len(scene.turns),16)
|
|
self.assertLessEqual(len(scene.key_events)//2,12)
|
|
self.assertTrue(all(k=='W' for k,_,_ in scene.key_events))
|
|
self.assertLessEqual(result['cumulative_horizontal_m'],6)
|
|
self.assertTrue(all(policy.inside(s['origin'],scene.geometry['prison']) for s in result['samples']))
|
|
def test_no_turn_or_opposite_turn_never_authorizes_w(self):
|
|
for attribute in ('ignore_turn','wrong_turn'):
|
|
scene=Scene();setattr(scene,attribute,True)
|
|
with self.subTest(attribute=attribute),self.assertRaisesRegex(policy.Refused,'heading failed|opposite|settling'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
def test_excessive_observed_turn_refuses_despite_bounded_pixels(self):
|
|
scene=Scene();scene.gain=.5
|
|
with self.assertRaisesRegex(policy.Refused,'angle bound'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
def test_no_forward_movement_does_not_treat_neutral_samples_as_success(self):
|
|
scene=Scene();scene.no_movement=True
|
|
with self.assertRaisesRegex(policy.Refused,'full-stop'):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True,False])
|
|
self.assertFalse(scene.held)
|
|
def test_long_coast_never_starts_second_press(self):
|
|
scene=Scene();scene.braking=.1
|
|
with self.assertRaises(policy.Refused):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True,False]);self.assertFalse(scene.coast_events)
|
|
def test_large_forward_displacement_fails_without_a_second_pulse(self):
|
|
scene=Scene();scene.move_scale=5
|
|
with self.assertRaises(policy.Refused):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True,False])
|
|
def test_stale_geometry_context_or_input_refuses_before_events(self):
|
|
for attribute in ('stale','changed_geometry','context_lost'):
|
|
scene=Scene();setattr(scene,attribute,True);scene.sleep(.11)
|
|
with self.subTest(attribute=attribute),self.assertRaises(policy.Refused):scene.run()
|
|
self.assertEqual(scene.key_events,[]);self.assertEqual(scene.turns,[])
|
|
def test_uncertain_press_is_paired_only_on_owned_focus(self):
|
|
scene=Scene();scene.fail_down=True
|
|
with self.assertRaisesRegex(OSError,'uncertain'):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True,False]);self.assertFalse(scene.held)
|
|
scene=Scene();original=scene.send
|
|
def lose(key,down):
|
|
original(key,down)
|
|
if down:scene.focus=False
|
|
scene.send=lose
|
|
with self.assertRaisesRegex(policy.Refused,'focus lost'):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True])
|
|
self.assertEqual(scene.report['approach_proof']['unconfirmed_held_key'],'W')
|
|
self.assertTrue(scene.report['approach_proof']['paired_release_errors'])
|
|
def test_expired_ready_has_no_extended_budget_or_inputs(self):
|
|
scene=Scene();scene.now=35
|
|
with self.assertRaisesRegex(policy.Refused,'Insufficient'):scene.run()
|
|
self.assertEqual(scene.key_events,[]);self.assertEqual(scene.turns,[])
|
|
def test_slow_guard_actual_hold_cap_aborts_after_release(self):
|
|
scene=Scene();original=scene.send
|
|
def slow(key,down):
|
|
original(key,down)
|
|
if down:scene.guard_cost=.41
|
|
scene.send=slow
|
|
with self.assertRaisesRegex(policy.Refused,'400ms'):scene.run()
|
|
self.assertFalse(scene.held);self.assertEqual([v[1] for v in scene.key_events],[True,False])
|
|
def test_geometry_is_native_relative_and_closed_scene_specific(self):
|
|
geometry=copy.deepcopy(Scene().geometry);target,_=policy.target_for(geometry)
|
|
self.assertEqual(target,(102.7,0.,95.5))
|
|
for k in ('prison','gate','context'):
|
|
geometry[k][0]+=500;geometry[k][2]-=300
|
|
translated,_=policy.target_for(geometry)
|
|
self.assertEqual(translated,(602.7,0.,-204.5))
|
|
geometry['gate'][0]+=1
|
|
with self.assertRaisesRegex(policy.Refused,'placement'):policy.target_for(geometry)
|
|
def test_delayed_native_rotation_is_not_confused_with_unchanged_settling(self):
|
|
scene=Scene();scene.turn_delay=.4;scene.angle=110.
|
|
scene.sample=scene.snapshot();scene.history=[copy.deepcopy(scene.sample)]
|
|
# A shorter initial heading isolates delayed native turn acknowledgement.
|
|
scene.position=[100.,0.,97.5]
|
|
scene.sample=scene.snapshot();scene.history=[copy.deepcopy(scene.sample)]
|
|
result=scene.run()
|
|
self.assertTrue(result['completed'])
|
|
self.assertTrue(all(m['response_sample'] > m['after_sample'] for m in result['motions']))
|
|
self.assertLess(scene.now,35)
|
|
|
|
def test_adapter_must_preserve_intermediate_samples_during_slow_guards(self):
|
|
scene=Scene();original=scene.observe
|
|
scene.observe=lambda:original()[-1:]
|
|
with self.assertRaisesRegex(policy.Refused,'sampling gap'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
|
|
def test_sample_tampering_or_gap_is_refused(self):
|
|
scene=Scene();original=scene.observe;calls=[0]
|
|
def skip():
|
|
records=original();calls[0]+=1
|
|
if calls[0]>1 and len(records)>1:return records[1:]
|
|
return records
|
|
scene.observe=skip
|
|
with self.assertRaisesRegex(policy.Refused,'sampling gap'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
|
|
def test_body_motion_during_heading_refuses_without_w(self):
|
|
scene=Scene();original=scene.move
|
|
def turn(dx,dy,guard):
|
|
original(dx,dy,guard);scene.position[0]+=.1
|
|
scene.move=turn
|
|
with self.assertRaisesRegex(policy.Refused,'translated'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
|
|
def test_transient_overshoot_in_batch_is_not_hidden_by_later_good_angle(self):
|
|
scene=Scene()
|
|
def turn(dx,dy,guard):
|
|
guard.check();guard.check()
|
|
scene.angle+=20;scene.sleep(.2);scene.angle-=10
|
|
scene.move=turn
|
|
with self.assertRaisesRegex(policy.Refused,'angle bound'):scene.run()
|
|
self.assertEqual(scene.key_events,[])
|
|
|
|
def test_missed_sampled_forward_is_missing_evidence_not_success(self):
|
|
scene=Scene();original=scene.observe
|
|
def hide():
|
|
records=original()
|
|
for s in records:s['forward']=0.;s['input']=[0.,0.,0.]
|
|
return records
|
|
scene.observe=hide
|
|
with self.assertRaisesRegex(policy.Refused,'W acknowledgement'):scene.run()
|
|
self.assertEqual([v[1] for v in scene.key_events],[True,False])
|
|
self.assertFalse(scene.held)
|
|
|
|
def test_second_attempt_cannot_reuse_the_same_report(self):
|
|
scene=Scene();report={'approach_proof':{'completed':False}}
|
|
with self.assertRaisesRegex(policy.Refused,'already exists'):scene.run(report=report)
|
|
self.assertEqual(scene.key_events,[]);self.assertEqual(scene.turns,[])
|
|
|
|
def test_calibration_must_be_same_run_and_nonzero_native_yaw(self):
|
|
scene=Scene()
|
|
for c in ({'nonce':'foreign','completed':True,'pixels':24}, {'nonce':'test','completed':False,'pixels':24}):
|
|
with self.assertRaises(policy.Refused):scene.run(calibration=c)
|
|
self.assertEqual(scene.turns,[])
|
|
|
|
def first_pulse_with_guard_cost(self, cost):
|
|
scene=Scene();scene.no_movement=True;original=scene.send
|
|
def send(key,down):
|
|
original(key,down)
|
|
if down:scene.guard_cost=cost
|
|
scene.send=send
|
|
with self.assertRaises(policy.Refused):scene.run()
|
|
self.assertFalse(scene.held)
|
|
self.assertEqual([event[1] for event in scene.key_events],[True,False])
|
|
return scene
|
|
|
|
def test_slow_release_guard_uses_initial_hold_without_extra_sleep(self):
|
|
scene=self.first_pulse_with_guard_cost(.20)
|
|
down,up=scene.key_events
|
|
self.assertAlmostEqual(up[2]-down[2],.20,places=6)
|
|
timing=scene.report['approach_proof']['release_timings'][0]
|
|
self.assertFalse(timing['cleanup']);self.assertEqual(len(timing['guards']),1)
|
|
self.assertAlmostEqual(timing['guards'][0]['entered_monotonic'],down[2])
|
|
self.assertAlmostEqual(timing['guards'][0]['returned_monotonic'],up[2])
|
|
self.assertAlmostEqual(timing['up_attempted_monotonic'],up[2])
|
|
self.assertAlmostEqual(timing['up_returned_monotonic'],up[2])
|
|
|
|
def test_fast_guard_waits_only_remainder_then_full_fresh_guard(self):
|
|
scene=self.first_pulse_with_guard_cost(.01)
|
|
down,up=scene.key_events
|
|
self.assertAlmostEqual(up[2]-down[2],.06,places=6)
|
|
timing=scene.report['approach_proof']['release_timings'][0]
|
|
first,second=timing['guards']
|
|
self.assertAlmostEqual(first['entered_monotonic'],down[2])
|
|
self.assertAlmostEqual(first['returned_monotonic']-down[2],.01,places=6)
|
|
self.assertAlmostEqual(second['entered_monotonic']-down[2],.05,places=6)
|
|
self.assertAlmostEqual(second['returned_monotonic'],timing['up_attempted_monotonic'])
|
|
|
|
def test_guard_refusal_records_attempt_then_finally_rechecks_before_release(self):
|
|
scene=Scene();original=scene.check;held_checks=0
|
|
def check():
|
|
nonlocal held_checks
|
|
if scene.held:
|
|
held_checks+=1
|
|
if held_checks==1:
|
|
scene.sleep(.02)
|
|
raise policy.Refused('first owned release guard refused')
|
|
original()
|
|
scene.check=check
|
|
with self.assertRaisesRegex(policy.Refused,'release guard refused'):scene.run()
|
|
proof=scene.report['approach_proof'];first,cleanup=proof['release_timings']
|
|
self.assertIn('refused',first['guards'][0]['error'])
|
|
self.assertIsNone(first['up_attempted_monotonic'])
|
|
self.assertTrue(cleanup['cleanup']);self.assertEqual(len(cleanup['guards']),1)
|
|
self.assertIsNotNone(cleanup['up_returned_monotonic'])
|
|
self.assertFalse(scene.held);self.assertIsNone(proof['unconfirmed_held_key'])
|
|
self.assertEqual([event[1] for event in scene.key_events],[True,False])
|
|
|
|
def test_fast_guard_cannot_authorize_release_after_wait_and_focus_loss(self):
|
|
scene=Scene();original_send=scene.send;original_check=scene.check;checks=0
|
|
def send(key,down):
|
|
original_send(key,down)
|
|
if down:scene.guard_cost=.01
|
|
def check():
|
|
nonlocal checks
|
|
if scene.held:
|
|
checks+=1
|
|
if checks>=2:scene.focus=False
|
|
original_check()
|
|
scene.send=send;scene.check=check
|
|
with self.assertRaisesRegex(policy.Refused,'focus lost'):scene.run()
|
|
proof=scene.report['approach_proof'];first,cleanup=proof['release_timings']
|
|
self.assertEqual(len(first['guards']),2)
|
|
self.assertIn('focus lost',first['guards'][1]['error'])
|
|
self.assertIsNone(first['up_attempted_monotonic'])
|
|
self.assertIsNone(cleanup['up_attempted_monotonic'])
|
|
self.assertEqual([event[1] for event in scene.key_events],[True])
|
|
self.assertEqual(proof['unconfirmed_held_key'],'W')
|
|
self.assertTrue(proof['paired_release_errors'])
|
|
|
|
def test_shorter_pulses_do_not_expand_original_pulse_budget(self):
|
|
scene=Scene();scene.guard_cost=.18 # Original synthetic scene now takes smaller steps.
|
|
with self.assertRaisesRegex(policy.Refused,'pulse budget'):scene.run()
|
|
proof=scene.report['approach_proof']
|
|
self.assertEqual(len(scene.key_events),24)
|
|
self.assertEqual(len(proof['release_timings']),12)
|
|
self.assertTrue(all(len(timing['guards'])==1 for timing in proof['release_timings']))
|
|
self.assertFalse(proof['completed']);self.assertFalse(scene.held)
|
|
self.assertEqual(proof['deadline_monotonic'],35)
|
|
|
|
def test_measured_stopping_deadzone_is_still_refused(self):
|
|
scene=Scene();scene.position=[102.7,0.,97.3];scene.angle=180.;scene.guard_cost=.33
|
|
scene.sample=scene.snapshot();scene.history=[copy.deepcopy(scene.sample)]
|
|
with self.assertRaisesRegex(policy.Refused,'measured stopping reserve'):scene.run()
|
|
proof=scene.report['approach_proof'];self.assertEqual(len(proof['stops']),1)
|
|
target,_=policy.target_for(scene.geometry)
|
|
remaining=policy.horizontal(scene.position,target)
|
|
self.assertGreater(remaining,.75)
|
|
self.assertLessEqual(remaining,proof['stops'][0]['distance']+.25)
|
|
self.assertEqual([event[1] for event in scene.key_events],[True,False])
|
|
self.assertFalse(proof['completed']);self.assertFalse(scene.held)
|
|
|
|
|
|
if __name__=='__main__':unittest.main()
|