ReviewOS

also looking at this

stacks/drone

feat: model one point-load intake with conserved momentum

#99
Merged chris wants to merge codex/cargo-fleet-custody into main
45 files +3133 -169
resources/functions/drone/controls/physics.tsmodified+112-42
Changes to resources/functions/drone/controls/physics.ts
@@ -8,53 +8,79 @@ export { rotateBodyToNed, rotateNedToBody } from '../estimation/frames'
88interface RigidBodyInput { positionM?: SensorVector, motorsN?: Motors, groundDownM?: number, groundGradient?: [number, number] }
99interface CarriedPointLoad { id: string, massKg: number, forwardM: number }
1010interface BodySnapshot { positionM: SensorVector, velocityMps: SensorVector, angularRateRadS: SensorVector, accelerationMps2: SensorVector, quaternionWxyz: Quaternion, motorsN: Motors, groundContact: boolean }
11interface LoadProperties { id: string, phase: 'attached' | 'separated', pointMassKg: number, forwardM: number, totalMassKg: number, cgBodyM: SensorVector, inertiaDiagonalKgM2: SensorVector }
12interface LoadSnapshot { properties: LoadProperties, centerPositionM: SensorVector, centerVelocityMps: SensorVector }
13interface SeparationSide { properties: LoadProperties, bodyPositionM: SensorVector, bodyVelocityMps: SensorVector, quaternionWxyz: Quaternion, angularRateRadS: SensorVector, actualMotorsN: Motors, centerPositionM: SensorVector, centerVelocityMps: SensorVector }
11type PointLoadPhase = 'awaiting' | 'attached' | 'separated'
12interface LoadProperties<P extends PointLoadPhase = 'attached' | 'separated'> { id: string, phase: P, pointMassKg: number, forwardM: number, totalMassKg: number, cgBodyM: SensorVector, inertiaDiagonalKgM2: SensorVector }
13interface LoadSnapshot<P extends PointLoadPhase = 'attached' | 'separated'> { properties: LoadProperties<P>, centerPositionM: SensorVector, centerVelocityMps: SensorVector }
14interface SeparationSide<P extends PointLoadPhase = 'attached' | 'separated'> { properties: LoadProperties<P>, bodyPositionM: SensorVector, bodyVelocityMps: SensorVector, quaternionWxyz: Quaternion, angularRateRadS: SensorVector, actualMotorsN: Motors, centerPositionM: SensorVector, centerVelocityMps: SensorVector }
1415type LoadSeparation = { outcome: 'separated', reason: 'prescribed-zero-ejection', before: SeparationSide, after: SeparationSide, parcel: { massKg: number, positionM: SensorVector, velocityMps: SensorVector } } | { outcome: 'refused', reason: 'already-separated' }
1516interface RigidBody { snapshot(): BodySnapshot, step(command: Motors, dtS: number, disturbance?: { windMps?: SensorVector, forceN?: SensorVector, torqueNm?: SensorVector }): BodySnapshot }
1617interface LoadedRigidBody extends RigidBody { loadSnapshot(): LoadSnapshot, separateCarriedLoad(): LoadSeparation }
18export interface IntakeParcel { id: string, massKg: number, positionM: SensorVector, velocityMps: SensorVector }
19export type PointLoadAttachment = {
20 outcome: 'attached'
21 reason: 'coincident-inelastic-point-capture'
22 before: SeparationSide<'awaiting'>
23 after: SeparationSide<'attached'>
24 parcel: IntakeParcel
25 energy: { kineticBeforeJ: number, kineticAfterJ: number, dissipatedJ: number, numericalToleranceJ: number, potentialBeforeJ: number, potentialAfterJ: number }
26} | { outcome: 'refused', reason: 'intake-consumed' | 'unexpected-parcel' | 'mount-not-coincident' }
27export interface ReceivingRigidBody extends RigidBody {
28 loadSnapshot(): LoadSnapshot<PointLoadPhase>
29 intakePoint(): { positionM: SensorVector, velocityMps: SensorVector }
30 attachParcel(input: unknown): PointLoadAttachment
31 separateCarriedLoad(): LoadSeparation | { outcome: 'refused', reason: 'awaiting-load' }
32}
1733function cargoCross(a: SensorVector, b: SensorVector): SensorVector {
1834 return [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
1935}
36function captureOwnRecord(value: unknown, allowed: string[], required: string[]) {
37 if (!value || typeof value !== 'object' || Object.getPrototypeOf(value) !== Object.prototype) throw new TypeError()
38 const descriptors = Object.getOwnPropertyDescriptors(value), names = Reflect.ownKeys(value)
39 if (names.some(name => typeof name !== 'string' || !allowed.includes(name)) || required.some(name => !Object.hasOwn(descriptors, name))) throw new TypeError()
40 const captured: Record<string, unknown> = {}
41 for (const name of names as string[]) {
42 const descriptor = descriptors[name]!
43 if (!descriptor.enumerable || !Object.hasOwn(descriptor, 'value') || descriptor.value === undefined) throw new TypeError()
44 captured[name] = descriptor.value
45 }
46 return captured
47}
48function captureFiniteTuple(value: unknown, length: number, min: number, max: number) {
49 if (!Array.isArray(value) || value.length !== length || Reflect.ownKeys(value).length !== length + 1) throw new TypeError()
50 const captured: number[] = []
51 for (let i = 0; i < length; i++) {
52 const descriptor = Object.getOwnPropertyDescriptor(value, String(i))
53 if (!descriptor?.enumerable || !Object.hasOwn(descriptor, 'value') || typeof descriptor.value !== 'number' || !Number.isFinite(descriptor.value) || descriptor.value < min || descriptor.value > max) throw new TypeError()
54 captured.push(descriptor.value)
55 }
56 return captured
57}
58function captureIntakeParcel(input: unknown): IntakeParcel {
59 try {
60 const values = captureOwnRecord(input, ['id', 'massKg', 'positionM', 'velocityMps'], ['id', 'massKg', 'positionM', 'velocityMps'])
61 values.positionM = captureFiniteTuple(values.positionM, 3, -1e6, 1e6)
62 values.velocityMps = captureFiniteTuple(values.velocityMps, 3, -1e6, 1e6)
63 if (typeof values.id !== 'string' || !/^[a-z][a-z0-9-]{0,39}$/.test(values.id) || typeof values.massKg !== 'number' || !Number.isFinite(values.massKg) || values.massKg <= 0 || values.massKg > .25) throw new TypeError()
64 return values as unknown as IntakeParcel
65 }
66 catch { throw new TypeError('cargo-intake-parcel') }
67}
2068/** New opt-in records are captured as exact own data. The absent-own-member path is deliberately legacy. */
21function captureCarriedPointLoad(input: unknown): { input: RigidBodyInput, load: CarriedPointLoad } | null {
69function captureCarriedPointLoad(input: unknown, member: 'carriedPointLoad' | 'expectedPointLoad' = 'carriedPointLoad'): { input: RigidBodyInput, load: CarriedPointLoad } | null {
2270 try {
23 if (!input || (typeof input !== 'object' && typeof input !== 'function') || !Object.getOwnPropertyDescriptor(input, 'carriedPointLoad')) return null
24 function record(value: unknown, allowed: string[], required: string[]) {
25 if (!value || typeof value !== 'object' || Object.getPrototypeOf(value) !== Object.prototype) throw new TypeError()
26 const descriptors = Object.getOwnPropertyDescriptors(value), names = Reflect.ownKeys(value)
27 if (names.some(name => typeof name !== 'string' || !allowed.includes(name)) || required.some(name => !Object.hasOwn(descriptors, name))) throw new TypeError()
28 const captured: Record<string, unknown> = {}
29 for (const name of names as string[]) {
30 const descriptor = descriptors[name]!
31 if (!descriptor.enumerable || !Object.hasOwn(descriptor, 'value') || descriptor.value === undefined) throw new TypeError()
32 captured[name] = descriptor.value
33 }
34 return captured
35 }
36 function tuple(value: unknown, length: number, min: number, max: number) {
37 if (!Array.isArray(value) || value.length !== length || Reflect.ownKeys(value).length !== length + 1) throw new TypeError()
38 const captured: number[] = []
39 for (let i = 0; i < length; i++) {
40 const descriptor = Object.getOwnPropertyDescriptor(value, String(i))
41 if (!descriptor?.enumerable || !Object.hasOwn(descriptor, 'value') || typeof descriptor.value !== 'number' || !Number.isFinite(descriptor.value) || descriptor.value < min || descriptor.value > max) throw new TypeError()
42 captured.push(descriptor.value)
43 }
44 return captured
45 }
46 const captured = record(input, ['positionM', 'motorsN', 'groundDownM', 'groundGradient', 'carriedPointLoad'], ['carriedPointLoad'])
47 const load = record(captured.carriedPointLoad, ['id', 'massKg', 'forwardM'], ['id', 'massKg', 'forwardM'])
71 if (!input || (typeof input !== 'object' && typeof input !== 'function') || !Object.getOwnPropertyDescriptor(input, member)) return null
72 const captured = captureOwnRecord(input, ['positionM', 'motorsN', 'groundDownM', 'groundGradient', member], [member])
73 const load = captureOwnRecord(captured[member], ['id', 'massKg', 'forwardM'], ['id', 'massKg', 'forwardM'])
4874 if (typeof load.id !== 'string' || !/^[a-z][a-z0-9-]{0,39}$/.test(load.id) || typeof load.massKg !== 'number' || !Number.isFinite(load.massKg) || load.massKg <= 0 || load.massKg > 0.25 || typeof load.forwardM !== 'number' || !Number.isFinite(load.forwardM) || load.forwardM < -0.1 || load.forwardM > 0.1) throw new TypeError()
4975 const body: RigidBodyInput = {}
50 if (Object.hasOwn(captured, 'positionM')) body.positionM = tuple(captured.positionM, 3, -1e6, 1e6) as SensorVector
51 if (Object.hasOwn(captured, 'motorsN')) body.motorsN = tuple(captured.motorsN, 4, 0, model.maxMotorThrustN) as Motors
76 if (Object.hasOwn(captured, 'positionM')) body.positionM = captureFiniteTuple(captured.positionM, 3, -1e6, 1e6) as SensorVector
77 if (Object.hasOwn(captured, 'motorsN')) body.motorsN = captureFiniteTuple(captured.motorsN, 4, 0, model.maxMotorThrustN) as Motors
5278 if (Object.hasOwn(captured, 'groundDownM')) {
5379 if (typeof captured.groundDownM !== 'number' || !Number.isFinite(captured.groundDownM) || captured.groundDownM < -1 || captured.groundDownM > 0) throw new TypeError()
5480 body.groundDownM = captured.groundDownM
5581 }
5682 if (Object.hasOwn(captured, 'groundGradient')) {
57 body.groundGradient = tuple(captured.groundGradient, 2, -0.1, 0.1) as [number, number]
83 body.groundGradient = captureFiniteTuple(captured.groundGradient, 2, -0.1, 0.1) as [number, number]
5884 if (Math.hypot(...body.groundGradient) > 0.1) throw new TypeError()
5985 }
6086 return { input: body, load: { id: load.id, massKg: load.massKg, forwardM: load.forwardM } }
@@ -66,6 +92,18 @@ export function createRigidBody(input?: RigidBodyInput): RigidBody
6692export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: CarriedPointLoad } = {}): RigidBody | LoadedRigidBody {
6793 const configured = captureCarriedPointLoad(input)
6894 if (configured) input = configured.input
95 // This factory never exposes the awaiting phase or the intake operation.
96 return createBody(input, configured, false) as RigidBody | LoadedRigidBody
97}
98
99/** An empty declared point joint. Physical world/policy ownership of the parcel belongs to the caller. */
100export function createReceivingRigidBody(input: RigidBodyInput & { expectedPointLoad: CarriedPointLoad }): ReceivingRigidBody {
101 const configured = captureCarriedPointLoad(input, 'expectedPointLoad')
102 if (!configured) throw new TypeError('cargo-receiver-constructor')
103 return createBody(configured.input, configured, true) as ReceivingRigidBody
104}
105
106function createBody(input: RigidBodyInput, configured: ReturnType<typeof captureCarriedPointLoad>, receiving: boolean): RigidBody | LoadedRigidBody | ReceivingRigidBody {
69107
70108 const groundDownM = input.groundDownM ?? 0
71109 const groundGradient = [...(input.groundGradient ?? [0, 0])]
@@ -78,11 +116,12 @@ export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: Car
78116 let velocityMps: SensorVector = [0, 0, 0], angularRateRadS: SensorVector = [0, 0, 0], accelerationMps2: SensorVector = [0, 0, 0]
79117 let quaternionWxyz: Quaternion = [1, 0, 0, 0], groundContact = false
80118 function snapshot() { return structuredClone({ positionM, velocityMps, angularRateRadS, accelerationMps2, quaternionWxyz, motorsN, groundContact }) }
81 let attached = true
82 function properties(): LoadProperties {
119 let phase: PointLoadPhase = receiving ? 'awaiting' : 'attached'
120 function properties(selectedPhase: PointLoadPhase = phase): LoadProperties<PointLoadPhase> {
121 const attached = selectedPhase === 'attached'
83122 const load = configured!.load, totalMassKg = attached ? model.massKg + load.massKg : model.massKg
84123 const shift = attached ? model.massKg * load.massKg / totalMassKg * load.forwardM ** 2 : 0
85 return { id: load.id, phase: attached ? 'attached' : 'separated', pointMassKg: load.massKg, forwardM: load.forwardM, totalMassKg, cgBodyM: [attached ? load.massKg * load.forwardM / totalMassKg : 0, 0, 0], inertiaDiagonalKgM2: [model.inertiaKgM2[0]!, model.inertiaKgM2[1]! + shift, model.inertiaKgM2[2]! + shift] }
124 return { id: load.id, phase: selectedPhase, pointMassKg: load.massKg, forwardM: load.forwardM, totalMassKg, cgBodyM: [attached ? load.massKg * load.forwardM / totalMassKg : 0, 0, 0], inertiaDiagonalKgM2: [model.inertiaKgM2[0]!, model.inertiaKgM2[1]! + shift, model.inertiaKgM2[2]! + shift] }
86125 }
87126 let centerPositionM: SensorVector | null = configured ? positionM.map((n, i) => n + properties().cgBodyM[i]!) as SensorVector : null
88127 let centerVelocityMps: SensorVector | null = configured ? [...velocityMps] : null
@@ -140,19 +179,50 @@ export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: Car
140179 },
141180 }
142181 if (!configured) return body
143 function loadSnapshot(): LoadSnapshot { return { properties: properties(), centerPositionM: [...centerPositionM!], centerVelocityMps: [...centerVelocityMps!] } }
144 function side(): SeparationSide {
182 function loadSnapshot(): LoadSnapshot<PointLoadPhase> { return { properties: properties(), centerPositionM: [...centerPositionM!], centerVelocityMps: [...centerVelocityMps!] } }
183 function side(): SeparationSide<PointLoadPhase> {
145184 return { properties: properties(), bodyPositionM: [...positionM], bodyVelocityMps: [...velocityMps], quaternionWxyz: [...quaternionWxyz], angularRateRadS: [...angularRateRadS], actualMotorsN: [...motorsN], centerPositionM: [...centerPositionM!], centerVelocityMps: [...centerVelocityMps!] }
146185 }
147 function separateCarriedLoad(...args: unknown[]): LoadSeparation {
186 function separateCarriedLoad(...args: unknown[]): LoadSeparation | { outcome: 'refused', reason: 'awaiting-load' } {
148187 if (args.length !== 0) throw new TypeError('cargo-load-separation-arguments')
149 if (!attached) return Object.freeze({ outcome: 'refused', reason: 'already-separated' })
188 if (phase === 'awaiting') return Object.freeze({ outcome: 'refused', reason: 'awaiting-load' })
189 if (phase === 'separated') return Object.freeze({ outcome: 'refused', reason: 'already-separated' })
150190 const before = side(), r: SensorVector = [configured!.load.forwardM, 0, 0]
151191 const positionLever = rotateBodyToNed(quaternionWxyz, r), velocityLever = rotateBodyToNed(quaternionWxyz, cargoCross(angularRateRadS, r))
152192 const parcel = { massKg: configured!.load.massKg, positionM: positionM.map((n, i) => n + positionLever[i]!) as SensorVector, velocityMps: velocityMps.map((n, i) => n + velocityLever[i]!) as SensorVector }
153 attached = false; centerPositionM = [...positionM]; centerVelocityMps = [...velocityMps]
154 return { outcome: 'separated', reason: 'prescribed-zero-ejection', before, after: side(), parcel }
193 phase = 'separated'; centerPositionM = [...positionM]; centerVelocityMps = [...velocityMps]
194 return { outcome: 'separated', reason: 'prescribed-zero-ejection', before: before as SeparationSide, after: side() as SeparationSide, parcel }
195 }
196 if (!receiving) return { ...body, loadSnapshot, separateCarriedLoad } as LoadedRigidBody
197 function intakePoint(): { positionM: SensorVector, velocityMps: SensorVector } {
198 const r: SensorVector = [configured!.load.forwardM, 0, 0], lever = rotateBodyToNed(quaternionWxyz, r), rate = rotateBodyToNed(quaternionWxyz, cargoCross(angularRateRadS, r))
199 return { positionM: positionM.map((n, i) => n + lever[i]!) as SensorVector, velocityMps: velocityMps.map((n, i) => n + rate[i]!) as SensorVector }
200 }
201 function attachParcel(input: unknown, ...extra: unknown[]): PointLoadAttachment {
202 if (extra.length) throw new TypeError('cargo-intake-parcel')
203 const parcel = captureIntakeParcel(input)
204 if (phase !== 'awaiting') return { outcome: 'refused', reason: 'intake-consumed' }
205 const mount = intakePoint()
206 if (parcel.id !== configured!.load.id || parcel.massKg !== configured!.load.massKg) return { outcome: 'refused', reason: 'unexpected-parcel' }
207 if (!parcel.positionM.every((n, i) => n === mount.positionM[i])) return { outcome: 'refused', reason: 'mount-not-coincident' }
208 const before = side() as SeparationSide<'awaiting'>, afterProperties = properties('attached'), total = afterProperties.totalMassKg
209 const c = afterProperties.cgBodyM, inertia = afterProperties.inertiaDiagonalKgM2, reduced = model.massKg * parcel.massKg / total
210 const relative = rotateNedToBody(quaternionWxyz, parcel.velocityMps.map((n, i) => n - velocityMps[i]!) as SensorVector)
211 const transfer = cargoCross([configured!.load.forwardM, 0, 0], relative)
212 const nextRate = angularRateRadS.map((n, i) => (model.inertiaKgM2[i]! * n + reduced * transfer[i]!) / inertia[i]!) as SensorVector
213 const nextCenterVelocity = velocityMps.map((n, i) => (model.massKg * n + parcel.massKg * parcel.velocityMps[i]!) / total) as SensorVector
214 const lever = rotateBodyToNed(quaternionWxyz, c), rateLever = rotateBodyToNed(quaternionWxyz, cargoCross(nextRate, c))
215 const nextCenterPosition = positionM.map((n, i) => n + lever[i]!) as SensorVector
216 const nextVelocity = nextCenterVelocity.map((n, i) => n - rateLever[i]!) as SensorVector
217 const square = (v: SensorVector) => v.reduce((sum, n) => sum + n * n, 0)
218 const kineticBeforeJ = .5 * model.massKg * square(velocityMps) + .5 * angularRateRadS.reduce((sum, n, i) => sum + model.inertiaKgM2[i]! * n * n, 0) + .5 * parcel.massKg * square(parcel.velocityMps)
219 const kineticAfterJ = .5 * total * square(nextCenterVelocity) + .5 * nextRate.reduce((sum, n, i) => sum + inertia[i]! * n * n, 0)
220 const dissipatedJ = kineticBeforeJ - kineticAfterJ, numericalToleranceJ = 64 * Number.EPSILON * Math.max(1, kineticBeforeJ, kineticAfterJ)
221 const potentialBeforeJ = -model.gravityMps2 * (model.massKg * positionM[2] + parcel.massKg * parcel.positionM[2]), potentialAfterJ = -model.gravityMps2 * total * nextCenterPosition[2]
222 if (![...nextRate, ...nextCenterVelocity, ...nextCenterPosition, ...nextVelocity, kineticBeforeJ, kineticAfterJ, dissipatedJ, potentialBeforeJ, potentialAfterJ].every(Number.isFinite) || dissipatedJ < -numericalToleranceJ) throw new RangeError('cargo-intake-numerical-state')
223 phase = 'attached'; angularRateRadS = nextRate; velocityMps = nextVelocity; centerPositionM = nextCenterPosition; centerVelocityMps = nextCenterVelocity
224 return { outcome: 'attached', reason: 'coincident-inelastic-point-capture', before, after: side() as SeparationSide<'attached'>, parcel, energy: { kineticBeforeJ, kineticAfterJ, dissipatedJ, numericalToleranceJ, potentialBeforeJ, potentialAfterJ } }
155225 }
156 return { ...body, loadSnapshot, separateCarriedLoad }
226 return { ...body, loadSnapshot, separateCarriedLoad, intakePoint, attachParcel }
157227}
158228export type RigidBodyState = ReturnType<ReturnType<typeof createRigidBody>['snapshot']>