Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
/precision reports the relay→Pi and Pi read-back hops on a clock the relay aligns with the arm server #493
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/481-precision-reports-the-relay-pi-and-pi-re → main 2 commits fork
13 files
changed +245
−14
Software/so101/bench/precision.ts +10 −1
| @@ -11,6 +11,7 @@ import { tmpdir } from "node:os"; | ||
| 11 | 11 | import { join } from "node:path"; |
| 12 | 12 | import WebSocket from "ws"; |
| 13 | 13 | import { FeetechBus } from "../src/bus.ts"; |
| 14 | import { wallMs } from "../src/clock.ts"; | |
| 14 | 15 | import { FakeBus } from "../src/fake-bus.ts"; |
| 15 | 16 | import type { Card } from "../src/precision.ts"; |
| 16 | 17 | import type { Scorecard } from "../src/precision-history.ts"; |
| @@ -58,13 +59,21 @@ export async function bench(script: readonly Step[] = gesture()): Promise<Card> | ||
| 58 | 59 | try { |
| 59 | 60 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); |
| 60 | 61 | await new Promise((r, j) => (ws.once("open", r), ws.once("error", j))); |
| 62 | // Loopback is its own clock: the stamp is the send time, and a watcher reports each status's age as the relay would. | |
| 63 | const watch = new WebSocket(`ws://127.0.0.1:${server.port}/watch`); | |
| 64 | await new Promise((r, j) => (watch.once("open", r), watch.once("error", j))); | |
| 65 | watch.on("message", (data) => { | |
| 66 | const { readAt } = JSON.parse(String(data)) as { readAt?: number }; | |
| 67 | if (typeof readAt === "number") watch.send(JSON.stringify({ readBack: [wallMs() - readAt] })); | |
| 68 | }); | |
| 61 | 69 | const start = performance.now(); |
| 62 | 70 | for (const step of script) { |
| 63 | 71 | const wait = step.at - (performance.now() - start); |
| 64 | 72 | if (wait > 0) await new Promise((r) => setTimeout(r, wait)); |
| 65 | ws.send(JSON.stringify(step.joints)); | |
| 73 | ws.send(JSON.stringify({ ...step.joints, sentAt: wallMs() })); | |
| 66 | 74 | } |
| 67 | 75 | await new Promise((r) => setTimeout(r, 300)); |
| 76 | watch.close(); | |
| 68 | 77 | ws.close(1000); |
| 69 | 78 | await new Promise((r) => ws.once("close", r)); |
| 70 | 79 | await new Promise((r) => setTimeout(r, 100)); |
Software/so101/src/arm-link.test.ts +1 −1
| @@ -5,7 +5,7 @@ import { ArmLink, parseStatus, type LinkState, type LinkTimings } from "./arm-li | ||
| 5 | 5 | |
| 6 | 6 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); |
| 7 | 7 | /** The issue's timings at 1/100: retries after 5, 10, 20, 40 ms, offline after 600 ms (60 s), 10 s down is 100 ms. */ |
| 8 | const FAST: LinkTimings = { backoffMs: [5, 10, 20, 40], staleMs: 60, offlineAfterMs: 600, connectTimeoutMs: 50, pollEveryMs: 10, pingEveryMs: 20 }; | |
| 8 | const FAST: LinkTimings = { backoffMs: [5, 10, 20, 40], staleMs: 60, offlineAfterMs: 600, connectTimeoutMs: 50, pollEveryMs: 10, pingEveryMs: 20, syncEveryMs: 3000 }; | |
| 9 | 9 | const status = (n: number) => ({ joints: [{ id: 1, position: n }], driving: null }); |
| 10 | 10 | |
| 11 | 11 | /** |
Software/so101/src/arm-link.ts +57 −1
| @@ -18,6 +18,8 @@ | ||
| 18 | 18 | |
| 19 | 19 | import type WebSocket from "ws"; |
| 20 | 20 | import { whyNot } from "./egress.ts"; |
| 21 | import { wallMs } from "./clock.ts"; | |
| 22 | import { bestSync, SYNC_WINDOW, type Exchange } from "./hops.ts"; | |
| 21 | 23 | import type { LiveMetrics } from "./live-metrics.ts"; |
| 22 | 24 | import { parseEntries, type MachineEntry } from "./machine-log.ts"; |
| 23 | 25 | |
| @@ -36,9 +38,11 @@ export interface LinkTimings { | ||
| 36 | 38 | pollEveryMs: number; |
| 37 | 39 | /** How often the watch is pinged, to time the round trip. */ |
| 38 | 40 | pingEveryMs: number; |
| 41 | /** How often the clock is aligned with the arm server's (the four-timestamp ping of src/hops.ts), and the read-back hops are reported to it. */ | |
| 42 | syncEveryMs: number; | |
| 39 | 43 | } |
| 40 | 44 | |
| 41 | export const DEFAULT_TIMINGS: LinkTimings = { backoffMs: [500, 1000, 2000, 4000], staleMs: 2000, offlineAfterMs: 60_000, connectTimeoutMs: 1500, pollEveryMs: 250, pingEveryMs: 1000 }; | |
| 45 | export const DEFAULT_TIMINGS: LinkTimings = { backoffMs: [500, 1000, 2000, 4000], staleMs: 2000, offlineAfterMs: 60_000, connectTimeoutMs: 1500, pollEveryMs: 250, pingEveryMs: 1000, syncEveryMs: 3000 }; | |
| 42 | 46 | |
| 43 | 47 | /** How the link is doing, measured: null until there is a measurement. */ |
| 44 | 48 | export interface LinkStats { |
| @@ -68,6 +72,21 @@ export interface ArmLinkOptions { | ||
| 68 | 72 | |
| 69 | 73 | export type Status = Record<string, unknown>; |
| 70 | 74 | |
| 75 | /** The arm server's answer to a clock ping: {"pong": {t0, t1, t2}}, all finite numbers; null for anything else. */ | |
| 76 | export function parsePong(text: string): { t0: number; t1: number; t2: number } | null { | |
| 77 | try { | |
| 78 | const pong = (JSON.parse(text) as { pong?: Record<string, unknown> } | null)?.pong; | |
| 79 | if (!pong || typeof pong !== "object") return null; | |
| 80 | const { t0, t1, t2 } = pong; | |
| 81 | return [t0, t1, t2].every((v) => typeof v === "number" && Number.isFinite(v)) ? { t0: t0 as number, t1: t1 as number, t2: t2 as number } : null; | |
| 82 | } catch { | |
| 83 | return null; | |
| 84 | } | |
| 85 | } | |
| 86 | ||
| 87 | /** The most read-back hops reported to the arm server at once: the watch takes 1 KiB a message. */ | |
| 88 | const MAX_REPORT = 60; | |
| 89 | ||
| 71 | 90 | const reason = (err: unknown) => { |
| 72 | 91 | const e = err as { message?: string; cause?: { code?: string }; code?: string }; |
| 73 | 92 | return e?.cause?.code ?? e?.code ?? e?.message ?? String(err); |
| @@ -108,6 +127,9 @@ export class ArmLink { | ||
| 108 | 127 | downSince = Date.now(); |
| 109 | 128 | readonly ready: Promise<void>; |
| 110 | 129 | private rttMs: number | null = null; |
| 130 | private readonly exchanges: Exchange[] = []; | |
| 131 | private readBack: number[] = []; | |
| 132 | private syncer: ReturnType<typeof setInterval> | undefined; | |
| 111 | 133 | private reconnects = 0; |
| 112 | 134 | private everLive = false; |
| 113 | 135 | private readonly arrivals: number[] = []; |
| @@ -138,6 +160,7 @@ export class ArmLink { | ||
| 138 | 160 | clearTimeout(this.stale); |
| 139 | 161 | clearTimeout(this.offline); |
| 140 | 162 | clearInterval(this.pinger); |
| 163 | clearInterval(this.syncer); | |
| 141 | 164 | this.ws?.terminate(); |
| 142 | 165 | this.ws = null; |
| 143 | 166 | this.settle(); |
| @@ -168,6 +191,10 @@ export class ArmLink { | ||
| 168 | 191 | pingedAt = performance.now(); |
| 169 | 192 | ws.ping(); |
| 170 | 193 | }, this.t.pingEveryMs); |
| 194 | this.exchanges.length = 0; | |
| 195 | clearInterval(this.syncer); | |
| 196 | this.syncer = setInterval(() => this.sync(ws), this.t.syncEveryMs); | |
| 197 | this.sync(ws); | |
| 171 | 198 | ws.on("pong", () => { |
| 172 | 199 | if (pingedAt) this.round(performance.now() - pingedAt); |
| 173 | 200 | }); |
| @@ -175,11 +202,20 @@ export class ArmLink { | ||
| 175 | 202 | const text = String(data); |
| 176 | 203 | const body = parseStatus(text); |
| 177 | 204 | if (!body) { |
| 205 | const pong = parsePong(text); | |
| 206 | if (pong) { | |
| 207 | this.exchanges.push({ ...pong, t3: wallMs() }); | |
| 208 | if (this.exchanges.length > SYNC_WINDOW) this.exchanges.shift(); | |
| 209 | return; | |
| 210 | } | |
| 178 | 211 | const machine = this.o.onMachine && parseMachine(text); |
| 179 | 212 | if (machine) this.o.onMachine!(machine.entries, machine.run); |
| 180 | 213 | return; |
| 181 | 214 | } |
| 182 | 215 | wait(); |
| 216 | // The arm server's read time, on its clock: how long the status took to get here is that against ours, aligned. | |
| 217 | const now = typeof body.readAt === "number" ? this.armNow() : null; | |
| 218 | if (now !== null && this.readBack.length < 600) this.readBack.push(Number((now - (body.readAt as number)).toFixed(2))); | |
| 183 | 219 | this.seen(body); |
| 184 | 220 | }); |
| 185 | 221 | ws.on("error", (err) => ((why = reason(err)), ws.terminate())); |
| @@ -188,6 +224,7 @@ export class ArmLink { | ||
| 188 | 224 | this.ws = null; |
| 189 | 225 | clearTimeout(this.stale); |
| 190 | 226 | clearInterval(this.pinger); |
| 227 | clearInterval(this.syncer); | |
| 191 | 228 | this.lost(why); |
| 192 | 229 | }); |
| 193 | 230 | } |
| @@ -197,6 +234,25 @@ export class ArmLink { | ||
| 197 | 234 | this.o.metrics?.observe("pi_rtt_ms", ms); |
| 198 | 235 | } |
| 199 | 236 | |
| 237 | /** The arm server's clock minus this one's, ms, from the best of the last few pings; null until one is answered. */ | |
| 238 | get offsetMs(): number | null { | |
| 239 | return bestSync(this.exchanges)?.offsetMs ?? null; | |
| 240 | } | |
| 241 | ||
| 242 | /** This moment on the arm server's clock; null until the offset is known. */ | |
| 243 | armNow(): number | null { | |
| 244 | const offset = this.offsetMs; | |
| 245 | return offset === null ? null : wallMs() + offset; | |
| 246 | } | |
| 247 | ||
| 248 | private sync(ws: WebSocket) { | |
| 249 | if (ws.readyState !== ws.OPEN) return; | |
| 250 | ws.send(JSON.stringify({ ping: wallMs() })); | |
| 251 | // The newest of what was seen since the last report: a sample of the hop, not all of it. | |
| 252 | if (this.readBack.length) ws.send(JSON.stringify({ readBack: this.readBack.slice(-MAX_REPORT) })); | |
| 253 | this.readBack = []; | |
| 254 | } | |
| 255 | ||
| 200 | 256 | private async poll() { |
| 201 | 257 | try { |
| 202 | 258 | const asked = performance.now(); |
Software/so101/src/clock.ts added +2
| @@ -0,0 +1,2 @@ | ||
| 1 | /** Wall-clock milliseconds with sub-millisecond resolution: what the relay and the arm server stamp hops with. */ | |
| 2 | export const wallMs = (): number => performance.timeOrigin + performance.now(); |
Software/so101/src/command-gate.ts +10
| @@ -53,6 +53,16 @@ export function parseCommand(text: string): Command | null { | ||
| 53 | 53 | return { seq: stamped ? (seq as number) : null, t: stamped ? (t as number) : null, epoch: Number.isSafeInteger(epoch) && (epoch as number) >= 0 ? (epoch as number) : null, joints }; |
| 54 | 54 | } |
| 55 | 55 | |
| 56 | /** When the relay sent the frame, ms on the receiver's clock, or null when it did not say (the CLI leader's frames) or the frame is not one. */ | |
| 57 | export function sentAtOf(text: string): number | null { | |
| 58 | try { | |
| 59 | const { sentAt } = JSON.parse(text) as { sentAt?: unknown }; | |
| 60 | return typeof sentAt === "number" && Number.isFinite(sentAt) ? sentAt : null; | |
| 61 | } catch { | |
| 62 | return null; | |
| 63 | } | |
| 64 | } | |
| 65 | ||
| 56 | 66 | /** Whether the frame says it is the last of a gesture: the target the arm is to land on before the stream closes. */ |
| 57 | 67 | export function isRelease(text: string): boolean { |
| 58 | 68 | try { |
Software/so101/src/frame-limit.test.ts +1 −1
| @@ -11,7 +11,7 @@ import { BURST, FLOOD_CLOSE, FLOOD_MS, FrameRate, frameBytes, MAX_FRAME_BYTES, T | ||
| 11 | 11 | import { LOST_CLOSE, parseRemote, startRelay, type RunningRelay } from "./relay.ts"; |
| 12 | 12 | import { startServer, type RunningServer } from "./server.ts"; |
| 13 | 13 | /** A frame as the arm sees it, without the STOP epoch the relay stamps on it. */ |
| 14 | const unstamped = (frame: string) => JSON.stringify(Object.fromEntries(Object.entries(JSON.parse(frame) as object).filter(([key]) => key !== "epoch"))); | |
| 14 | const unstamped = (frame: string) => JSON.stringify(Object.fromEntries(Object.entries(JSON.parse(frame) as object).filter(([key]) => key !== "epoch" && key !== "sentAt"))); | |
| 15 | 15 | |
| 16 | 16 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); |
| 17 | 17 | const shut = (s: { close(): Promise<void> } | undefined) => (s ? Promise.race([s.close(), sleep(500)]) : undefined); |
Software/so101/src/precision-history.test.ts +38
| @@ -5,6 +5,8 @@ import { join } from "node:path"; | ||
| 5 | 5 | import WebSocket from "ws"; |
| 6 | 6 | import { FeetechBus } from "./bus.ts"; |
| 7 | 7 | import { FakeBus } from "./fake-bus.ts"; |
| 8 | import { wallMs } from "./clock.ts"; | |
| 9 | import { READ_BACK_HOP, RELAY_HOP } from "./precision.ts"; | |
| 8 | 10 | import { deployedCommit, PrecisionHistory, type Scorecard } from "./precision-history.ts"; |
| 9 | 11 | import { startServer, type RunningServer } from "./server.ts"; |
| 10 | 12 | |
| @@ -123,6 +125,42 @@ describe("GET /precision on the arm server", () => { | ||
| 123 | 125 | expect(after.window.trackingP95Deg).not.toBeNull(); |
| 124 | 126 | }); |
| 125 | 127 | |
| 128 | test("stamped frames time relay→Pi, unstamped ones are accepted and not counted, and the watch answers the clock ping and takes the read-back hop", async () => { | |
| 129 | const hops = async () => (await card()).window.hops; | |
| 130 | const before = (await hops())[RELAY_HOP]!.n; | |
| 131 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); | |
| 132 | await new Promise((r) => ws.once("open", r)); | |
| 133 | ws.send(JSON.stringify({ "1": 2000 })); // the CLI leader's: no stamp | |
| 134 | await sleep(60); | |
| 135 | expect((await hops())[RELAY_HOP]!.n).toBe(before); | |
| 136 | ws.send(JSON.stringify({ "1": 2010, sentAt: wallMs() - 7 })); | |
| 137 | ws.send(JSON.stringify({ "1": 2020, sentAt: wallMs() + 5000 })); // a clock that disagrees: not counted | |
| 138 | await sleep(60); | |
| 139 | const relayHop = (await hops())[RELAY_HOP]!; | |
| 140 | expect(relayHop.n).toBe(before + 1); | |
| 141 | expect(relayHop.p50).toBeGreaterThanOrEqual(7); | |
| 142 | expect(relayHop.p50).toBeLessThan(200); | |
| 143 | ws.close(); | |
| 144 | ||
| 145 | const watch = new WebSocket(`ws://127.0.0.1:${server.port}/watch`); | |
| 146 | const heard: Array<Record<string, any>> = []; | |
| 147 | watch.on("message", (d) => heard.push(JSON.parse(String(d)))); | |
| 148 | await new Promise((r) => watch.once("open", r)); | |
| 149 | const t0 = wallMs(); | |
| 150 | watch.send(JSON.stringify({ ping: t0 })); | |
| 151 | watch.send(JSON.stringify({ readBack: [2, 4, -1, "x", 6] })); | |
| 152 | watch.send("not json"); | |
| 153 | await sleep(100); | |
| 154 | const pong = heard.find((m) => m.pong)!.pong; | |
| 155 | expect(pong.t0).toBe(t0); | |
| 156 | expect(pong.t2).toBeGreaterThanOrEqual(pong.t1); | |
| 157 | expect(pong.t1).toBeGreaterThanOrEqual(t0); | |
| 158 | expect(heard.find((m) => m.joints)!.readAt).toBeGreaterThanOrEqual(t0 - 1000); | |
| 159 | const back = (await hops())[READ_BACK_HOP]!; | |
| 160 | expect(back).toMatchObject({ n: 3, p50: 4, p95: 6 }); | |
| 161 | watch.close(); | |
| 162 | }); | |
| 163 | ||
| 126 | 164 | test("a turn that ends mid-move (the relay's /stop with hold: false) is a turn-end snap-back, not a STOP, and leaves the arm free", async () => { |
| 127 | 165 | const before = (await card()).window.snapBacks.byCause; |
| 128 | 166 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); |
Software/so101/src/precision-history.ts +8
| @@ -86,6 +86,14 @@ export class PrecisionHistory { | ||
| 86 | 86 | return this.; |
| 87 | 87 | } |
| 88 | 88 | |
| 89 | /** A link hop: in the session being driven when there is one, else in the rolling window alone. */ | |
| 90 | hop(name: string, ms: number): boolean { | |
| 91 | const at = this.(); | |
| 92 | if (this.) return this..hop(name, ms, at); | |
| 93 | const probe = new Tracker(at, [this.]); | |
| 94 | return probe.hop(name, ms, at); | |
| 95 | } | |
| 96 | ||
| 89 | 97 | /** A snap-back the page saw: in the session being driven when there is one, else in the rolling window alone (a reconnect comes after the stream). */ |
| 90 | 98 | pageSnapBack(cause: Cause, sizeDeg: number): void { |
| 91 | 99 | const at = this.(); |
Software/so101/src/precision.test.ts +20 −1
| @@ -1,5 +1,5 @@ | ||
| 1 | 1 | import { describe, expect, test } from "bun:test"; |
| 2 | import { ARRIVE_TICKS, CAUSES, emptyRecords, percentile, SETTLE_MS, summarize, trim, Tracker } from "./precision.ts"; | |
| 2 | import { ARRIVE_TICKS, CAUSES, emptyRecords, MAX_HOP_MS, READ_BACK_HOP, RELAY_HOP, percentile, SETTLE_MS, summarize, trim, Tracker } from "./precision.ts"; | |
| 3 | 3 | |
| 4 | 4 | describe("percentile", () => { |
| 5 | 5 | test("nearest rank, unsorted input, none for an empty list", () => { |
| @@ -97,6 +97,25 @@ describe("bus to motion", () => { | ||
| 97 | 97 | }); |
| 98 | 98 | }); |
| 99 | 99 | |
| 100 | describe("link hops", () => { | |
| 101 | test("relay→Pi and read-back→relay get p50/p95 beside bus→move, and latencyP95Ms adds all three; a negative or absurd one is not counted", () => { | |
| 102 | const t = new Tracker(0); | |
| 103 | for (const ms of [4, 6, 8, 10]) expect(t.hop(RELAY_HOP, ms, 100)).toBe(true); | |
| 104 | expect(t.hop(READ_BACK_HOP, 3, 100)).toBe(true); | |
| 105 | expect(t.hop(RELAY_HOP, -1, 100)).toBe(false); | |
| 106 | expect(t.hop(RELAY_HOP, Number.NaN, 100)).toBe(false); | |
| 107 | expect(t.hop(RELAY_HOP, MAX_HOP_MS + 1, 100)).toBe(false); | |
| 108 | t.sample(1, 2000, 0); | |
| 109 | t.wrote(1, 10); | |
| 110 | t.sample(1, 2010, 35); | |
| 111 | const card = summarize(t.records); | |
| 112 | expect(card.hops[RELAY_HOP]).toMatchObject({ n: 4, p50: 6, p95: 10 }); | |
| 113 | expect(card.hops[READ_BACK_HOP]).toMatchObject({ n: 1, p95: 3 }); | |
| 114 | expect(card.latencyP95Ms).toBe(10 + 3 + 25); | |
| 115 | expect(summarize(t.records, 101).hops[RELAY_HOP]!.n).toBe(0); | |
| 116 | }); | |
| 117 | }); | |
| 118 | ||
| 100 | 119 | describe("snap-backs", () => { |
| 101 | 120 | test("every listed cause is counted with that cause, per 100 releases", () => { |
| 102 | 121 | const t = new Tracker(0); |
Software/so101/src/precision.ts +26 −4
| @@ -7,7 +7,9 @@ | ||
| 7 | 7 | * ARRIVE_TICKS; then, for SETTLE_MS: the overshoot past the target, the settling time (until the |
| 8 | 8 | * last entry into the band) and the oscillations (each exit and re-entry is one) |
| 9 | 9 | * snap-backs each time the arm is kept from the driver's target without the driver doing it, with its cause |
| 10 | * latency, by hop for now the arm server's own: a Goal_Position write to the first read-back past MOVE_TICKS | |
| 10 | * latency, by hop the arm server's own (a Goal_Position write to the first read-back past MOVE_TICKS), and the two | |
| 11 | * the relay times on a clock it aligns with the arm server's (src/hops.ts): relay→Pi, from the stamp on | |
| 12 | * a forwarded command, and read-back→relay, from the read time on a status | |
| 11 | 13 | * |
| 12 | 14 | * One `Tracker` per drive session; `summarize` folds any span of its records into the same numbers, which is how the |
| 13 | 15 | * rolling window and the stored session rows are made. |
| @@ -44,6 +46,11 @@ export function parseSnapBack(body: unknown): { cause: Cause; sizeDeg: number } | ||
| 44 | 46 | return { cause, sizeDeg: Number(sizeDeg.toFixed(2)) }; |
| 45 | 47 | } |
| 46 | 48 | |
| 49 | /** The hops timed across the link, by name: a duration is only counted when it is from 0 to MAX_HOP_MS. */ | |
| 50 | export const RELAY_HOP = "relay→Pi"; | |
| 51 | export const READ_BACK_HOP = "read-back→relay"; | |
| 52 | export const MAX_HOP_MS = 60_000; | |
| 53 | ||
| 47 | 54 | export type Kind = "error" | "arrive" | "overshoot" | "settle" | "busToMove"; |
| 48 | 55 | /** Timestamped samples, by joint and kind: [at, value]. */ |
| 49 | 56 | export interface Records { |
| @@ -51,9 +58,11 @@ export interface Records { | ||
| 51 | 58 | snaps: Array<{ at: number; cause: Cause; id: number | null; sizeDeg: number | null }>; |
| 52 | 59 | releases: number[]; |
| 53 | 60 | oscillations: Array<readonly [number, number]>; |
| 61 | /** Link hops (RELAY_HOP, READ_BACK_HOP): [at, ms]. */ | |
| 62 | hops: Map<string, Array<readonly [number, number]>>; | |
| 54 | 63 | } |
| 55 | 64 | |
| 56 | export const emptyRecords = (): Records => ({ joints: new Map(), snaps: [], releases: [], oscillations: [] }); | |
| 65 | export const emptyRecords = (): Records => ({ joints: new Map(), snaps: [], releases: [], oscillations: [], hops: new Map() }); | |
| 57 | 66 | |
| 58 | 67 | /** Nearest-rank percentile of unsorted values; null for none. */ |
| 59 | 68 | export function percentile(values: readonly number[], p: number): number | null { |
| @@ -94,7 +103,7 @@ export interface Card { | ||
| 94 | 103 | trackingP95Deg: number | null; |
| 95 | 104 | arriveP95Ms: number | null; |
| 96 | 105 | snapBacksPer100Releases: number | null; |
| 97 | /** End to end: the sum of every measured hop's p95. Only the arm server's own hop is measured so far. */ | |
| 106 | /** End to end: the sum of every measured hop's p95. */ | |
| 98 | 107 | latencyP95Ms: number | null; |
| 99 | 108 | /** The joint with the highest p95 tracking error. */ |
| 100 | 109 | worstJoint: string | null; |
| @@ -135,7 +144,7 @@ export function summarize(records: Records, since = -Infinity): Card { | ||
| 135 | 144 | for (const s of snaps) byCause[s.cause]++; |
| 136 | 145 | const releases = records.releases.filter((at) => at >= since).length; |
| 137 | 146 | const worst = joints.filter((j) => j.errorDeg.p95 !== null).sort((a, b) => b.errorDeg.p95! - a.errorDeg.p95!)[0]; |
| 138 | const hops = { "bus→move": spread(allBus, 0) }; | |
| 147 | const hops: Record<string, Spread> = { [RELAY_HOP]: spread(after(records.hops.get(RELAY_HOP) ?? []), 2), [READ_BACK_HOP]: spread(after(records.hops.get(READ_BACK_HOP) ?? []), 2), "bus→move": spread(allBus, 0) }; | |
| 139 | 148 | const measured = Object.values(hops).filter((h) => h.p95 !== null); |
| 140 | 149 | return { |
| 141 | 150 | trackingP95Deg: round(percentile(allErrors.map((t) => t / TICKS_PER_DEGREE), 95)), |
| @@ -161,6 +170,7 @@ export function trim(records: Records, since: number): void { | ||
| 161 | 170 | keep(records.snaps, (s) => s.at); |
| 162 | 171 | keep(records.releases, (at) => at); |
| 163 | 172 | keep(records.oscillations, ([at]) => at); |
| 173 | for (const list of records.hops.values()) keep(list, ([at]) => at); | |
| 164 | 174 | } |
| 165 | 175 | |
| 166 | 176 | interface Joint { |
| @@ -288,6 +298,18 @@ export class Tracker { | ||
| 288 | 298 | for (const records of [this.records, ...this.]) records.snaps.push({ at, cause, id: null, sizeDeg }); |
| 289 | 299 | } |
| 290 | 300 | |
| 301 | /** One crossing of a link hop, ms on the aligned clock; a negative or absurd one (the clocks disagree) is not counted. */ | |
| 302 | hop(name: string, ms: number, at: number): boolean { | |
| 303 | if (typeof ms !== "number" || !Number.isFinite(ms) || ms < 0 || ms > MAX_HOP_MS) return false; | |
| 304 | for (const records of [this.records, ...this.]) { | |
| 305 | let list = records.hops.get(name); | |
| 306 | if (!list) records.hops.set(name, (list = [])); | |
| 307 | list.push([at, ms]); | |
| 308 | if (list.length > KEEP) list.shift(); | |
| 309 | } | |
| 310 | return true; | |
| 311 | } | |
| 312 | ||
| 291 | 313 | /** The driver let go: a dead-man hold, or the end of the stream. */ |
| 292 | 314 | release(at: number): void { |
| 293 | 315 | for (const records of [this.records, ...this.]) records.releases.push(at); |
Software/so101/src/relay.test.ts +40 −1
| @@ -8,13 +8,14 @@ import { gunzipSync } from "node:zlib"; | ||
| 8 | 8 | import WebSocket, { WebSocketServer } from "ws"; |
| 9 | 9 | import { FeetechBus } from "./bus.ts"; |
| 10 | 10 | import { CameraHub } from "./cameras.ts"; |
| 11 | import { wallMs } from "./clock.ts"; | |
| 11 | 12 | import { stamper } from "./command-gate.ts"; |
| 12 | 13 | import { FakeBus } from "./fake-bus.ts"; |
| 13 | 14 | import { OFFLINE_FRAME } from "./offline-frame.ts"; |
| 14 | 15 | import { parseRemote, sameOrigin, startRelay, type Remote, type RunningRelay } from "./relay.ts"; |
| 15 | 16 | import { startServer, type RunningServer } from "./server.ts"; |
| 16 | 17 | /** A frame as the arm sees it, without the STOP epoch the relay stamps on it. */ |
| 17 | const unstamped = (frame: string) => JSON.stringify(Object.fromEntries(Object.entries(JSON.parse(frame) as object).filter(([key]) => key !== "epoch"))); | |
| 18 | const unstamped = (frame: string) => JSON.stringify(Object.fromEntries(Object.entries(JSON.parse(frame) as object).filter(([key]) => key !== "epoch" && key !== "sentAt"))); | |
| 18 | 19 | |
| 19 | 20 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); |
| 20 | 21 | // Bun's node:http can keep a server-closed connection from ending (see server.test.ts): cap every close. |
| @@ -408,6 +409,44 @@ describe("relay", () => { | ||
| 408 | 409 | expect((await fetch(`${base}/move`, { method: "POST", body: '{"targets":{"1":2000}}' })).status).toBe(200); |
| 409 | 410 | }); |
| 410 | 411 | |
| 412 | test("the relay aligns its clock with an arm server 5 s ahead: stamps its frames on that clock and reports the read-back hop", async () => { | |
| 413 | const SKEW = 5000; | |
| 414 | const armClock = () => wallMs() + SKEW; | |
| 415 | const frames: Array<{ sentAt?: number; arrived: number }> = []; | |
| 416 | const reports: number[] = []; | |
| 417 | const wss = new WebSocketServer({ noServer: true }); | |
| 418 | camera = createServer((_req, res) => res.writeHead(404).end()); | |
| 419 | camera.on("upgrade", (req, socket, head) => | |
| 420 | wss.handleUpgrade(req, socket, head, (ws) => { | |
| 421 | ws.on("message", (d) => { | |
| 422 | const body = JSON.parse(String(d)) as { ping?: number; readBack?: number[]; sentAt?: number }; | |
| 423 | if (req.url === "/follow") return void frames.push({ sentAt: body.sentAt, arrived: armClock() }); | |
| 424 | if (typeof body.ping === "number") ws.send(JSON.stringify({ pong: { t0: body.ping, t1: armClock(), t2: armClock() } })); | |
| 425 | if (body.readBack) reports.push(...body.readBack); | |
| 426 | }); | |
| 427 | }), | |
| 428 | ); | |
| 429 | const pump = setInterval(() => wss.clients.forEach((c) => c.send(JSON.stringify({ readAt: armClock() - 2, joints: [] }))), 10); | |
| 430 | try { | |
| 431 | relay = await startRelay({ port: 0, simulation: simBase, remote: remoteAt(await listening(camera)), link: { ...FAST, staleMs: 500, syncEveryMs: 20 } }); | |
| 432 | const ws = new WebSocket(`ws://127.0.0.1:${relay.port}/follow`); | |
| 433 | await new Promise((r, j) => (ws.once("open", r), ws.once("error", j))); | |
| 434 | await sleep(150); | |
| 435 | ws.send(stamper()({ 1: 2050 })); | |
| 436 | await sleep(50); | |
| 437 | expect(frames).toHaveLength(1); | |
| 438 | const hop = frames[0]!.arrived - frames[0]!.sentAt!; | |
| 439 | expect(hop).toBeGreaterThanOrEqual(-1); // the skew is gone: a hop of a few ms, not 5000 | |
| 440 | expect(hop).toBeLessThan(50); | |
| 441 | expect(reports.length).toBeGreaterThan(0); | |
| 442 | for (const ms of reports) expect(Math.abs(ms - 2)).toBeLessThan(50); // read-back→relay: the 2 ms the fake aged its read, plus a hop | |
| 443 | ws.close(); | |
| 444 | } finally { | |
| 445 | clearInterval(pump); | |
| 446 | for (const c of wss.clients) c.terminate(); | |
| 447 | } | |
| 448 | }); | |
| 449 | ||
| 411 | 450 | test("a stream on the real arm goes quiet while the link reconnects, and flows again once it is back", async () => { |
| 412 | 451 | // An arm server whose /watch can be silenced while its /follow stays up: what the relay passes on is counted. |
| 413 | 452 | let watching = true; |
Software/so101/src/relay.ts +6 −1
| @@ -61,6 +61,7 @@ import { Readable } from "node:stream"; | ||
| 61 | 61 | import { pipeline } from "node:stream/promises"; |
| 62 | 62 | import WebSocket, { WebSocketServer } from "ws"; |
| 63 | 63 | import { LiveMetrics } from "./live-metrics.ts"; |
| 64 | import { wallMs } from "./clock.ts"; | |
| 64 | 65 | import { ArmLink, type LinkTimings } from "./arm-link.ts"; |
| 65 | 66 | import { MachineFeed } from "./machine-feed.ts"; |
| 66 | 67 | import { refusalBatch } from "./refusals.ts"; |
| @@ -891,7 +892,11 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 891 | 892 | const values = (text: string) => perServo(parseFrame(text) ?? []); |
| 892 | 893 | const rate = new FrameRate(); |
| 893 | 894 | /** A frame as it goes to the arm server: carrying the STOP epoch it was sent under. */ |
| 894 | const stamped = (frame: string, release: boolean) => JSON.stringify({ ...(JSON.parse(frame) as object), ...(release ? { type: "release" } : {}), epoch }); | |
| 895 | const stamped = (frame: string, release: boolean) => { | |
| 896 | // `sentAt` is the send time on the arm server's clock (ours plus the offset the link keeps), so it can time the hop; none until the offset is known. | |
| 897 | const sentAt = real ? armLink?.armNow() ?? null : wallMs(); | |
| 898 | return JSON.stringify({ ...(JSON.parse(frame) as object), ...(release ? { type: "release" } : {}), epoch, ...(sentAt === null ? {} : { sentAt: Number(sentAt.toFixed(3)) }) }); | |
| 899 | }; | |
| 895 | 900 | /** Set when this relay ends the stream for breaking the limits: the arm then holds, as for a stream closed on purpose. */ |
| 896 | 901 | let overLimit = false; |
| 897 | 902 | browser.on("message", (raw) => { |
Software/so101/src/server.ts +26 −3
| @@ -73,7 +73,7 @@ import { fileURLToPath } from "node:url"; | ||
| 73 | 73 | import { ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts"; |
| 74 | 74 | import { WebSocketServer, type WebSocket } from "ws"; |
| 75 | 75 | import { browserModule } from "./browser-modules.ts"; |
| 76 | import { Clock, CommandGate, HELD_CLOSE, isRelease } from "./command-gate.ts"; | |
| 76 | import { Clock, CommandGate, HELD_CLOSE, isRelease, sentAtOf } from "./command-gate.ts"; | |
| 77 | 77 | import { FLOOD_CLOSE, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts"; |
| 78 | 78 | import type { FeetechBus } from "./bus.ts"; |
| 79 | 79 | import { ArmModel, gripperTicksToRadians, ticksToRadians, type Vec3, type Violation } from "./kinematics.ts"; |
| @@ -81,7 +81,8 @@ import { fitTablePlane, MAX_TILT_DEG } from "./table-plane.ts"; | ||
| 81 | 81 | import { clamp, type Frame } from "./mapping.ts"; |
| 82 | 82 | import { MachineLog, perServo } from "./machine-log.ts"; |
| 83 | 83 | import { Metrics } from "./metrics.ts"; |
| 84 | import { ARRIVE_TICKS, parseSnapBack, type Cause, type Tracker } from "./precision.ts"; | |
| 84 | import { wallMs } from "./clock.ts"; | |
| 85 | import { ARRIVE_TICKS, parseSnapBack, READ_BACK_HOP, RELAY_HOP, type Cause, type Tracker } from "./precision.ts"; | |
| 85 | 86 | import { deployedCommit, PrecisionHistory } from "./precision-history.ts"; |
| 86 | 87 | import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts"; |
| 87 | 88 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; |
| @@ -1429,6 +1430,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1429 | 1430 | return end(ws, FLOOD_CLOSE, "too many frames: the stream was closed"); |
| 1430 | 1431 | } |
| 1431 | 1432 | if (pace === "drop") return void stats.gated++; |
| 1433 | // A frame the relay stamped says when it was sent, on this server's clock (the relay aligns it): relay→Pi. The CLI leader's has no stamp and is not counted. | |
| 1434 | const sentAt = sentAtOf(String(data)); | |
| 1435 | if (sentAt !== null) tracker.hop(RELAY_HOP, wallMs() - sentAt, Date.now()); | |
| 1432 | 1436 | const verdict = admission.admit(String(data)); |
| 1433 | 1437 | if (!verdict.ok) { |
| 1434 | 1438 | if (verdict.drop !== "malformed") { |
| @@ -1527,18 +1531,33 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1527 | 1531 | const watchQuietMs = options.watchQuietMs ?? 200; |
| 1528 | 1532 | let watched = ""; |
| 1529 | 1533 | let watchedAt = 0; |
| 1534 | /** What a watcher may say: {"ping": t0} is answered with {"pong": {t0, t1, t2}} (the four-timestamp ping, src/hops.ts); {"readBack": [ms…]} is the relay's read-back hop. Anything else is ignored. */ | |
| 1535 | const watcherMessage = (text: string, t1: number): string | null => { | |
| 1536 | let body: { ping?: unknown; readBack?: unknown } | null; | |
| 1537 | try { | |
| 1538 | body = JSON.parse(text); | |
| 1539 | } catch { | |
| 1540 | return null; | |
| 1541 | } | |
| 1542 | if (!body || typeof body !== "object") return null; | |
| 1543 | if (typeof body.ping === "number" && Number.isFinite(body.ping)) return JSON.stringify({ pong: { t0: body.ping, t1, t2: wallMs() } }); | |
| 1544 | if (Array.isArray(body.readBack)) for (const ms of body.readBack.slice(0, 64)) if (typeof ms === "number") precision.hop(READ_BACK_HOP, ms); | |
| 1545 | return null; | |
| 1546 | }; | |
| 1530 | 1547 | const machineFrame = (entries: readonly unknown[]) => JSON.stringify({ machine: entries, run }); |
| 1531 | 1548 | const broadcast = async () => { |
| 1532 | 1549 | if (reading) return; // a slow bus read is not stacked behind another: the next tick asks again |
| 1533 | 1550 | reading = true; |
| 1534 | 1551 | try { |
| 1535 | 1552 | const text = JSON.stringify(await status()); |
| 1553 | // The read time rides on every message, outside the comparison: a status that did not change is still not sent again. | |
| 1554 | const sent = `{"readAt":${wallMs().toFixed(3)},${text.slice(1)}`; | |
| 1536 | 1555 | const now = performance.now(); |
| 1537 | 1556 | const same = text === watched && now - watchedAt < watchQuietMs; |
| 1538 | 1557 | if (!same) [watched, watchedAt] = [text, now]; |
| 1539 | 1558 | for (const w of watchers.clients) { |
| 1540 | 1559 | if (w.readyState !== w.OPEN || (same && !fresh.has(w))) continue; |
| 1541 | w.send(text); | |
| 1560 | w.send(sent); | |
| 1542 | 1561 | // A new watcher's first message is a status; the machine log so far comes right after it. |
| 1543 | 1562 | if (fresh.delete(w)) w.send(machineFrame(machine.recent(200))); |
| 1544 | 1563 | } |
| @@ -1559,6 +1578,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1559 | 1578 | }); |
| 1560 | 1579 | fresh.add(ws); |
| 1561 | 1580 | void broadcast(); |
| 1581 | ws.on("message", (data) => { | |
| 1582 | const reply = watcherMessage(String(data), wallMs()); | |
| 1583 | if (reply) ws.send(reply); | |
| 1584 | }); | |
| 1562 | 1585 | ws.on("error", () => ws.terminate()); |
| 1563 | 1586 | ws.on("close", () => { |
| 1564 | 1587 | if (watchers.clients.size > 0 || !watching) return; |
new above
offline