also looking at this
feat: model one point-load intake with conserved momentum
#99
45 files
+3133
-169
| @@ -8,53 +8,79 @@ export { rotateBodyToNed, rotateNedToBody } from '../estimation/frames' | ||
| 8 | 8 | interface RigidBodyInput { positionM?: SensorVector, motorsN?: Motors, groundDownM?: number, groundGradient?: [number, number] } |
| 9 | 9 | interface CarriedPointLoad { id: string, massKg: number, forwardM: number } |
| 10 | 10 | interface BodySnapshot { positionM: SensorVector, velocityMps: SensorVector, angularRateRadS: SensorVector, accelerationMps2: SensorVector, quaternionWxyz: Quaternion, motorsN: Motors, groundContact: boolean } |
| 11 | interface LoadProperties { id: string, phase: 'attached' | 'separated', pointMassKg: number, forwardM: number, totalMassKg: number, cgBodyM: SensorVector, inertiaDiagonalKgM2: SensorVector } | |
| 12 | interface LoadSnapshot { properties: LoadProperties, centerPositionM: SensorVector, centerVelocityMps: SensorVector } | |
| 13 | interface SeparationSide { properties: LoadProperties, bodyPositionM: SensorVector, bodyVelocityMps: SensorVector, quaternionWxyz: Quaternion, angularRateRadS: SensorVector, actualMotorsN: Motors, centerPositionM: SensorVector, centerVelocityMps: SensorVector } | |
| 11 | type PointLoadPhase = 'awaiting' | 'attached' | 'separated' | |
| 12 | interface LoadProperties<P extends PointLoadPhase = 'attached' | 'separated'> { id: string, phase: P, pointMassKg: number, forwardM: number, totalMassKg: number, cgBodyM: SensorVector, inertiaDiagonalKgM2: SensorVector } | |
| 13 | interface LoadSnapshot<P extends PointLoadPhase = 'attached' | 'separated'> { properties: LoadProperties<P>, centerPositionM: SensorVector, centerVelocityMps: SensorVector } | |
| 14 | interface SeparationSide<P extends PointLoadPhase = 'attached' | 'separated'> { properties: LoadProperties<P>, bodyPositionM: SensorVector, bodyVelocityMps: SensorVector, quaternionWxyz: Quaternion, angularRateRadS: SensorVector, actualMotorsN: Motors, centerPositionM: SensorVector, centerVelocityMps: SensorVector } | |
| 14 | 15 | type LoadSeparation = { outcome: 'separated', reason: 'prescribed-zero-ejection', before: SeparationSide, after: SeparationSide, parcel: { massKg: number, positionM: SensorVector, velocityMps: SensorVector } } | { outcome: 'refused', reason: 'already-separated' } |
| 15 | 16 | interface RigidBody { snapshot(): BodySnapshot, step(command: Motors, dtS: number, disturbance?: { windMps?: SensorVector, forceN?: SensorVector, torqueNm?: SensorVector }): BodySnapshot } |
| 16 | 17 | interface LoadedRigidBody extends RigidBody { loadSnapshot(): LoadSnapshot, separateCarriedLoad(): LoadSeparation } |
| 18 | export interface IntakeParcel { id: string, massKg: number, positionM: SensorVector, velocityMps: SensorVector } | |
| 19 | export 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' } | |
| 27 | export 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 | } | |
| 17 | 33 | function cargoCross(a: SensorVector, b: SensorVector): SensorVector { |
| 18 | 34 | 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]] |
| 19 | 35 | } |
| 36 | function 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 | } | |
| 48 | function 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 | } | |
| 58 | function 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 | } | |
| 20 | 68 | /** New opt-in records are captured as exact own data. The absent-own-member path is deliberately legacy. */ |
| 21 | function captureCarriedPointLoad(input: unknown): { input: RigidBodyInput, load: CarriedPointLoad } | null { | |
| 69 | function captureCarriedPointLoad(input: unknown, member: 'carriedPointLoad' | 'expectedPointLoad' = 'carriedPointLoad'): { input: RigidBodyInput, load: CarriedPointLoad } | null { | |
| 22 | 70 | 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']) | |
| 48 | 74 | 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() |
| 49 | 75 | 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 | |
| 52 | 78 | if (Object.hasOwn(captured, 'groundDownM')) { |
| 53 | 79 | if (typeof captured.groundDownM !== 'number' || !Number.isFinite(captured.groundDownM) || captured.groundDownM < -1 || captured.groundDownM > 0) throw new TypeError() |
| 54 | 80 | body.groundDownM = captured.groundDownM |
| 55 | 81 | } |
| 56 | 82 | 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] | |
| 58 | 84 | if (Math.hypot(...body.groundGradient) > 0.1) throw new TypeError() |
| 59 | 85 | } |
| 60 | 86 | return { input: body, load: { id: load.id, massKg: load.massKg, forwardM: load.forwardM } } |
| @@ -66,6 +92,18 @@ export function createRigidBody(input?: RigidBodyInput): RigidBody | ||
| 66 | 92 | export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: CarriedPointLoad } = {}): RigidBody | LoadedRigidBody { |
| 67 | 93 | const configured = captureCarriedPointLoad(input) |
| 68 | 94 | 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. */ | |
| 100 | export 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 | ||
| 106 | function createBody(input: RigidBodyInput, configured: ReturnType<typeof captureCarriedPointLoad>, receiving: boolean): RigidBody | LoadedRigidBody | ReceivingRigidBody { | |
| 69 | 107 | |
| 70 | 108 | const groundDownM = input.groundDownM ?? 0 |
| 71 | 109 | const groundGradient = [...(input.groundGradient ?? [0, 0])] |
| @@ -78,11 +116,12 @@ export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: Car | ||
| 78 | 116 | let velocityMps: SensorVector = [0, 0, 0], angularRateRadS: SensorVector = [0, 0, 0], accelerationMps2: SensorVector = [0, 0, 0] |
| 79 | 117 | let quaternionWxyz: Quaternion = [1, 0, 0, 0], groundContact = false |
| 80 | 118 | 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' | |
| 83 | 122 | const load = configured!.load, totalMassKg = attached ? model.massKg + load.massKg : model.massKg |
| 84 | 123 | 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] } | |
| 86 | 125 | } |
| 87 | 126 | let centerPositionM: SensorVector | null = configured ? positionM.map((n, i) => n + properties().cgBodyM[i]!) as SensorVector : null |
| 88 | 127 | let centerVelocityMps: SensorVector | null = configured ? [...velocityMps] : null |
| @@ -140,19 +179,50 @@ export function createRigidBody(input: RigidBodyInput & { carriedPointLoad?: Car | ||
| 140 | 179 | }, |
| 141 | 180 | } |
| 142 | 181 | 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> { | |
| 145 | 184 | return { properties: properties(), bodyPositionM: [...positionM], bodyVelocityMps: [...velocityMps], quaternionWxyz: [...quaternionWxyz], angularRateRadS: [...angularRateRadS], actualMotorsN: [...motorsN], centerPositionM: [...centerPositionM!], centerVelocityMps: [...centerVelocityMps!] } |
| 146 | 185 | } |
| 147 | function separateCarriedLoad(...args: unknown[]): LoadSeparation { | |
| 186 | function separateCarriedLoad(...args: unknown[]): LoadSeparation | { outcome: 'refused', reason: 'awaiting-load' } { | |
| 148 | 187 | 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' }) | |
| 150 | 190 | const before = side(), r: SensorVector = [configured!.load.forwardM, 0, 0] |
| 151 | 191 | const positionLever = rotateBodyToNed(quaternionWxyz, r), velocityLever = rotateBodyToNed(quaternionWxyz, cargoCross(angularRateRadS, r)) |
| 152 | 192 | 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 } } | |
| 155 | 225 | } |
| 156 | return { ...body, loadSnapshot, separateCarriedLoad } | |
| 226 | return { ...body, loadSnapshot, separateCarriedLoad, intakePoint, attachParcel } | |
| 157 | 227 | } |
| 158 | 228 | export type RigidBodyState = ReturnType<ReturnType<typeof createRigidBody>['snapshot']> |