Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
bench/real-probe.ts steps one joint a little, reports arrive time, final error and overshoot, refuses while a turn is held, and always restores the start pose #543
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/517-bench-real-probe-ts-steps-one-joint-a-li → main 3 commits fork
3 files
changed +315
−0
Software/README.md +2
| @@ -1105,6 +1105,8 @@ Where to read it: | ||
| 1105 | 1105 | `bun Software/so101/bench/limits-sweep.ts [url]` drives each joint in turn through a fixed set of targets inside its bounds (the others held still) and prints that joint's p95 error, time to arrive, overshoot and snap-backs from `/precision`; with no url it runs on the fake bus, with a url it refuses to start unless nobody is driving and stops at once on STOP or any refusal. |
| 1106 | 1106 | The sweep's targets are fixed; the measured numbers move a little with the machine's timers. |
| 1107 | 1107 | |
| 1108 | `bun run Software/so101/bench/real-probe.ts <url> --joint N [--step ticks]` steps one joint a little (60 ticks by default, toward the middle of its bounds, clamped inside them) over `/follow`, samples `/status` at about 50 Hz and prints the time to arrive within 12 ticks, the final error and the overshoot. It refuses to start unless nobody is driving and there is no STOP or E-stop, stops at once if one appears, and always returns the joint to its start pose, even after an abort. | |
| 1109 | ||
| 1108 | 1110 | `bun run Software/so101/bench/jam-rate.ts <url> [--minutes N] [--events]` reads `/metrics` at the start and end of the window (default 60 min) and prints jams per hour, by joint, and each jam's load, ms and progress; `--events` reports from the recent events alone. It only reads. |
| 1109 | 1111 | |
| 1110 | 1112 | `bun bench/precision-report.ts <url>` (the arm server's URL) reads the hourly roll-up from `GET /precision` and prints |
Software/so101/bench/real-probe.test.ts added +150
| @@ -0,0 +1,150 @@ | ||
| 1 | import { expect, test } from "bun:test"; | |
| 2 | import WebSocket from "ws"; | |
| 3 | import { fakeArm } from "./precision.ts"; | |
| 4 | import { aim, probe } from "./real-probe.ts"; | |
| 5 | ||
| 6 | const position = async (url: string, id: number) => | |
| 7 | ((await (await fetch(`${url}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === id)!.position; | |
| 8 | ||
| 9 | test("aim goes toward the middle and stays inside the bounds", () => { | |
| 10 | expect(aim(1000, 60, 1000, 3000)).toBe(1060); | |
| 11 | expect(aim(3000, 60, 1000, 3000)).toBe(2940); | |
| 12 | expect(aim(1000, 5000, 1000, 3000)).toBe(3000); | |
| 13 | }); | |
| 14 | ||
| 15 | test("a step gives numbers and the joint ends at its start pose", async () => { | |
| 16 | const arm = await fakeArm(); | |
| 17 | try { | |
| 18 | const before = await position(arm.url, 2); | |
| 19 | const p = await probe(arm.url, 2, 80); | |
| 20 | expect(p.start).toBe(before); | |
| 21 | expect(p.target).not.toBe(before); | |
| 22 | expect(p.arriveMs).not.toBeNull(); | |
| 23 | expect(p.finalError).toBeLessThanOrEqual(12); | |
| 24 | expect(p.overshoot).toBeGreaterThanOrEqual(0); | |
| 25 | expect(Math.abs((await position(arm.url, 2)) - before)).toBeLessThanOrEqual(12); | |
| 26 | } finally { | |
| 27 | await arm.close(); | |
| 28 | } | |
| 29 | }, 20_000); | |
| 30 | ||
| 31 | test("it refuses to start while someone drives", async () => { | |
| 32 | const arm = await fakeArm(); | |
| 33 | try { | |
| 34 | const ws = new WebSocket(`${arm.url.replace("http", "ws")}/follow`); | |
| 35 | await new Promise((r) => ws.once("open", r)); | |
| 36 | await expect(probe(arm.url, 2)).rejects.toThrow("is driving the arm"); | |
| 37 | ws.close(1000); | |
| 38 | } finally { | |
| 39 | await arm.close(); | |
| 40 | } | |
| 41 | }, 10_000); | |
| 42 | ||
| 43 | /** Runs fn with /status answers rewritten by change(n, body), n counting from 1; fetch is put back after. */ | |
| 44 | async function withStatus<T>(change: (n: number, body: any) => object, fn: () => Promise<T>): Promise<T> { | |
| 45 | const real = globalThis.fetch; | |
| 46 | let n = 0; | |
| 47 | globalThis.fetch = (async (input: Parameters<typeof fetch>[0], init?: RequestInit) => { | |
| 48 | const res = await real(input, init); | |
| 49 | if (!String(input).endsWith("/status")) return res; | |
| 50 | return Response.json(change(++n, await res.json())); | |
| 51 | }) as typeof fetch; | |
| 52 | try { | |
| 53 | return await fn(); | |
| 54 | } finally { | |
| 55 | globalThis.fetch = real; | |
| 56 | } | |
| 57 | } | |
| 58 | ||
| 59 | // Status call 1 is the start check, 2 the first sample, 3 the second: the stop shows on call 3 only, as a short press would. | |
| 60 | test.each([ | |
| 61 | ["STOP", { held: { by: "owner" } }], | |
| 62 | ["emergency stop", { estop: true }], | |
| 63 | ])("a %s mid-run stops the probe and the joint goes back", async (why, flag) => { | |
| 64 | const arm = await fakeArm(); | |
| 65 | try { | |
| 66 | const before = await position(arm.url, 2); | |
| 67 | await withStatus( | |
| 68 | (n, body) => (n === 3 ? { ...body, ...flag } : body), | |
| 69 | async () => { | |
| 70 | await expect(probe(arm.url, 2, 80)).rejects.toThrow(`stopped: ${why}`); | |
| 71 | }, | |
| 72 | ); | |
| 73 | expect(Math.abs((await position(arm.url, 2)) - before)).toBeLessThanOrEqual(12); | |
| 74 | } finally { | |
| 75 | await arm.close(); | |
| 76 | } | |
| 77 | }, 20_000); | |
| 78 | ||
| 79 | test("an abort signal stops the probe and the joint goes back", async () => { | |
| 80 | const arm = await fakeArm(); | |
| 81 | try { | |
| 82 | const before = await position(arm.url, 2); | |
| 83 | const abort = new AbortController(); | |
| 84 | setTimeout(() => abort.abort(), 100); | |
| 85 | await expect(probe(arm.url, 2, 80, abort.signal)).rejects.toThrow("stopped: interrupted"); | |
| 86 | expect(Math.abs((await position(arm.url, 2)) - before)).toBeLessThanOrEqual(12); | |
| 87 | } finally { | |
| 88 | await arm.close(); | |
| 89 | } | |
| 90 | }, 20_000); | |
| 91 | ||
| 92 | test("a joint that does not come home is reported, not called restored", async () => { | |
| 93 | const arm = await fakeArm(); | |
| 94 | try { | |
| 95 | const start = await position(arm.url, 2); | |
| 96 | await withStatus( | |
| 97 | (n, body) => (n === 1 ? body : { ...body, joints: body.joints.map((j: any) => (j.id === 2 ? { ...j, position: start + 200 } : j)) }), | |
| 98 | async () => { | |
| 99 | await expect(probe(arm.url, 2, 80)).rejects.toThrow(`joint 2 did not return to its start pose (at ${start + 200}, start ${start})`); | |
| 100 | }, | |
| 101 | ); | |
| 102 | } finally { | |
| 103 | await arm.close(); | |
| 104 | } | |
| 105 | }, 20_000); | |
| 106 | ||
| 107 | test("a stop that also stops the restore keeps the first error and says the restore failed", async () => { | |
| 108 | const arm = await fakeArm(); | |
| 109 | try { | |
| 110 | await withStatus( | |
| 111 | (n, body) => (n >= 3 ? { ...body, held: { by: "owner" } } : body), | |
| 112 | async () => { | |
| 113 | const err = await probe(arm.url, 2, 80).catch((e) => e); | |
| 114 | expect(err.message).toContain("stopped: STOP (and the restore failed: stopped: STOP)"); | |
| 115 | expect(err.cause).toBeInstanceOf(Error); | |
| 116 | }, | |
| 117 | ); | |
| 118 | } finally { | |
| 119 | await arm.close(); | |
| 120 | } | |
| 121 | }, 20_000); | |
| 122 | ||
| 123 | test("a step of 0 or not a number is refused before anything moves", async () => { | |
| 124 | const arm = await fakeArm(); | |
| 125 | try { | |
| 126 | await expect(probe(arm.url, 2, 0)).rejects.toThrow("--step must be"); | |
| 127 | await expect(probe(arm.url, 2, NaN)).rejects.toThrow("--step must be"); | |
| 128 | } finally { | |
| 129 | await arm.close(); | |
| 130 | } | |
| 131 | }); | |
| 132 | ||
| 133 | test("it refuses while a turn is held", async () => { | |
| 134 | const arm = await fakeArm(); | |
| 135 | try { | |
| 136 | const real = globalThis.fetch; | |
| 137 | globalThis.fetch = (async (input: Parameters<typeof fetch>[0], init?: RequestInit) => { | |
| 138 | const res = await real(input, init); | |
| 139 | if (!String(input).endsWith("/status")) return res; | |
| 140 | return Response.json({ ...((await res.json()) as object), held: { by: "owner" } }); | |
| 141 | }) as typeof fetch; | |
| 142 | try { | |
| 143 | await expect(probe(arm.url, 2)).rejects.toThrow("STOP is in force"); | |
| 144 | } finally { | |
| 145 | globalThis.fetch = real; | |
| 146 | } | |
| 147 | } finally { | |
| 148 | await arm.close(); | |
| 149 | } | |
| 150 | }, 10_000); |
Software/so101/bench/real-probe.ts added +163
| @@ -0,0 +1,163 @@ | ||
| 1 | /** | |
| 2 | * One small step of one joint on a live arm, measured. | |
| 3 | * bun run Software/so101/bench/real-probe.ts <url> --joint N [--step ticks] | |
| 4 | * Drives joint N (its servo id) over /follow from where it is to start±step (default 60 ticks, toward the middle of | |
| 5 | * its bounds, clamped inside them), samples /status at about 50 Hz and prints the time to arrive within 12 ticks, | |
| 6 | * the final error and the overshoot. It refuses to start unless /status shows nobody driving, no STOP and no E-stop; | |
| 7 | * if STOP or an E-stop appears mid-run it stops at once. Every run ends by returning the joint to its start pose. | |
| 8 | */ | |
| 9 | import WebSocket from "ws"; | |
| 10 | ||
| 11 | export const ARRIVE_TICKS = 12; | |
| 12 | export const DEFAULT_STEP = 60; | |
| 13 | const SAMPLE_MS = 20; | |
| 14 | const RUN_MS = 3000; | |
| 15 | const RESTORE_MS = 4000; | |
| 16 | ||
| 17 | interface Joint { | |
| 18 | id: number; | |
| 19 | joint: string | null; | |
| 20 | position: number | null; | |
| 21 | lo: number | null; | |
| 22 | hi: number | null; | |
| 23 | } | |
| 24 | interface Status { | |
| 25 | joints: Joint[]; | |
| 26 | driving: string | null; | |
| 27 | held: unknown; | |
| 28 | estop: unknown; | |
| 29 | } | |
| 30 | export interface Probe { | |
| 31 | start: number; | |
| 32 | target: number; | |
| 33 | arriveMs: number | null; | |
| 34 | finalError: number; | |
| 35 | overshoot: number; | |
| 36 | /** The joint was read back within ARRIVE_TICKS of its start after the run. */ | |
| 37 | restored: boolean; | |
| 38 | } | |
| 39 | ||
| 40 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 41 | const stopped = (s: Status) => (s.estop ? "emergency stop" : s.held ? "STOP" : null); | |
| 42 | const status = async (url: string): Promise<Status> => { | |
| 43 | const res = await fetch(`${url}/status`); | |
| 44 | if (!res.ok) throw new Error(`/status: ${res.status}`); | |
| 45 | return (await res.json()) as Status; | |
| 46 | }; | |
| 47 | ||
| 48 | /** The target: step ticks toward the middle of the bounds, held inside them. */ | |
| 49 | export function aim(start: number, step: number, lo: number, hi: number): number { | |
| 50 | const dir = start <= (lo + hi) / 2 ? 1 : -1; | |
| 51 | return Math.min(hi, Math.max(lo, start + dir * Math.abs(step))); | |
| 52 | } | |
| 53 | ||
| 54 | async function open(url: string): Promise<WebSocket> { | |
| 55 | const ws = new WebSocket(`${url.replace(/^http/, "ws")}/follow`); | |
| 56 | await new Promise((r, j) => (ws.once("open", r), ws.once("error", j), ws.once("close", (c) => j(new Error(`the stream was refused (${c})`))))); | |
| 57 | return ws; | |
| 58 | } | |
| 59 | ||
| 60 | /** Sends the target and samples until the time is up; halts (throws) on STOP, an E-stop, a closed stream or an abort. */ | |
| 61 | async function run(url: string, ws: WebSocket, joint: Joint, target: number, ms: number, signal?: AbortSignal, onSample?: (pos: number, t: number) => void): Promise<void> { | |
| 62 | let closed: number | null = null; | |
| 63 | ws.on("close", (code) => (closed = code)); | |
| 64 | ws.send(JSON.stringify({ [joint.id]: target })); | |
| 65 | const t0 = performance.now(); | |
| 66 | while (performance.now() - t0 < ms) { | |
| 67 | await sleep(SAMPLE_MS); | |
| 68 | const s = await status(url); | |
| 69 | const halt = stopped(s) ?? (closed !== null ? `the stream closed (${closed})` : signal?.aborted ? "interrupted" : null); | |
| 70 | if (halt) throw new Error(`stopped: ${halt}`); | |
| 71 | const pos = s.joints.find((j) => j.id === joint.id)?.position; | |
| 72 | if (typeof pos !== "number") throw new Error(`joint ${joint.id} has no position`); | |
| 73 | onSample?.(pos, performance.now() - t0); | |
| 74 | if (!onSample && Math.abs(pos - target) <= ARRIVE_TICKS) return; | |
| 75 | } | |
| 76 | } | |
| 77 | ||
| 78 | /** Sends the joint back to start and reads it back; throws unless it is there. */ | |
| 79 | async function restore(url: string, ws: WebSocket | null, joint: Joint, start: number): Promise<void> { | |
| 80 | try { | |
| 81 | if (!ws || ws.readyState !== WebSocket.OPEN) ws = await open(url); | |
| 82 | await run(url, ws, joint, start, RESTORE_MS); | |
| 83 | const at = (await status(url)).joints.find((j) => j.id === joint.id)?.position; | |
| 84 | if (typeof at !== "number" || Math.abs(at - start) > ARRIVE_TICKS) throw new Error(`joint ${joint.id} did not return to its start pose (at ${at ?? "unknown"}, start ${start})`); | |
| 85 | } finally { | |
| 86 | ws?.close(1000); | |
| 87 | } | |
| 88 | } | |
| 89 | ||
| 90 | /** Steps the joint and answers the numbers. Throws, naming why, where it must not start, must stop, or cannot get the joint home; the joint is sent back to its start either way. */ | |
| 91 | export async function probe(url: string, id: number, step = DEFAULT_STEP, signal?: AbortSignal): Promise<Probe> { | |
| 92 | if (!Number.isFinite(step) || step === 0) throw new Error("--step must be a non-zero number of ticks"); | |
| 93 | const s = await status(url); | |
| 94 | if (s.driving) throw new Error(`refusing to start: ${s.driving} is driving the arm`); | |
| 95 | const why = stopped(s); | |
| 96 | if (why) throw new Error(`refusing to start: ${why} is in force`); | |
| 97 | const joint = s.joints.find((j) => j.id === id); | |
| 98 | if (!joint) throw new Error(`no joint ${id}; the arm has ${s.joints.map((j) => j.id).join(", ")}`); | |
| 99 | if (joint.position === null || joint.lo === null || joint.hi === null) throw new Error(`joint ${id} has no position or bounds`); | |
| 100 | const start = joint.position; | |
| 101 | const target = aim(start, step, joint.lo, joint.hi); | |
| 102 | if (target === start) throw new Error(`joint ${id} cannot move ${step} ticks inside its bounds ${joint.lo}..${joint.hi}`); | |
| 103 | const dir = Math.sign(target - start); | |
| 104 | ||
| 105 | let ws: WebSocket | null = null; | |
| 106 | let arriveMs: number | null = null; | |
| 107 | let overshoot = 0; | |
| 108 | let last = start; | |
| 109 | let failure: Error | null = null; | |
| 110 | try { | |
| 111 | ws = await open(url); | |
| 112 | await run(url, ws, joint, target, RUN_MS, signal, (pos, t) => { | |
| 113 | last = pos; | |
| 114 | if (arriveMs === null && Math.abs(pos - target) <= ARRIVE_TICKS) arriveMs = Math.round(t); | |
| 115 | overshoot = Math.max(overshoot, dir * (pos - target)); | |
| 116 | }); | |
| 117 | } catch (err) { | |
| 118 | failure = err as Error; | |
| 119 | } | |
| 120 | try { | |
| 121 | await restore(url, ws, joint, start); | |
| 122 | } catch (err) { | |
| 123 | // The first error is the one that says what went wrong; the failed restore rides with it. | |
| 124 | if (!failure) throw err; | |
| 125 | throw new Error(`${failure.message} (and the restore failed: ${(err as Error).message})`, { cause: failure }); | |
| 126 | } | |
| 127 | if (failure) throw failure; | |
| 128 | return { start, target, arriveMs, finalError: Math.abs(last - target), overshoot: Math.round(overshoot), restored: true }; | |
| 129 | } | |
| 130 | ||
| 131 | export function report(id: number, p: Probe): string { | |
| 132 | return [ | |
| 133 | `joint ${id}: ${p.start} → ${p.target} (${p.target - p.start > 0 ? "+" : ""}${p.target - p.start} ticks)`, | |
| 134 | `arrive within ${ARRIVE_TICKS} ticks: ${p.arriveMs === null ? "never" : `${p.arriveMs} ms`}`, | |
| 135 | `final error: ${p.finalError} ticks`, | |
| 136 | `overshoot: ${p.overshoot} ticks`, | |
| 137 | p.restored ? "restored to the start pose" : "NOT restored to the start pose", | |
| 138 | ].join("\n"); | |
| 139 | } | |
| 140 | ||
| 141 | if (import.meta.main) { | |
| 142 | const args = process.argv.slice(2); | |
| 143 | const flag = (n: string) => { | |
| 144 | const i = args.indexOf(n); | |
| 145 | return i >= 0 ? args[i + 1] : undefined; | |
| 146 | }; | |
| 147 | const url = args.find((a) => /^https?:\/\//.test(a)); | |
| 148 | const id = Number(flag("--joint")); | |
| 149 | const step = flag("--step") === undefined ? DEFAULT_STEP : Number(flag("--step")); | |
| 150 | if (!url || !Number.isInteger(id)) { | |
| 151 | console.error("usage: bun run Software/so101/bench/real-probe.ts <url> --joint N [--step ticks]"); | |
| 152 | process.exit(2); | |
| 153 | } | |
| 154 | // Ctrl-C does not skip the restore: the run is told to stop and its finally sends the joint home. | |
| 155 | const abort = new AbortController(); | |
| 156 | process.on("SIGINT", () => abort.abort()); | |
| 157 | try { | |
| 158 | console.log(report(id, await probe(url.replace(/\/$/, ""), id, step, abort.signal))); | |
| 159 | } catch (err) { | |
| 160 | console.error((err as Error).message); | |
| 161 | process.exit(1); | |
| 162 | } | |
| 163 | } |
new above
offline