ReviewOS

also looking at this

stacks/drone

feat: model phase-bound cargo fleet failures

#104
Merged chris wants to merge codex/cargo-fleet-phase-faults into main
25 files +1844 -13
resources/functions/drone/cargo/station.tsadded+166-0
Changes to resources/functions/drone/cargo/station.ts
@@ -0,0 +1,166 @@
1import type { IntakeParcel } from '../controls/physics'
2import type { SensorVector } from '../sensors/types'
3import { multirotorModel } from '../controls/model'
4
5interface StationDeclaration {
6 atMs: number
7 parcel: IntakeParcel
8 pickup: { centerM: SensorVector, radiusM: number }
9 destinationM: SensorVector
10 maxSpeedMps: number
11 maxAccelerationMps2: number
12}
13export interface CargoStationSnapshot {
14 evidence: 'modeled-cargo-station-v1'
15 physicalActuationEnabled: false
16 phase: 'falling' | 'held' | 'missed' | 'moving' | 'braking' | 'cancelled' | 'ready' | 'departed'
17 atMs: number
18 startedAtMs: number | null
19 durationMs: number | null
20 cancelRequestedAtMs: number | null
21 stoppedAtMs: number | null
22 ownedMassKg: number
23 parcel: IntakeParcel | null
24 forceN: SensorVector
25 impact: { atMs: number, positionM: SensorVector, velocityBeforeMps: SensorVector, impulseNs: SensorVector, dissipationJ: number } | null
26 energy: { initialJ: number, currentJ: number, departedJ: number, stationNetWorkJ: number, positiveWorkJ: number, negativeWorkJ: number, planeDissipationJ: number, residualJ: number }
27}
28function record(input: unknown, names: string[]) {
29 if (!input || typeof input !== 'object' || Object.getPrototypeOf(input) !== Object.prototype || Reflect.ownKeys(input).length !== names.length) throw new TypeError()
30 const captured: Record<string, unknown> = {}
31 for (const name of names) {
32 const d = Object.getOwnPropertyDescriptor(input, name)
33 if (!d?.enumerable || !Object.hasOwn(d, 'value')) throw new TypeError()
34 captured[name] = d.value
35 }
36 return captured
37}
38function vector(input: unknown, min: number, max: number): SensorVector {
39 if (!Array.isArray(input) || input.length !== 3 || Reflect.ownKeys(input).length !== 4) throw new TypeError()
40 const captured: number[] = []
41 for (let i = 0; i < 3; i++) {
42 const d = Object.getOwnPropertyDescriptor(input, String(i))
43 if (!d?.enumerable || !Object.hasOwn(d, 'value') || typeof d.value !== 'number' || !Number.isFinite(d.value) || d.value < min || d.value > max) throw new TypeError()
44 captured.push(d.value)
45 }
46 return captured as SensorVector
47}
48const within = (v: unknown, min: number, max: number): v is number => typeof v === 'number' && Number.isFinite(v) && v >= min && v <= max
49const distance = (a: SensorVector, b: SensorVector) => Math.hypot(...a.map((n, i) => n - b[i]!))
50function durationMs(lengthM: number, speed: number, acceleration: number) {
51 // Peak derivatives of 10u^3 - 15u^4 + 6u^5 are 15/8 and 10/sqrt(3).
52 return Math.ceil(1000 * Math.max(.5, 1.875 * lengthM / speed, Math.sqrt(10 / Math.sqrt(3) * lengthM / acceleration)) / 8) * 8
53}
54function capture(input: unknown): StationDeclaration {
55 try {
56 const v = record(input, ['atMs', 'parcel', 'pickup', 'destinationM', 'maxSpeedMps', 'maxAccelerationMps2'])
57 const p = record(v.parcel, ['id', 'massKg', 'positionM', 'velocityMps']), pickup = record(v.pickup, ['centerM', 'radiusM'])
58 if (!Number.isSafeInteger(v.atMs) || !within(v.atMs, 0, Number.MAX_SAFE_INTEGER - 180000) || v.atMs % 4 !== 0
59 || typeof p.id !== 'string' || !/^[a-z][a-z0-9-]{0,39}$/.test(p.id) || !within(p.massKg, Number.MIN_VALUE, .25)
60 || !within(pickup.radiusM, .05, 2) || !within(v.maxSpeedMps, .05, 1) || !within(v.maxAccelerationMps2, .05, 2)) throw new TypeError()
61 const positionM = vector(p.positionM, -1000, 1000), velocityMps = vector(p.velocityMps, -5, 5), centerM = vector(pickup.centerM, -1000, 1000), destinationM = vector(v.destinationM, -1000, 1000)
62 if (!within(centerM[2], -1, 0) || !within(positionM[2], centerM[2] - 3, centerM[2]) || destinationM[2] < centerM[2] - 1 || destinationM[2] > centerM[2]
63 || distance(centerM, destinationM) + pickup.radiusM > 20
64 || durationMs(distance(centerM, destinationM) + pickup.radiusM, v.maxSpeedMps, v.maxAccelerationMps2) > 120000) throw new TypeError()
65 return { atMs: v.atMs, parcel: { id: p.id, massKg: p.massKg, positionM, velocityMps }, pickup: { centerM, radiusM: pickup.radiusM }, destinationM, maxSpeedMps: v.maxSpeedMps, maxAccelerationMps2: v.maxAccelerationMps2 }
66 }
67 catch { throw new TypeError('cargo-station-constructor') }
68}
69
70/** One point parcel on a prescribed, finite powered station rail. No hardware IO or custody ACK. */
71export function createCargoStationHandler(input: unknown) {
72 const config = capture(input), gravity = multirotorModel.gravityMps2
73 let parcel: IntakeParcel | null = config.parcel, phase: CargoStationSnapshot['phase'] = 'falling', nowMs = config.atMs
74 let startedAtMs: number | null = null, motionMs: number | null = null, fromM: SensorVector | null = null
75 let cancelRequestedAtMs: number | null = null, stoppedAtMs: number | null = null
76 let brake: { atMs: number, positionM: SensorVector, velocityMps: SensorVector, speedMps: number } | null = null
77 let forceN: SensorVector = [0, 0, 0], impact: CargoStationSnapshot['impact'] = null
78 const mechanical = (p: IntakeParcel) => .5 * p.massKg * p.velocityMps.reduce((sum, n) => sum + n * n, 0) - p.massKg * gravity * p.positionM[2]
79 const initialJ = mechanical(parcel)
80 let departedJ = 0, stationNetWorkJ = 0, positiveWorkJ = 0, negativeWorkJ = 0, planeDissipationJ = 0
81 function capturePlane(atMs: number) {
82 const p = parcel!, before = [...p.velocityMps] as SensorVector
83 const dissipationJ = .5 * p.massKg * before.reduce((sum, n) => sum + n * n, 0)
84 p.positionM[2] = config.pickup.centerM[2]
85 impact = { atMs, positionM: [...p.positionM], velocityBeforeMps: before, impulseNs: before.map(n => -p.massKg * n) as SensorVector, dissipationJ }
86 planeDissipationJ += dissipationJ
87 p.velocityMps = [0, 0, 0]; forceN = [0, 0, -p.massKg * gravity]
88 phase = cancelRequestedAtMs !== null ? 'cancelled' : Math.hypot(p.positionM[0] - config.pickup.centerM[0], p.positionM[1] - config.pickup.centerM[1]) <= config.pickup.radiusM ? 'held' : 'missed'
89 if (cancelRequestedAtMs !== null) stoppedAtMs = atMs
90 }
91 if (parcel.positionM[2] === config.pickup.centerM[2] && parcel.velocityMps[2] >= 0) capturePlane(nowMs)
92 function retainWork(beforeJ: number) {
93 const work = mechanical(parcel!) - beforeJ
94 stationNetWorkJ += work; positiveWorkJ += Math.max(0, work); negativeWorkJ += Math.min(0, work)
95 }
96 function snapshot(): CargoStationSnapshot {
97 const currentJ = parcel ? mechanical(parcel) : 0
98 return structuredClone({ evidence: 'modeled-cargo-station-v1', physicalActuationEnabled: false, phase, atMs: nowMs, startedAtMs, durationMs: motionMs, cancelRequestedAtMs, stoppedAtMs, parcel,
99 ownedMassKg: parcel?.massKg ?? 0, forceN, impact, energy: { initialJ, currentJ, departedJ, stationNetWorkJ, positiveWorkJ, negativeWorkJ, planeDissipationJ, residualJ: currentJ + departedJ + planeDissipationJ - initialJ - stationNetWorkJ } })
100 }
101 return {
102 snapshot,
103 start(...args: unknown[]): { outcome: 'started', atMs: number, durationMs: number } | { outcome: 'refused', reason: 'not-held' | 'deadline' } {
104 if (args.length) throw new TypeError('cargo-station-arguments')
105 if (phase !== 'held') return { outcome: 'refused', reason: 'not-held' }
106 const duration = durationMs(distance(parcel!.positionM, config.destinationM), config.maxSpeedMps, config.maxAccelerationMps2)
107 if (nowMs - config.atMs + duration > 180000) return { outcome: 'refused', reason: 'deadline' }
108 fromM = [...parcel!.positionM]; motionMs = duration; startedAtMs = nowMs; phase = 'moving'
109 return { outcome: 'started', atMs: nowMs, durationMs: duration }
110 },
111 cancel(...args: unknown[]): { outcome: 'cancelling' | 'cancelled', atMs: number } | { outcome: 'refused', reason: 'already-cancelled' | 'departed' } {
112 if (args.length) throw new TypeError('cargo-station-arguments')
113 if (phase === 'departed') return { outcome: 'refused', reason: 'departed' }
114 if (cancelRequestedAtMs !== null) return { outcome: 'refused', reason: 'already-cancelled' }
115 cancelRequestedAtMs = nowMs
116 const speedMps = Math.hypot(...parcel!.velocityMps)
117 if (phase === 'moving' && speedMps > 0) {
118 brake = { atMs: nowMs, positionM: [...parcel!.positionM], velocityMps: [...parcel!.velocityMps], speedMps }
119 phase = 'braking'
120 }
121 else if (phase !== 'falling') { phase = 'cancelled'; stoppedAtMs = nowMs }
122 return { outcome: phase === 'cancelled' ? 'cancelled' : 'cancelling', atMs: nowMs }
123 },
124 step(atMs: number, ...extra: unknown[]) {
125 if (extra.length || !Number.isSafeInteger(atMs) || atMs !== nowMs + 4 || atMs - config.atMs > 180000) throw new TypeError('cargo-station-clock')
126 if (phase === 'falling') {
127 const p = parcel!, down = p.positionM[2] - config.pickup.centerM[2], vz = p.velocityMps[2]
128 const speedAtPlane = Math.sqrt(vz * vz - 2 * gravity * down)
129 // Stable positive root for downward motion, including a point already at the plane.
130 const hitSeconds = vz >= 0 ? (down === 0 ? 0 : -2 * down / (vz + speedAtPlane)) : (-vz + speedAtPlane) / gravity
131 const seconds = Math.min(.004, hitSeconds)
132 p.positionM = p.positionM.map((n, i) => n + p.velocityMps[i]! * seconds + (i === 2 ? .5 * gravity * seconds ** 2 : 0)) as SensorVector
133 p.velocityMps[2] += gravity * seconds
134 if (hitSeconds <= .004) capturePlane(nowMs + hitSeconds * 1000)
135 }
136 else if (phase === 'braking') {
137 const b = brake!, p = parcel!, beforeJ = mechanical(p), stopSeconds = b.speedMps / config.maxAccelerationMps2
138 const seconds = Math.min((atMs - b.atMs) / 1000, stopSeconds), fraction = Math.max(0, 1 - seconds / stopSeconds)
139 p.positionM = b.positionM.map((n, i) => n + b.velocityMps[i]! * (seconds - .5 * seconds ** 2 / stopSeconds)) as SensorVector
140 p.velocityMps = b.velocityMps.map(n => n * fraction) as SensorVector
141 forceN = b.velocityMps.map((n, i) => p.massKg * (-n / stopSeconds - (i === 2 ? gravity : 0))) as SensorVector
142 retainWork(beforeJ)
143 if (seconds === stopSeconds) { phase = 'cancelled'; stoppedAtMs = b.atMs + stopSeconds * 1000; forceN = [0, 0, -p.massKg * gravity] }
144 }
145 else if (phase === 'moving') {
146 const beforeJ = mechanical(parcel!), seconds = motionMs! / 1000, u = Math.min(1, (atMs - startedAtMs!) / motionMs!)
147 const s = 10 * u ** 3 - 15 * u ** 4 + 6 * u ** 5, ds = 30 * u ** 2 * (1 - u) ** 2, dds = 60 * u * (1 - u) * (1 - 2 * u)
148 const delta = config.destinationM.map((n, i) => n - fromM![i]!) as SensorVector
149 parcel!.positionM = u === 1 ? [...config.destinationM] : fromM!.map((n, i) => n + delta[i]! * s) as SensorVector
150 parcel!.velocityMps = delta.map(n => n * ds / seconds) as SensorVector
151 forceN = delta.map((n, i) => parcel!.massKg * (n * dds / seconds ** 2 - (i === 2 ? gravity : 0))) as SensorVector
152 retainWork(beforeJ)
153 if (u === 1) phase = 'ready'
154 }
155 nowMs = atMs
156 return snapshot()
157 },
158 takeReadyParcel(...args: unknown[]): { outcome: 'departed', atMs: number, parcel: IntakeParcel } | { outcome: 'refused', reason: 'not-ready' } {
159 if (args.length) throw new TypeError('cargo-station-arguments')
160 if (phase !== 'ready') return { outcome: 'refused', reason: 'not-ready' }
161 const outgoing = structuredClone(parcel!)
162 departedJ = mechanical(parcel!); parcel = null; phase = 'departed'; forceN = [0, 0, 0]
163 return { outcome: 'departed', atMs: nowMs, parcel: outgoing }
164 },
165 }
166}