229 lines
12 KiB
Python
229 lines
12 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=.18;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=.4
|
|
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
|
|
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,[])
|
|
|
|
|
|
if __name__=='__main__':unittest.main()
|