ReviewOS
Reload

also looking at this

stacks/drone

test: retain owned browser startup process evidence

#100
Merged chris wants to merge codex/browser-startup-process-evidence into main
21 files +1516 -155

Review threads live on the whole diff, not on one commit, so none are shown here - a thread's line means something in the branch's final form, and painting it into an intermediate step would put it on code it is not about.

docs/adr/0036-point-load-intake.mdadded+53-0
@@ -0,0 +1,53 @@
1# ADR 0036: One point-load intake for a receiving vehicle
2
3Status: accepted for software simulation.
4
5## Context
6
7Cargo fleet handover needs a physical parcel to leave one carrier, undergo finite modeled
8station handling and enter another carrier's actual dynamics. A custody-label change or
9an initially loaded second vehicle would bypass that mass transfer. The rigid body already
10supports an attached point load and one zero-ejection separation, but had no empty joint.
11
12## Decision
13
14Add `createReceivingRigidBody` over the same integrator, with a captured `expectedPointLoad`.
15Its lifecycle is `awaiting -> attached -> separated`. The initial body has base mass/inertia;
16the planned load is not an actual parcel. A successful intake consumes this one opportunity.
17The old `createRigidBody` constructor still starts attached and exposes no intake method or
18awaiting phase. Its default/no-load numerical paths and result projections remain unchanged.
19
20The receiving joint reports its position and velocity. The incoming parcel record contains
21its own id, mass, position and velocity. Match id/mass exactly and require exact coincident
22binary64 position components at invocation. This is an ideal programmatic contact condition,
23not a sensor tolerance or claim of physical positioning precision. Captured records and
24finite tuple elements reject accessors. Valid refusal or malformed input does not mutate
25the body or consume its intake opportunity.
26
27Let base mass be `m`, point mass `p`, body-forward joint vector `r`, and combined mass `M=m+p`.
28The new body-frame CG is `c=p*r/M`. Combined center velocity is `(m*vBody+p*vParcel)/M`.
29About that center, body-frame angular momentum is base `I*omega` plus
30`m*p/M * cross(r, inverseRotate(vParcel-vBody))`. Dividing by the combined principal inertia
31gives the new angular rate. Reconstruct datum velocity from center velocity and `omega cross c`.
32The joint is on the body x axis, so the existing diagonal parallel-axis inertia is sufficient.
33
34The base and incoming point jointly determine conserved linear/angular momentum. Structural
35position, quaternion and actual motor state remain unchanged. Kinetic energy may decrease;
36retain before/after energy and raw dissipation. Reject non-finite candidates or a gain beyond
37`64 * Number.EPSILON * max(1, kineticBeforeJ, kineticAfterJ)` before committing state. Keep the
38raw represented loss, including tolerated tiny negatives. NED potential is `-mass*g*down`.
39The event reports both potential values separately.
40
41## Consequences
42
43The primitive models an instantaneous inelastic impulse, not contact duration, force peaks,
44parcel spin/deformation, latch actuation or restitution. The retained acceleration is the last
45continuous integration sample, not an invented impact acceleration. A later physics step uses
46the new mass/CG/inertia without resetting the controller, estimator, pose or motor lag.
47
48World/policy code must own and consume the single parcel, admit the physical operation, model
49finite transport to the joint and capture mechanism/vehicle acknowledgement with current
50authority and source epochs. This primitive does not globally prevent a caller from supplying
51the same record to two independent worlds. A joint pose is not a source parcel or proof of custody.
52Station handling and the actual two-vehicle protocol remain unimplemented at this checkpoint.
53No physical output, authenticated identity, battery endurance or fleet completion is claimed.
docs/api.mdmodified+5-0
@@ -57,6 +57,11 @@ The [cargo guide](cargo-delivery.md) defines the flat synthetic origin, finite l
5757one-release and receipt semantics, phase-bound faults, native CLI exit codes and remaining
5858fleet/physical/BVLOS work. Budgets are declared power ceilings, not electrical measurements.
5959
60`createReceivingRigidBody({ expectedPointLoad, ...bodyOptions })` in `controls/physics.ts`
61provides the separate [point-load intake prerequisite](cargo-delivery.md#point-load-intake-prerequisite).
62It integrates real modeled mass/CG changes after one coincident inelastic point capture.
63The caller still owns the parcel, finite station handling and captured fleet custody protocol.
64
6065## Target sources
6166
6267`createGpsTargetSource({ targetId, origin, maxSpeedMps?, now? })` converts trusted phone or GNSS fixes
docs/cargo-delivery.mdmodified+31-0
@@ -149,8 +149,39 @@ temperature limit, voltage-sag prediction or Pi performance result. Changing mas
149149the actual modeled dynamics; it does not identify an electrical load law. A successful
150150budget is conditional on that ceiling and is not measured endurance qualification.
151151
152## Point-load intake prerequisite
153
154`createReceivingRigidBody` in `controls/physics.ts` adds an empty declared joint for a
155future parcel. Supply `expectedPointLoad: { id, massKg, forwardM }`; the mass and offset
156bounds match the carried-load model. Its `awaiting` phase has the base body's actual
157mass/inertia. `intakePoint()` returns only joint position and velocity, not a parcel or
158custody evidence.
159
160`attachParcel({ id, massKg, positionM, velocityMps })` accepts an existing modeled point
161parcel only at the current joint position, equal component by component. Identity and
162mass must match the declaration. The caller must move its one physical-world parcel
163there; this primitive does not transport, copy or globally own it. Malformed own-data
164records throw without changing the body. Valid foreign or noncoincident parcels are
165refused without consuming the joint.
166
167Capture conserves total linear/angular momentum and applies a perfectly inelastic point
168joint. Structural position, orientation and actual motors remain continuous; a non-co-moving
169parcel can impart a genuine velocity/angular-rate impulse. Captured mass changes the actual
170subsequent dynamics, CG and inertia. The event retains detached before/after state, incoming
171parcel and raw kinetic/potential energy accounting. Dissipation is mechanical energy,
172not battery Wh or an identified mechanism efficiency. Tiny binary64 negative loss within
173the reported numerical tolerance is retained rather than clamped.
174
175The joint accepts once, can later separate through the existing zero-ejection model and
176cannot refill after separation. The legacy `createRigidBody` carried-load interface keeps
177its original phase/method contract. [ADR 0036](adr/0036-point-load-intake.md) gives the model
178and limits. This primitive supplies no finite-duration contact forces or impact IMU sample,
179release/intake acknowledgement, station handling, authorization or fleet custody handover.
180Those integrations remain open under issue #25.
181
152182## BVLOS operations evidence
153183
184
154185Country and intended operation must be established separately from library support.
155186Sources below were checked October 24, 2026; inspect their current requirements for the
156187actual operation. This software issues no authorization.
resources/functions/drone/controls/physics.tsmodified+112-42
@@ -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']>
tests/tsconfig.runtime.jsonmodified+1-0
@@ -23,6 +23,7 @@
2323 "./unit/controls.test.ts",
2424 "../resources/functions/drone/cargo/**/*.ts",
2525 "./unit/cargo-load.test.ts",
26 "./unit/cargo-intake.test.ts",
2627 "./unit/cargo-delivery.test.ts",
2728 "./unit/cargo-delivery-config.test.ts",
2829 "./unit/cargo-delivery-phase-faults.test.ts",
tests/unit/cargo-intake.test.tsadded+167-0
@@ -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})