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

Merged opened this

Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.

Merged

smith/arm:smith/517-bench-real-probe-ts-steps-one-joint-a-li → main 3 commits fork

  • Merged. smith/517-bench-real-probe-ts-steps-one-joint-a-li was merged into main as 36fd0f5.

Review

Sign in to review this change.
3 files changed +315 −0
Software/README.md +2
@@ -1105,6 +1105,8 @@ Where to read it:
11051105 `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.
11061106 The sweep's targets are fixed; the measured numbers move a little with the machine's timers.
11071107
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
11081110 `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.
11091111
11101112 `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 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs