ReviewOS

also looking at this

stacks/drone

feat: add retained cargo fleet cancellation and recovery

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