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
tests/unit/cargo-intake.test.tsadded+167-0
Changes to tests/unit/cargo-intake.test.ts
@@ -0,0 +1,167 @@
1import { expect, test } from 'bun:test'
2import { createReceivingRigidBody, createRigidBody } from '../../resources/functions/drone/controls/physics'
3import type { SensorVector } from '../../resources/functions/drone/sensors/types'
4import type { Quaternion } from '../../resources/functions/drone/estimation/frames'
5
6const plus = (a: SensorVector, b: SensorVector) => a.map((n, i) => n + b[i]!) as SensorVector
7const minus = (a: SensorVector, b: SensorVector) => a.map((n, i) => n - b[i]!) as SensorVector
8const scale = (a: SensorVector, n: number) => a.map(v => v * n) as SensorVector
9const cross = (a: SensorVector, b: SensorVector): SensorVector => [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]
10const dot = (a: SensorVector, b: SensorVector) => a.reduce((sum, n, i) => sum + n * b[i]!, 0)
11function rotate([w, x, y, z]: Quaternion, v: SensorVector): SensorVector {
12 return [[1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)], [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)], [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)]].map(row => row.reduce((sum, n, i) => sum + n * v[i]!, 0)) as SensorVector
13}
14function closeVector(actual: SensorVector, expected: SensorVector) { expect(Math.max(...minus(actual, expected).map(Math.abs))).toBeLessThanOrEqual(2e-12) }
15function mechanical(state: ReturnType<ReturnType<typeof createReceivingRigidBody>['snapshot']>, parcel: { massKg: number, positionM: SensorVector, velocityMps: SensorVector }) {
16 const origin: SensorVector = [7, -3, 5], inertia: SensorVector = [.03, .03, .05]
17 const baseP = scale(state.velocityMps, 1.5), parcelP = scale(parcel.velocityMps, parcel.massKg)
18 const spin = rotate(state.quaternionWxyz, state.angularRateRadS.map((n, i) => n * inertia[i]!) as SensorVector)
19 return { momentum: plus(baseP, parcelP), angular: plus(spin, plus(cross(minus(state.positionM, origin), baseP), cross(minus(parcel.positionM, origin), parcelP))),
20 kinetic: .75 * dot(state.velocityMps, state.velocityMps) + .5 * state.angularRateRadS.reduce((sum, n, i) => sum + inertia[i]! * n * n, 0) + .5 * parcel.massKg * dot(parcel.velocityMps, parcel.velocityMps),
21 potential: -9.80665 * (1.5 * state.positionM[2] + parcel.massKg * parcel.positionM[2]) }
22}
23
24// Actual physics seam: proves point-load capture and state continuity, not a
25// mechanism acknowledgement, vehicle authority or fleet custody protocol.
26test('a receiving body captures one coincident point mass with the worked inelastic momentum result', () => {
27 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } })
28 expect(body.loadSnapshot().properties.phase).toBe('awaiting')
29 expect(body.loadSnapshot().properties.totalMassKg).toBe(1.5)
30 const before = body.snapshot(), point = body.intakePoint()
31 expect(point.positionM).toEqual([.08, 0, -1])
32 const event = body.attachParcel({ id: 'parcel-a', massKg: .1, positionM: point.positionM, velocityMps: [0, 1, 0] })
33 expect(event.outcome).toBe('attached')
34 if (event.outcome !== 'attached') throw new Error('Expected actual modeled capture')
35 expect(event.before.properties.phase).toBe('awaiting')
36 expect(event.after.properties.phase).toBe('attached')
37 expect(event.after.properties.totalMassKg).toBe(1.6)
38 expect(event.after.properties.cgBodyM).toEqual([.005, 0, 0])
39 // Rational expected values allow the binary64 accumulation error, not a model change.
40 for (const [i, expected] of [.03, .0306, .0506].entries()) expect(event.after.properties.inertiaDiagonalKgM2[i]!).toBeCloseTo(expected, 14)
41 expect(body.snapshot().angularRateRadS[2]).toBeCloseTo(.1482213438735178, 13)
42 expect(body.snapshot().velocityMps[1]).toBeCloseTo(.06175889328063241, 13)
43 expect(event.energy.kineticBeforeJ).toBeCloseTo(.05, 14)
44 expect(event.energy.kineticAfterJ).toBeCloseTo(.003680830039525692, 14)
45 expect(event.energy.dissipatedJ).toBeCloseTo(.04631916996047431, 14)
46 expect(body.snapshot().positionM).toEqual(before.positionM)
47 expect(body.snapshot().quaternionWxyz).toEqual(before.quaternionWxyz)
48 expect(body.snapshot().motorsN).toEqual(before.motorsN)
49 expect(body.attachParcel({ id: 'parcel-a', massKg: .1, positionM: point.positionM, velocityMps: [0, 1, 0] })).toEqual({ outcome: 'refused', reason: 'intake-consumed' })
50 const separated = body.separateCarriedLoad()
51 expect(separated.outcome).toBe('separated')
52 if (separated.outcome !== 'separated') throw new Error('Expected actual subsequent release')
53 expect(mechanical(body.snapshot(), separated.parcel).kinetic).toBeCloseTo(.003680830039525692, 14)
54})
55
56test('rotated captures conserve world momentum about an independent origin and dissipate relative motion', () => {
57 for (const massKg of [.01, .1, .25]) for (const forwardM of [-.1, 0, .1]) {
58 const body = createReceivingRigidBody({ positionM: [1, -2, -1], motorsN: [2, 2, 2, 2], expectedPointLoad: { id: 'parcel-a', massKg, forwardM } })
59 for (let i = 0; i < 40; i++) body.step([3, 2.9, 3.1, 3], .004, { windMps: [.2, -.1, 0], torqueNm: [.02, -.01, .03] })
60 const before = body.snapshot(), point = body.intakePoint()
61 const parcel = { id: 'parcel-a', massKg, ...point, velocityMps: plus(point.velocityMps, [.2, -.3, .1]) }
62 closeVector(point.positionM, plus(before.positionM, rotate(before.quaternionWxyz, [forwardM, 0, 0])))
63 const initial = mechanical(before, parcel), event = body.attachParcel(parcel)
64 expect(event.outcome).toBe('attached')
65 if (event.outcome !== 'attached') throw new Error('Expected rotated capture')
66 const after = body.snapshot(), afterPoint = { massKg, positionM: plus(after.positionM, rotate(after.quaternionWxyz, [forwardM, 0, 0])), velocityMps: plus(after.velocityMps, rotate(after.quaternionWxyz, cross(after.angularRateRadS, [forwardM, 0, 0]))) }
67 const final = mechanical(after, afterPoint)
68 closeVector(final.momentum, initial.momentum)
69 closeVector(final.angular, initial.angular)
70 expect(Math.abs(final.potential - initial.potential)).toBeLessThanOrEqual(2e-12)
71 expect(initial.kinetic - final.kinetic).toBeGreaterThan(0)
72 expect(event.energy.kineticBeforeJ).toBeCloseTo(initial.kinetic, 12)
73 expect(event.energy.kineticAfterJ).toBeCloseTo(final.kinetic, 12)
74 expect(event.energy.dissipatedJ).toBeCloseTo(initial.kinetic - final.kinetic, 12)
75 expect(event.energy.potentialBeforeJ).toBeCloseTo(initial.potential, 12)
76 expect(event.energy.potentialAfterJ).toBeCloseTo(final.potential, 12)
77 expect(after.positionM).toEqual(before.positionM)
78 expect(after.quaternionWxyz).toEqual(before.quaternionWxyz)
79 expect(after.motorsN).toEqual(before.motorsN)
80 event.after.bodyVelocityMps[0] = 999; event.parcel.positionM[0] = 999
81 expect(body.snapshot()).toEqual(after)
82 }
83})
84
85test('co-moving intake preserves body motion through a later separation', () => {
86 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: -.08 } })
87 for (let i = 0; i < 30; i++) body.step([4, 3.8, 4.1, 4], .004, { torqueNm: [.01, -.02, .03] })
88 const before = body.snapshot(), parcel = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }, event = body.attachParcel(parcel)
89 expect(event.outcome).toBe('attached')
90 if (event.outcome !== 'attached') throw new Error('Expected co-moving capture')
91 closeVector(body.snapshot().velocityMps, before.velocityMps)
92 closeVector(body.snapshot().angularRateRadS, before.angularRateRadS)
93 expect(Math.abs(event.energy.dissipatedJ)).toBeLessThanOrEqual(2e-12)
94 const separated = body.separateCarriedLoad()
95 expect(separated.outcome).toBe('separated')
96 if (separated.outcome !== 'separated') throw new Error('Expected subsequent separation')
97 closeVector(separated.parcel.positionM, parcel.positionM)
98 closeVector(separated.parcel.velocityMps, parcel.velocityMps)
99 closeVector(body.snapshot().velocityMps, before.velocityMps)
100 closeVector(body.snapshot().angularRateRadS, before.angularRateRadS)
101 expect(body.loadSnapshot().properties.phase).toBe('separated')
102 expect(body.loadSnapshot().properties.totalMassKg).toBe(1.5)
103 expect(body.attachParcel(parcel)).toEqual({ outcome: 'refused', reason: 'intake-consumed' })
104 expect(body.separateCarriedLoad()).toEqual({ outcome: 'refused', reason: 'already-separated' })
105})
106
107test('the actual subsequent acceleration uses the received mass and the empty receiver has no phantom load', () => {
108 for (const attached of [false, true]) {
109 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: 0 } })
110 if (attached) expect(body.attachParcel({ id: 'parcel-a', massKg: .1, ...body.intakePoint() }).outcome).toBe('attached')
111 const state = body.step([4, 4, 4, 4], .004), mass = attached ? 1.6 : 1.5
112 const thrust = 16 * (1 - Math.exp(-.004 / .04))
113 expect(state.accelerationMps2[2]).toBeCloseTo(9.80665 - thrust / mass, 12)
114 expect(body.loadSnapshot().properties.totalMassKg).toBe(mass)
115 }
116})
117
118test('intake refuses foreign or noncoincident parcels without consuming or moving the body', () => {
119 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } })
120 const before = body.snapshot(), point = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }
121 expect(body.separateCarriedLoad()).toEqual({ outcome: 'refused', reason: 'awaiting-load' })
122 for (const candidate of [{ ...point, id: 'parcel-b' }, { ...point, massKg: .2 }]) expect(body.attachParcel(candidate)).toEqual({ outcome: 'refused', reason: 'unexpected-parcel' })
123 expect(body.attachParcel({ ...point, positionM: [.081, 0, -1] })).toEqual({ outcome: 'refused', reason: 'mount-not-coincident' })
124 expect(body.snapshot()).toEqual(before)
125 expect(body.loadSnapshot().properties.phase).toBe('awaiting')
126 expect(body.attachParcel(point).outcome).toBe('attached')
127})
128
129test('intake captures exact own data and malformed input cannot evaluate getters or mutate state', () => {
130 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: 0 } })
131 const before = body.snapshot(), point = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }
132 let called = false
133 const accessor = { ...point, get velocityMps() { called = true; return [0, 0, 0] } }
134 const tuple = [0, 0, 0]; Object.defineProperty(tuple, '0', { enumerable: true, get() { called = true; return 0 } })
135 for (const value of [null, { ...point, extra: true }, { ...point, massKg: NaN }, { ...point, velocityMps: [0, Infinity, 0] }, accessor, { ...point, velocityMps: tuple }, { ...point, positionM: [, 0, -1] }]) expect(() => body.attachParcel(value)).toThrow('cargo-intake-parcel')
136 const proxy = new Proxy(point, { ownKeys() { throw new Error('private-caller-sentinel') } })
137 expect(() => body.attachParcel(proxy)).toThrow(/^cargo-intake-parcel$/)
138 expect(() => Reflect.apply(body.attachParcel, body, [point, true])).toThrow('cargo-intake-parcel')
139 expect(called).toBe(false)
140 expect(body.snapshot()).toEqual(before)
141 expect(body.loadSnapshot().properties.phase).toBe('awaiting')
142 expect(() => createReceivingRigidBody({ expectedPointLoad: { get id() { called = true; return 'parcel-a' }, massKg: .1, forwardM: 0 } })).toThrow()
143 expect(called).toBe(false)
144 expect(body.attachParcel(point).outcome).toBe('attached')
145})
146
147test('joint contact is current physical state and the planned identity is captured once', () => {
148 const expected = { id: 'parcel-a', massKg: .1, forwardM: .08 }
149 const body = createReceivingRigidBody({ positionM: [0, 0, -1], expectedPointLoad: expected })
150 const stale = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }
151 expected.id = 'parcel-b'; expected.massKg = .2; expected.forwardM = -.08
152 body.step([0, 0, 0, 0], .004)
153 expect(body.attachParcel(stale)).toEqual({ outcome: 'refused', reason: 'mount-not-coincident' })
154 expect(body.loadSnapshot().properties.phase).toBe('awaiting')
155 const current = { id: 'parcel-a', massKg: .1, ...body.intakePoint() }
156 expect(body.attachParcel(current).outcome).toBe('attached')
157 expect(body.loadSnapshot().properties.forwardM).toBe(.08)
158})
159
160test('legacy body factories keep their original methods and attached/separated phases', () => {
161 expect(Object.keys(createRigidBody())).toEqual(['snapshot', 'step'])
162 const legacy = createRigidBody({ carriedPointLoad: { id: 'parcel-a', massKg: .1, forwardM: .08 } })
163 expect(Object.keys(legacy)).toEqual(['snapshot', 'step', 'loadSnapshot', 'separateCarriedLoad'])
164 expect(legacy.loadSnapshot().properties.phase).toBe('attached')
165 expect(legacy.separateCarriedLoad().outcome).toBe('separated')
166 expect(legacy.loadSnapshot().properties.phase).toBe('separated')
167})