Public: anyone can read this; the crew can change it.

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

Netcode for the arm: 25 Hz actual position, the page predicts the arm, intent and actual marks on every slider #473

Closed opened this in answer to
Closed without merging

quill/arm:quill/438-netcode-for-the-arm-25-hz-actual-positio → main 9 commits fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
18 files changed +395 −47
Software/README.md +14 −6
@@ -137,8 +137,10 @@ The API, all JSON (the full list with bodies is at the top of
137137 `{"1": ticks, ..., "seq": n, "t": ms}` frames (at most 1 KiB each, or the
138138 stream closes with 1009; past 200 a second they are dropped, and a stream that
139139 keeps sending twice that is closed with 1008; either way the arm holds, on the
140 arm server and on the relay alike: `so101/src/frame-limit.ts`); and the read-only WebSocket `/watch`, which sends
141 the `/status` body every 250 ms, one bus read shared by every watcher, and
140 arm server and on the relay alike: `so101/src/frame-limit.ts`); and the read-only WebSocket `/watch`, which reads
141 the `/status` body every 40 ms (Present_Position through Present_Temperature in
142 one sync read, torque at 2 Hz) and sends it when it changed, or after 200 ms
143 of the same, one bus read shared by every watcher, and
142144 between them the machine log: every register written on the bus, every change
143145 of driver, hold and E-stop, and every safety clamp, as it happens. Each joint
144146 in `/status` also carries its last goal (`target`), its calibrated range
@@ -167,7 +169,12 @@ on a LAN or behind Tailscale.
167169 arm (its own URDF and meshes, drag a joint and the arm follows; on a phone one
168170 finger on a link drags that joint, two fingers turn the view and pinch zooms
169171 without moving the arm; the target is drawn solid, the real arm as a faint
170 ghost until it gets there), a slider and
172 blue ghost until it gets there, and between them a pale ghost: the arm as the
173 page predicts it, the server's own Governor profile (`src/prediction.ts`, the
174 trapezoid in `src/safety.ts`) run from each confirmed position toward your
175 command and eased onto every new status without a jump; it is off for
176 reduced motion), a slider, with a mark for your target and one for the
177 arm's measured position, and
171178 hold-to-jog buttons per joint that stream targets like a leader does, a
172179 button per saved pose plus save and delete, hold and release, record and
173180 play, one STOP button (pressed, not released; space or Esc) with a separate
@@ -270,8 +277,8 @@ load only when they're used. The relay writes the link and the camera list
270277 into the page (`<script type="application/json" id="boot">`), so the camera
271278 starts as soon as the script runs. The arm's limits, settings, poses and
272279 model are all requested at once after that. One WebSocket, `/live`, carries
273 the turn, the queue, the chat and the arm's status. The status is pushed only
274 when it changes, so a page whose socket is open doesn't poll `/status`. `/status` and that push both carry
280 the turn, the queue, the chat and the arm's status. The status is pushed as the
281 arm sends it (25 times a second while it moves), only when it changed, so a page whose socket is open doesn't poll `/status`. `/status` and that push both carry
275282 `driver`: `{id, name, owner}` of whoever holds the turn, or `null` when nobody
276283 does. The owner shows as driving after a take-over. A 503 while the arm link
277284 is reconnecting has no status and so no `driver`. The terminal (`src/tty.ts`)
@@ -318,7 +325,8 @@ or `"source": "simulated: <why>"`.
318325
319326 With `ARM_SERVER` unset, the app runs the simulated arm and says
320327 **Simulated**. With it set, the app holds one WebSocket to the arm server's
321 `/watch` (`src/arm-link.ts`), which pushes its status four times a second, and
328 `/watch` (`src/arm-link.ts`), which pushes its status 25 times a second while the arm moves (and five times
329 a second while it sits still), and
322330 every viewer's `/status` is answered from that. The Pi is asked once, however
323331 many people are watching. An arm server from before `/watch` is polled over
324332 HTTP instead. One slow or dropped answer changes nothing. The link only counts
Software/so101/src/browser-modules.ts +1 −1
@@ -9,7 +9,7 @@ import { readFileSync, statSync } from "node:fs";
99 import { dirname, join } from "node:path";
1010 import { fileURLToPath } from "node:url";
1111
12 export const BROWSER_MODULES: ReadonlySet<string> = new Set(["leader-arm.ts", "teleop.ts", "bus.ts", "protocol.ts", "sts3215.ts", "mapping.ts", "leader-map.js"]);
12 export const BROWSER_MODULES: ReadonlySet<string> = new Set(["leader-arm.ts", "teleop.ts", "bus.ts", "protocol.ts", "sts3215.ts", "mapping.ts", "leader-map.js", "prediction.ts", "safety.ts"]);
1313
1414 const here = dirname(fileURLToPath(import.meta.url));
1515 const cache = new Map<string, { stamp: string; js: string }>();
Software/so101/src/bus.test.ts +14
@@ -38,6 +38,20 @@ describe("FeetechBus", () => {
3838 ]);
3939 });
4040
41 test("syncReadBlock reads position, load and temperature in one transaction", async () => {
42 const fake = new FakeBus([1, 2, 3], { drop: new Set([2]) });
43 fake.servos.get(3)!.set([0x00, 0x04, 0, 0, 0x2c, 0x01, 120, 41], 56); // 1024 ticks, load 300, 12.0 V, 41 °C
44 let packets = 0;
45 const write = fake.write.bind(fake);
46 fake.write = (...args: Parameters<typeof fake.write>) => (packets++, write(...args));
47 const bus = new FeetechBus(fake);
48 const block = await bus.syncReadBlock([1, 2, 3], [STS3215.Present_Position, STS3215.Present_Load, STS3215.Present_Temperature]);
49 expect(packets).toBe(1);
50 expect(block.get(3)).toEqual([1024, 300, 41]);
51 expect(block.has(2)).toBe(false);
52 expect(block.get(1)![0]).toBe(2048);
53 });
54
4155 test("syncWrite sets every listed servo without waiting for replies", async () => {
4256 const fake = new FakeBus([1, 2]);
4357 const bus = new FeetechBus(fake);
Software/so101/src/bus.ts +18 −1
@@ -6,7 +6,7 @@ import {
66 encodeInstruction,
77 type StatusPacket,
88 } from "./protocol.ts";
9 import { STS3215, decodeValue, encodeValue, type Register } from "./sts3215.ts";
9 import { STS3215, decodeAt, decodeValue, encodeValue, type Register } from "./sts3215.ts";
1010
1111 export interface OpenOptions {
1212 /** STS3215s ship at 1 Mbaud; LeRobot keeps them there. */
@@ -191,6 +191,23 @@ export class FeetechBus {
191191 return values;
192192 }
193193
194 /**
195 * Several registers that sit side by side (Present_Position through Present_Temperature, say), read from many servos
196 * in one bus transaction: one round trip where a syncRead per register would take one each. Each servo that answered
197 * maps to its values in the order the registers were named.
198 */
199 async syncReadBlock(ids: readonly number[], registers: readonly Register[]): Promise<Map<number, number[]>> {
200 const start = Math.min(...registers.map((r) => r.address));
201 const length = Math.max(...registers.map((r) => r.address + r.size)) - start;
202 const packet = encodeInstruction(BROADCAST_ID, Instruction.SyncRead, [start, length, ...ids]);
203 const frames = await this.#transact(packet, ids, BASE_TIMEOUT_MS + PER_MOTOR_MS * ids.length);
204 const values = new Map<number, number[]>();
205 for (const frame of frames) {
206 if (frame.params.length === length) values.set(frame.id, registers.map((r) => decodeAt(frame.params, start, r)));
207 }
208 return values;
209 }
210
194211 /** Write the same register on many servos in one bus transaction. No status comes back. */
195212 async syncWrite(register: Register, values: Iterable<readonly [number, number]>): Promise<void> {
196213 const list = [...values];
Software/so101/src/frame-limit.test.ts +1 −1
@@ -131,7 +131,7 @@ describe("the relay's /follow", () => {
131131 await Promise.race([new Promise((r) => camera.close(r)), sleep(500)]);
132132 });
133133 beforeAll(async () => {
134 simulation = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, ids: [1, 2, 3, 4, 5, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-limit-")), "poses.json"), watchEveryMs: 10 });
134 simulation = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, ids: [1, 2, 3, 4, 5, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-limit-")), "poses.json"), watchEveryMs: 10, watchQuietMs: 50 });
135135 });
136136 afterAll(() => shut(simulation));
137137
Software/so101/src/lobby.test.ts +1 −1
@@ -726,7 +726,7 @@ describe("lobby", () => {
726726 const alice = await join(port, ALICE);
727727 await Promise.all([anon.first((m) => m.type === "chat"), alice.first((m) => m.type === "chat")]);
728728 expect(lobby!.watching).toBe(2);
729 const heard = [anon, alice].map((c) => c.next((m) => m.type === "status"));
729 const heard = [anon, alice].map((c) => c.next((m) => m.type === "status" && Array.isArray(m.joints))); // the one published here, not the arm's own
730730 lobby!.publish({ type: "status", joints: [] });
731731 for (const m of await Promise.all(heard)) expect(m.joints).toEqual([]);
732732 });
Software/so101/src/machine-feed.test.ts +3 −3
@@ -24,7 +24,7 @@ async function fakePi(ids: number[]) {
2424 two.set([123, 1 << 2], 60); // Present_Load: 123 with the sign bit (10) set, -12.3 %
2525 two[63] = 41; // Present_Temperature
2626 two[40] = 1; // Torque_Enable
27 const server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10 });
27 const server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10, watchQuietMs: 0 });
2828 return server;
2929 }
3030
@@ -64,7 +64,7 @@ afterEach(async () => {
6464
6565 async function rig() {
6666 pi = await fakePi([1, 2, 6]);
67 sim = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10 });
67 sim = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10, watchQuietMs: 20 });
6868 relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${sim.port}`, remote: parseRemote({ ARM_SERVER: `ws://127.0.0.1:${pi.port}` }), link: FAST });
6969 return relay;
7070 }
@@ -113,7 +113,7 @@ test("a clamp shows up in the log the moment it is applied, with the command tha
113113 });
114114
115115 test("a panel that opens later gets the log so far; the simulation has a panel too", async () => {
116 sim = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10 });
116 sim = await startServer(new FeetechBus(new FakeBus([1, 2, 3, 4, 5, 6])), { host: "127.0.0.1", port: 0, posesFile: join(mkdtempSync(join(tmpdir(), "so101-machine-")), "poses.json"), watchEveryMs: 10, watchQuietMs: 20 });
117117 relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${sim.port}`, link: FAST });
118118 const first = await panel(relay.port);
119119 await first.until(() => first.messages.find((m) => m.type === "status"), "a status from the simulation");
Software/so101/src/prediction.test.ts added +79
@@ -0,0 +1,79 @@
1 import { describe, expect, test } from "bun:test";
2 import { Prediction, RECONCILE_MS } from "./prediction.ts";
3 import { Governor } from "./safety.ts";
4
5 const MOTION = { speed: 1024, accel: 4096 };
6 const joint = (position: number) => [{ id: 1, position, lo: 500, hi: 3500 }];
7
8 describe("Prediction", () => {
9 test("runs the arm's own Governor: from one confirmation, the page's profile is the server's", () => {
10 const p = new Prediction();
11 p.confirm(joint(2048), MOTION, 1000);
12 p.aim(1, 3000);
13 const server = new Governor(() => new Map([[1, { min: 500, max: 3500 }]]), () => MOTION);
14 server.reset(new Map([[1, 2048]]));
15 server.aim(1, 3000);
16 for (let k = 1; k <= 60; k++) {
17 server.step(1 / 60);
18 expect(Math.abs(p.at(1000 + (k * 1000) / 60).get(1)! - server.state(1)!.p)).toBeLessThan(1e-6);
19 }
20 });
21
22 test("never faster or harder than the envelope the status names", () => {
23 const p = new Prediction();
24 p.confirm(joint(1000), { speed: 500, accel: 2000 }, 0);
25 p.aim(1, 3000);
26 let [last, v] = [1000, 0];
27 for (let t = 10; t <= 2000; t += 10) {
28 const at = p.at(t).get(1)!;
29 const speed = (at - last) / 0.01;
30 expect(speed).toBeLessThanOrEqual(500 + 1e-6);
31 expect(Math.abs(speed - v) / 0.01).toBeLessThanOrEqual(2000 * 1.01);
32 [last, v] = [at, speed];
33 }
34 });
35
36 test("a confirmation that disagrees is eased in, never a jump", () => {
37 const p = new Prediction();
38 p.confirm(joint(2048), MOTION, 0);
39 p.aim(1, 2500);
40 const drawn = p.at(200).get(1)!;
41 p.confirm(joint(drawn - 80), MOTION, 200); // the arm is 80 ticks behind the prediction
42 expect(Math.abs(p.at(200).get(1)! - drawn)).toBeLessThan(1e-6); // drawn where it was the instant the status came
43 let prev = drawn;
44 for (let t = 204; t <= 200 + 6 * RECONCILE_MS; t += 4) {
45 const now = p.at(t).get(1)!;
46 expect(Math.abs(now - prev)).toBeLessThan(MOTION.speed * 0.004 + 6); // a frame's travel plus a slice of the gap, never the 80
47 prev = now;
48 }
49 });
50
51 test("with nowhere to go and the gap closed, it is still; the arm that arrives is where the prediction rests", () => {
52 const p = new Prediction();
53 p.confirm(joint(2048), MOTION, 0);
54 p.aim(1, 2100);
55 expect(p.moving(0)).toBe(true);
56 p.at(1000);
57 p.confirm(joint(2100), MOTION, 1000);
58 expect(p.moving(1000 + 10 * RECONCILE_MS)).toBe(false);
59 expect(p.at(1000 + 10 * RECONCILE_MS).get(1)).toBeCloseTo(2100, 1);
60 });
61
62 test("a target past the joint's bounds is held to them, as the arm holds it", () => {
63 const p = new Prediction();
64 p.confirm(joint(3400), MOTION, 0);
65 p.aim(1, 4000);
66 expect(p.at(3000).get(1)).toBe(3500);
67 });
68 test("a joint nobody commands rests on its measured position: nothing trails it", () => {
69 const p = new Prediction();
70 p.confirm(joint(2048), MOTION, 0);
71 p.aim(1, 3000);
72 p.at(100);
73 p.confirm(joint(2100), MOTION, 100);
74 p.rest(1, 2100);
75 expect(p.at(100).get(1)).toBe(2100);
76 expect(p.moving(100)).toBe(false);
77 expect(p.at(500).get(1)).toBe(2100);
78 });
79 });
Software/so101/src/prediction.ts added +89
@@ -0,0 +1,89 @@
1 /**
2 * What the arm will do next, as the page sees it: the server's own Governor (src/safety.ts) run in the browser from the
3 * last confirmed position toward the driver's target, on the same speed and acceleration envelope, so the page shows
4 * where the arm is heading before the arm can say so. Served at /app/prediction.ts and imported by the tests.
5 *
6 * Each status the arm sends confirms where it really is. The profile restarts from there (keeping its velocity), and
7 * the gap that opened between what was drawn and what was measured is eased out over a few frames rather than shown
8 * as a jump: server reconciliation, as game netcode does it. What is left between the prediction and the measured
9 * position is the honest error.
10 */
11
12 import { Governor, type Bounds } from "./safety.ts";
13
14 /** How fast a correction fades, ms: the drawn arm closes 63 % of the gap in this time, and never jumps. */
15 export const RECONCILE_MS = 60;
16 /** The longest step the profile is advanced in at once, s: a frame after a stall is several small steps, as on the arm. */
17 const MAX_STEP_S = 1 / 60;
18 /** Until a status says otherwise: the server's defaults, 90 °/s and 360 °/s², in ticks. */
19 const DEFAULT_MOTION = { speed: 1024, accel: 4096 };
20
21 export interface Confirmed {
22 id: number;
23 position: number | null;
24 lo?: number | null;
25 hi?: number | null;
26 }
27
28 export class Prediction {
29 readonly #bounds = new Map<number, Bounds>();
30 #motion = DEFAULT_MOTION;
31 readonly #governor = new Governor(() => this.#bounds, () => this.#motion);
32 /** Per joint, what was drawn less what the profile says, at the last confirmation; it fades to nothing. */
33 readonly #offset = new Map<number, number>();
34 #confirmedAt = 0;
35 /** When the profile was last advanced; null before the first look. */
36 #at: number | null = null;
37
38 /** A status came: each joint's measured position and bounds, and the envelope the arm's Governor holds to. */
39 confirm(joints: readonly Confirmed[], motion: { speed: number; accel: number } | null | undefined, now: number): void {
40 if (motion && motion.speed > 0 && motion.accel > 0) this.#motion = { speed: motion.speed, accel: motion.accel };
41 const drawn = this.at(now);
42 for (const j of joints) {
43 if (typeof j.position !== "number" || !Number.isFinite(j.position)) continue;
44 if (typeof j.lo === "number" && typeof j.hi === "number" && j.lo <= j.hi) this.#bounds.set(j.id, { min: j.lo, max: j.hi });
45 else if (!this.#bounds.has(j.id)) this.#bounds.set(j.id, { min: 0, max: 4095 });
46 const was = drawn.get(j.id);
47 this.#governor.place(j.id, j.position);
48 this.#offset.set(j.id, was === undefined ? 0 : was - j.position);
49 }
50 this.#confirmedAt = now;
51 }
52
53 /** Where the driver sent a joint (or, with no command, where it is): the profile heads there. */
54 aim(id: number, goal: number): void {
55 this.#governor.aim(id, goal);
56 }
57
58 /** A joint nobody commands: it rests where it was measured, with nothing to predict and no correction left to ease. */
59 rest(id: number, position: number): void {
60 this.#governor.pin(id, position);
61 this.#offset.set(id, 0);
62 }
63
64 /** Whether the profile still has somewhere to go, or a correction is still being eased out. */
65 moving(now: number): boolean {
66 if (this.#governor.moving) return true;
67 const fade = Math.exp(-(now - this.#confirmedAt) / RECONCILE_MS);
68 for (const o of this.#offset.values()) if (Math.abs(o * fade) >= 0.5) return true;
69 return false;
70 }
71
72 /** Each joint's predicted position at `now`, ticks (fractional), after advancing the profile to then. */
73 at(now: number): Map<number, number> {
74 let left = this.#at === null ? 0 : Math.max(0, Math.min(1, (now - this.#at) / 1000)); // a hidden tab's minutes are not replayed
75 this.#at = now;
76 while (left > 0) {
77 const dt = Math.min(left, MAX_STEP_S);
78 this.#governor.step(dt);
79 left -= dt;
80 }
81 const fade = Math.exp(-(now - this.#confirmedAt) / RECONCILE_MS);
82 const out = new Map<number, number>();
83 for (const [id, offset] of this.#offset) {
84 const s = this.#governor.state(id);
85 if (s) out.set(id, s.p + offset * fade);
86 }
87 return out;
88 }
89 }
Software/so101/src/relay.test.ts +2 −2
@@ -24,7 +24,7 @@ const listening = (server: Server | TcpServer) => new Promise<number>((r) => ser
2424 /** An arm server on a fake bus; `ids` tells the simulation (six joints) and the "real arm" (three) apart. */
2525 async function arm(ids: number[]): Promise<RunningServer> {
2626 const fake = new FakeBus(ids);
27 return startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids, posesFile: join(mkdtempSync(join(tmpdir(), "so101-relay-")), "poses.json"), watchdogMs: 60, watchEveryMs: 10 });
27 return startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids, posesFile: join(mkdtempSync(join(tmpdir(), "so101-relay-")), "poses.json"), watchdogMs: 60, watchEveryMs: 10, watchQuietMs: 20 });
2828 }
2929 const remoteAt = (port: number, camera?: number): Remote => parseRemote({ ARM_SERVER: `ws://127.0.0.1:${port}`, ...(camera ? { ARM_CAMERA_1: `http://127.0.0.1:${camera}/stream` } : {}) })!;
3030 const JPEG = Buffer.from([0xff, 0xd8, 0x4a, 0x50, 0x45, 0x47, 0xff, 0xd9]);
@@ -401,7 +401,7 @@ describe("relay", () => {
401401 const closed = await new Promise<{ code: number; why: string }>((r) => ws.on("close", (code, why) => r({ code, why: String(why) })));
402402 expect(closed).toEqual({ code: 1013, why: "reconnecting to the arm…" });
403403
404 remote = await startServer(new FeetechBus(new FakeBus([1, 2, 6])), { host: "127.0.0.1", port, ids: [1, 2, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-relay-")), "poses.json"), watchEveryMs: 10 });
404 remote = await startServer(new FeetechBus(new FakeBus([1, 2, 6])), { host: "127.0.0.1", port, ids: [1, 2, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-relay-")), "poses.json"), watchEveryMs: 10, watchQuietMs: 20 });
405405 await sleep(120);
406406 expect((await link(base)).mode).toBe("live");
407407 expect(await source(base)).toBe(`live 127.0.0.1:${port}`);
Software/so101/src/relay.ts +24 −7
@@ -30,7 +30,7 @@
3030 *
3131 * The owner's speed cap (src/speed-cap.ts) is applied here too, to moves, poses, grips, playbacks and every stream frame.
3232 *
33 * WS /live the lobby: the turn, the queue, the chat, and the arm's /status pushed when it changes
33 * WS /live the lobby: the turn, the queue, the chat, and the arm's /status pushed as it comes, when it changed
3434 * WS /machine read-only, for anyone: the arm's raw status, the link's round trip and frame rate, and every
3535 * command in and out with every safety clamp, pushed as it happens (src/machine-feed.ts)
3636 * POST /join {"name": "…"}: become someone here, no forge account needed (src/accounts.ts); the answer
@@ -284,7 +284,11 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
284284 open: (url, timeoutMs) => open(url, timeoutMs),
285285 timings: options.link,
286286 metrics,
287 onStatus: (status) => onArm() && machine.status(status),
287 onStatus: (status) => {
288 if (!onArm()) return;
289 machine.status(status);
290 void push(); // on to the pages as it comes, at the arm's 25 Hz: no wait for the next tick
291 },
288292 onMachine: (entries, run) => {
289293 if (!onArm()) return;
290294 machine.arm(entries, run);
@@ -334,7 +338,11 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
334338 remote: { http: simulation, ws: simulationWs, host: "simulation" },
335339 open: (url, timeoutMs) => open(url, timeoutMs),
336340 timings: options.link,
337 onStatus: (status) => !onArm() && machine.status(status),
341 onStatus: (status) => {
342 if (onArm()) return;
343 machine.status(status);
344 void push();
345 },
338346 onMachine: (entries, run) => !onArm() && machine.arm(entries, run),
339347 });
340348 },
@@ -467,13 +475,18 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
467475
468476 /**
469477 * The same status pushed over /live, so a page needs one socket for the pose, the chat and the queue rather than
470 * polling: read once per tick however many watch, sent only when it changed. {"type": "status", "status": {...}|null, "error"?}
478 * polling: sent as each status comes from the arm (or the simulation), and read once per tick besides for what changes
479 * here (the driver, a link that dropped); however many watch, sent only when it changed.
480 * {"type": "status", "status": {...}|null, "error"?}
471481 */
472 const statusEveryMs = options.statusEveryMs ?? 200;
482 const statusEveryMs = options.statusEveryMs ?? 40; // the arm's own bus cycle: the simulation is asked this often
473483 let pushed = "";
474484 let pushing = false;
475 const push = async () => {
476 if (!lobby || pushing) return;
485 /** A status came while one was being pushed: push again after, so the newest is never the one left behind. */
486 let again = false;
487 const push = async (): Promise<void> => {
488 if (!lobby) return;
489 if (pushing) return void (again = true);
477490 if (lobby.watching === 0) return void (pushed = ""); // the next page to open the room hears it at once
478491 pushing = true;
479492 let text: string;
@@ -487,6 +500,10 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
487500 }
488501 if (text !== pushed) lobby.publish(text);
489502 pushed = text;
503 if (again) {
504 again = false;
505 return push();
506 }
490507 };
491508
492509 /** A static answer, gzipped: whole, so the length is known; kept when the browser may cache it too. */
Software/so101/src/safety.ts +13
@@ -182,6 +182,19 @@ export class Governor {
182182 return this.#tracks.get(id)?.goal;
183183 }
184184
185 /** Where a joint is on its profile (fractional ticks) and how fast it moves (ticks/s, signed). */
186 state(id: number): { p: number; v: number } | undefined {
187 const t = this.#tracks.get(id);
188 return t && { p: t.p, v: t.v };
189 }
190
191 /** Move a joint's place on its profile to `p`, keeping its velocity and goal: a prediction corrected by what was measured. */
192 place(id: number, p: number): void {
193 const t = this.#tracks.get(id);
194 if (t) t.p = p;
195 else this.pin(id, p);
196 }
197
185198 /** Whether any joint has somewhere left to go. */
186199 get moving(): boolean {
187200 for (const t of this.#tracks.values()) if (t.p !== t.goal || t.v !== 0) return true;
Software/so101/src/server.test.ts +22 −5
@@ -106,10 +106,15 @@ describe("so101 serve", () => {
106106 const [a, b] = await Promise.all([watch(), watch()]);
107107 expect(a.first.joints.map((j) => j.id)).toEqual([1, 2, 6]);
108108 expect(b.first.driving).toBeNull();
109 let more = 0;
110 a.ws.on("message", () => more++);
111 await sleep(600); // pushed every 250 ms
112 expect(more).toBeGreaterThanOrEqual(1);
109 const more: Array<{ joints: Array<{ id: number; position: number }> }> = [];
110 a.ws.on("message", (d) => more.push(JSON.parse(String(d))));
111 await sleep(120);
112 expect(more.length).toBe(0); // nothing moved: nothing sent until 200 ms of quiet
113 fake.servos.get(1)!.set([0x34, 0x08], 56); // the arm moves to 2100
114 const at = performance.now();
115 while (!more.length && performance.now() - at < 500) await sleep(5);
116 expect(performance.now() - at).toBeLessThan(100); // read at the 25 Hz bus cycle, sent on change
117 expect(more[0]!.joints.find((j) => j.id === 1)!.position).toBe(2100);
113118 a.ws.close();
114119 b.ws.close();
115120 const refused = await new Promise<string>((resolve) => {
@@ -150,7 +155,19 @@ describe("viewer page", () => {
150155 expect(module.status).toBe(200);
151156 expect(html).toContain("intent.target(j.id, j.position"); // the slider row and the model both ask before showing the pose
152157 expect(html.match(/intent\.target\(/g)!.length).toBeGreaterThanOrEqual(2);
153 expect(html).toContain("intent.set(j.id, Number(range.value)"); // every input event is a target
158 expect(html).toContain("command(j.id, Number(range.value))"); // every input event is a target
159 expect(html).toMatch(/function command\(id, ticks\) \{\s*intent\.set\(id, ticks/); // ...held by the intent
160 });
161
162 test("predicts the arm: the Governor served at /app/, a second ghost, eased in on each status, off for reduced motion", async () => {
163 const html = await (await fetch(`${base}/`)).text();
164 expect(html).toContain('import("/app/prediction.ts")');
165 for (const name of ["prediction.ts", "safety.ts"]) expect((await fetch(`${base}/app/${name}`)).status).toBe(200);
166 expect(html).toContain("makeGhost(viewer.robot, THREE, predictedTone)");
167 expect(html).toContain("prediction.confirm(d.joints, d.motion, now)");
168 expect(html).toContain('matchMedia("(prefers-reduced-motion: reduce)")');
169 const status = (await (await fetch(`${base}/status`)).json()) as { motion: { speed: number; accel: number } };
170 expect(status.motion).toEqual({ speed: Math.round(90 * (4096 / 360)), accel: Math.round(360 * (4096 / 360)) }); // the envelope the page runs
154171 expect(html).toContain("explainLock()");
155172 });
156173
Software/so101/src/server.ts +33 −8
@@ -45,7 +45,8 @@
4545 * "seq" and "t" (src/command-gate.ts): then a stale, replayed or pre-resume one is dropped.
4646 * A frame over 1 KiB closes the stream; frames past 200 a second are dropped, and a stream that
4747 * keeps that up is closed (src/frame-limit.ts)
48 * WS /watch read-only: the server sends the /status body every watchEveryMs, one bus read shared by every watcher;
48 * WS /watch read-only: the server reads the /status body every watchEveryMs (40: the 25 Hz bus cycle), one bus
49 * read shared by every watcher, and sends it when it changed, or after 200 ms of the same;
4950 * the hosted app's relay keeps one open, so viewers see the arm without each of them asking the Pi.
5051 * Between them, {"machine": [entries], "run": "<id>"}: every register written on the bus, every
5152 * change of driver, hold and E-stop, and every safety clamp, as it happens (src/machine-log.ts);
@@ -108,8 +109,10 @@ export interface ServerOptions {
108109 easeSpeed?: number;
109110 /** How often the stream's jam watch samples position and load, ms. */
110111 jamSampleMs?: number;
111 /** How often each /watch connection is sent the status, ms. */
112 /** How often the status is read for /watch, ms; it is sent when it changed (and at least every watchQuietMs). */
112113 watchEveryMs?: number;
114 /** Longest /watch goes without sending an unchanged status, ms (200): keep it inside the relay link's staleMs. */
115 watchQuietMs?: number;
113116 }
114117
115118 export interface RunningServer {
@@ -162,6 +165,10 @@ class HttpError extends Error {
162165 const MAX_BODY = 1_000_000;
163166 const URDF = "so101_new_calib.urdf";
164167 /** Load, in percent of the servo's maximum, that counts as pushing against something. */
168 /** The present block status reads in one sync read; Present_Velocity and Present_Voltage ride along unread. */
169 const PRESENT = [STS3215.Present_Position, STS3215.Present_Load, STS3215.Present_Temperature] as const;
170 /** How stale the torque flags in a status may be, ms: positions go out at the bus cycle, torque at 2 Hz. */
171 const TORQUE_EVERY_MS = 500;
165172 const JAM_LOAD = 45;
166173 const JAM_WINDOW = 6;
167174 /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */
@@ -199,8 +206,12 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
199206 const machine = new MachineLog();
200207 const run = randomUUID();
201208 const goals = new Map<number, number>();
209 /** Torque_Enable sits apart from the present block, so it is read at most every TORQUE_EVERY_MS, and at once after it is written. */
210 let torque = new Map<number, number>();
211 let torqueAt = -Infinity;
202212 bus.onSyncWrite = (register, values) => {
203213 if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v), tracking?.wrote(id, Date.now());
214 if (register === STS3215.Torque_Enable) torqueAt = -Infinity;
204215 machine.note("sent", registerName(register), { values: perServo(values) });
205216 };
206217 /** The stream's dead-man hold is in force: no frame came for holdAfterMs, and the arm stays where it was. */
@@ -561,10 +572,15 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
561572 }
562573
563574 const status = async () => {
564 const positions = await bus.syncRead(ids, STS3215.Present_Position);
565 const loads = await bus.syncRead(ids, STS3215.Present_Load);
566 const temps = await bus.syncRead(ids, STS3215.Present_Temperature);
567 const torque = await bus.syncRead(ids, STS3215.Torque_Enable);
575 // Present_Position (56) through Present_Temperature (63) is one block: one round trip, not one per register.
576 const block = await bus.syncReadBlock(ids, PRESENT);
577 const positions = new Map([...block].map(([id, [p]]) => [id, p!]));
578 const loads = new Map([...block].map(([id, [, l]]) => [id, l!]));
579 const temps = new Map([...block].map(([id, [, , t]]) => [id, t!]));
580 if (performance.now() - torqueAt >= TORQUE_EVERY_MS) {
581 torque = await bus.syncRead(ids, STS3215.Torque_Enable);
582 torqueAt = performance.now();
583 }
568584 const b = bounds();
569585 const joints = ids.map((id) => ({
570586 id,
@@ -603,6 +619,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
603619 estop,
604620 held: gate.held,
605621 machine: { run, seq: machine.seq },
622 /** The Governor's envelope in ticks/s and ticks/s²: the page runs the same profile to predict the arm (src/prediction.ts). */
623 motion: speedLimits(),
606624 };
607625 };
608626
@@ -1412,17 +1430,24 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
14121430 // ------------------------------------------------------------------ the watch
14131431 // Read-only: the status, pushed. One timer reads the bus while anyone watches; a watcher's messages are ignored.
14141432 const watchers = new WebSocketServer({ noServer: true, maxPayload: 1024 });
1415 const watchEveryMs = options.watchEveryMs ?? 250;
1433 const watchEveryMs = options.watchEveryMs ?? 40;
14161434 let watching: ReturnType<typeof setInterval> | undefined;
14171435 let reading = false;
1436 /** The last status sent and when: an unchanged one is not sent again until watchQuietMs pass, so the link stays live. */
1437 const watchQuietMs = options.watchQuietMs ?? 200;
1438 let watched = "";
1439 let watchedAt = 0;
14181440 const machineFrame = (entries: readonly unknown[]) => JSON.stringify({ machine: entries, run });
14191441 const broadcast = async () => {
14201442 if (reading) return; // a slow bus read is not stacked behind another: the next tick asks again
14211443 reading = true;
14221444 try {
14231445 const text = JSON.stringify(await status());
1446 const now = performance.now();
1447 const same = text === watched && now - watchedAt < watchQuietMs;
1448 if (!same) [watched, watchedAt] = [text, now];
14241449 for (const w of watchers.clients) {
1425 if (w.readyState !== w.OPEN) continue;
1450 if (w.readyState !== w.OPEN || (same && !fresh.has(w))) continue;
14261451 w.send(text);
14271452 // A new watcher's first message is a status; the machine log so far comes right after it.
14281453 if (fresh.delete(w)) w.send(machineFrame(machine.recent(200)));
Software/so101/src/slider-follow.test.ts +20 −1
@@ -9,7 +9,7 @@ import { FeetechBus } from "./bus.ts";
99 import { FakeBus } from "./fake-bus.ts";
1010 import { sign } from "./identity.ts";
1111 import { Lobby } from "./lobby.ts";
12 import { startRelay, type RunningRelay } from "./relay.ts";
12 import { parseRemote, startRelay, type RunningRelay } from "./relay.ts";
1313 import { startServer, type RunningServer } from "./server.ts";
1414
1515 const KEY = "test-identity-key";
@@ -91,3 +91,22 @@ test("a non-driver's frame is not applied", async () => {
9191 await sleep(TICK_MS);
9292 expect(goals(before).filter(([, ticks]) => ticks === 3000)).toEqual([]);
9393 });
94
95 test("a move on the arm reaches the page as the arm sends it, not on the relay's next tick", async () => {
96 lobby = new Lobby({ identityKey: KEY, accounts: new Accounts() });
97 const remote = parseRemote({ ARM_SERVER: `ws://127.0.0.1:${simulation.port}` })!;
98 relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${simulation.port}`, remote, lobby, statusEveryMs: 60_000 });
99 const page = await open(relay.port, "/live", ALICE);
100 const seen: number[] = [];
101 page.on("message", (d) => {
102 const m = JSON.parse(String(d));
103 const at = m.type === "status" ? m.status?.joints?.find((j: { id: number }) => j.id === 2)?.position : undefined;
104 if (typeof at === "number") seen.push(at);
105 });
106 await sleep(300);
107 fake.servos.get(2)!.set([0x10, 0x09], 56); // the shoulder is at 2320
108 const from = performance.now();
109 while (!seen.includes(2320) && performance.now() - from < 1000) await sleep(5);
110 expect(seen).toContain(2320);
111 expect(performance.now() - from).toBeLessThan(150); // one 40 ms bus cycle and the hop, not a 60 s tick
112 });
Software/so101/src/ui/ghost.js +12 −5
@@ -1,5 +1,6 @@
11 /**
2 * The real arm as a faint ghost beside the solid model, which shows the target. A copy of the loaded robot sharing its
2 * The real arm as a faint ghost beside the solid model, which shows the target (and, in a second tone, the arm as the
3 * page predicts it). A copy of the loaded robot sharing its
34 * meshes' geometry, drawn in one translucent material, never hit by a pick, and shown only while the two poses differ.
45 * Served at /ui/ and imported by the tests.
56 */
@@ -19,13 +20,19 @@ export function apart(shown, real, eps = APART) {
1920 return false;
2021 }
2122
23 /** The confirmed arm: the blue ghost. */
24 export const ACTUAL = { color: 0x9ecbff, opacity: 0.22 };
25 /** The predicted arm (src/prediction.ts): neutral, a little stronger, so it reads as the one in motion. */
26 export const PREDICTED = { color: 0xe6edf3, opacity: 0.3 };
27
2228 /**
23 * A ghost of `robot` (a URDFRobot), added beside it. `set` takes the real pose in radians by joint name.
24 * @param {any} robot @param {any} THREE
29 * A ghost of `robot` (a URDFRobot), added beside it. `set` takes the pose in radians by joint name: the real one by
30 * default, or the predicted one drawn in PREDICTED.
31 * @param {any} robot @param {any} THREE @param {{ color: number, opacity: number }} [tone]
2532 */
26 export function makeGhost(robot, THREE) {
33 export function makeGhost(robot, THREE, tone = ACTUAL) {
2734 const ghost = robot.clone();
28 const look = new THREE.MeshBasicMaterial({ color: 0x9ecbff, transparent: true, opacity: 0.22, depthWrite: false });
35 const look = new THREE.MeshBasicMaterial({ color: tone.color, transparent: true, opacity: tone.opacity, depthWrite: false });
2936 ghost.traverse((/** @type {any} */ o) => {
3037 o.raycast = () => {};
3138 if (o.isMesh) { o.material = look; o.castShadow = o.receiveShadow = false; o.renderOrder = 1; }
Software/so101/src/ui/ghost.test.ts +9 −1
@@ -1,5 +1,5 @@
11 import { describe, expect, test } from "bun:test";
2 import { apart, makeGhost } from "./ghost.js";
2 import { ACTUAL, apart, makeGhost, PREDICTED } from "./ghost.js";
33
44 describe("apart", () => {
55 test("the ghost shows only while the target and the real pose differ", () => {
@@ -49,4 +49,12 @@ describe("makeGhost", () => {
4949 ghost.set({ elbow_flex: 1.2 }); ghost.hide();
5050 expect(copy.visible).toBe(false);
5151 });
52 test("the predicted arm is a second ghost in its own tone, apart from the blue confirmed one", () => {
53 const { robot, added } = fakeRobot();
54 makeGhost(robot, THREE);
55 makeGhost(robot, THREE, PREDICTED);
56 expect(added[0].children[0].material.opts.color).toBe(ACTUAL.color);
57 expect(added[1].children[0].material.opts.color).toBe(PREDICTED.color);
58 expect(PREDICTED.color).not.toBe(ACTUAL.color);
59 });
5260 });
Software/so101/src/viewer.html +40 −5
@@ -1148,7 +1148,7 @@
11481148 // ---------------------------------------------------------------- the 3D model
11491149 // three.js is most of what the page downloads, so it comes after the first view: the room and the camera work without it.
11501150 const viewer = $("model");
1151 let THREE = null, model3D = null, dragging = null, table = null, grid = null, marker = null, tablePlaneZ = null, makeGhost = null, ghost = null;
1151 let THREE = null, model3D = null, dragging = null, table = null, grid = null, marker = null, tablePlaneZ = null, makeGhost = null, ghost = null, predicted = null, predictedTone = null;
11521152 function load3D() {
11531153 if (!modelInfo?.ok) return Promise.resolve(false);
11541154 return (model3D ??= (async () => {
@@ -1156,7 +1156,7 @@
11561156 import("three"), import("three/examples/jsm/loaders/STLLoader.js"), import("urdf-loader/manipulator"), import("/ui/__VERSION__/table.js"),
11571157 import("/ui/__VERSION__/model-touch.js"), import("/ui/__VERSION__/ghost.js"),
11581158 ]);
1159 THREE = three; tablePlaneZ = plane.tablePlaneZ; makeGhost = ghosts.makeGhost;
1159 THREE = three; tablePlaneZ = plane.tablePlaneZ; makeGhost = ghosts.makeGhost; predictedTone = ghosts.PREDICTED;
11601160 if (!customElements.get("urdf-viewer")) customElements.define("urdf-viewer", URDFManipulator);
11611161 viewer.loadMeshFunc = (path, manager, material, done) => {
11621162 new STLLoader(manager).load(path, (geometry) => done(new THREE.Mesh(geometry, material ?? new THREE.MeshPhongMaterial({ color: 0x9aa4ae, shininess: 30 }))), undefined, (err) => done(null, err));
@@ -1192,6 +1192,7 @@
11921192 viewer.world.add(table, grid, marker);
11931193 viewer.plane.visible = false; // the element's own shadow plane sits under the lowest part, which would be below the table
11941194 ghost = makeGhost(viewer.robot, THREE); // the real arm, faint, while the solid model shows the target (src/ui/ghost.js)
1195 predicted = makeGhost(viewer.robot, THREE, predictedTone); // the arm as it will be, between the two (src/prediction.ts)
11951196 if (status) applyModel(status);
11961197 viewer.camera.position.set(0.5, 0.38, 0.62);
11971198 viewer.controls.target.set(0.08, 0.1, 0);
@@ -1204,7 +1205,7 @@
12041205 if (dragging !== e.detail || !viewer.robot) return;
12051206 const id = IDS[e.detail]; if (!id || !limits[id]) return;
12061207 const ticks = clampTicks(id, toTicks(id, viewer.robot.joints[e.detail].angle));
1207 intent.set(id, ticks, performance.now());
1208 command(id, ticks);
12081209 stream.send({ [id]: ticks });
12091210 });
12101211 /** The table and its grid sit on the plane the zone enforces (src/ui/table.js), so moving the table moves the drawing. */
@@ -1216,7 +1217,7 @@
12161217 }
12171218 function applyModel(d) {
12181219 placeTable(d);
1219 if (!viewer.robot || calib) return void ghost?.hide(); // while calibrating, the model shows the pose to copy
1220 if (!viewer.robot || calib) return void (ghost?.hide(), predicted?.hide()); // while calibrating, the model shows the pose to copy
12201221 const real = {};
12211222 for (const j of d.joints) {
12221223 const name = NAMES[j.id];
@@ -1227,6 +1228,7 @@
12271228 viewer.setJointValue(name, toRad(j.id, want ?? j.position));
12281229 }
12291230 ghost?.set(real);
1231 predict(d);
12301232 // A stream frame the zone refuses shows as the status's `blocked` in enforce mode; the next clear status ends it.
12311233 if (d.blocked && d.zone === "enforce") refused({ error: `${sayViolation(d.blocked)}; back the leader off` });
12321234 else if (!d.blocked && $("refusal").dataset.stream) accepted();
@@ -1417,6 +1419,39 @@
14171419 // ---------------------------------------------------------------- live status, sliders and jog
14181420 const joints = $("joints");
14191421 const built = new Map();
1422 // The arm as it will be: the arm's own Governor run here (src/prediction.ts, served at /app/) from each confirmed
1423 // position toward your command, drawn as a second ghost that moves along the profile until the arm confirms it.
1424 // The solid model is your command, the blue ghost the arm as last measured. Off for reduced motion.
1425 const still = matchMedia("(prefers-reduced-motion: reduce)");
1426 let prediction = null, predicting = 0;
1427 import("/app/prediction.ts").then((m) => { prediction = new m.Prediction(); }, () => {}); // an arm server too old to serve it: no prediction
1428 function predict(d) {
1429 if (!prediction || !predicted || still.matches || leaderMirrored()) return void predicted?.hide();
1430 const now = performance.now();
1431 prediction.confirm(d.joints, d.motion, now);
1432 for (const j of d.joints) {
1433 if (j.position == null) continue;
1434 const want = intent.target(j.id, j.position, now);
1435 if (want === null) prediction.rest(j.id, j.position); else prediction.aim(j.id, want);
1436 }
1437 animate();
1438 }
1439 /** The person sent a joint somewhere: the intent holds it, and the prediction heads there before any status says so. */
1440 function command(id, ticks) {
1441 intent.set(id, ticks, performance.now());
1442 prediction?.aim(id, ticks);
1443 animate();
1444 }
1445 function animate() { if (!predicting && prediction && predicted && !still.matches) predicting = requestAnimationFrame(drawPrediction); }
1446 function drawPrediction() {
1447 predicting = 0;
1448 const now = performance.now(), pose = {};
1449 for (const [id, ticks] of prediction.at(now)) if (NAMES[id] && limits[id]) pose[NAMES[id]] = toRad(id, ticks);
1450 predicted.set(pose); // shown only while it differs from the solid model, your command
1451 viewer.redraw?.();
1452 if (prediction.moving(now)) animate();
1453 }
1454
14201455 /** A phone's compact row has room for a word, not "shoulder_pan". */
14211456 const SHORT = { 1: "Base", 2: "Shoulder", 3: "Elbow", 4: "Wrist", 5: "Roll", 6: "Grip" };
14221457 let sliding = null;
@@ -1436,7 +1471,7 @@
14361471 const range = div.querySelector("input");
14371472 range.setAttribute("aria-label", j.joint ?? `joint ${j.id}`);
14381473 range.addEventListener("pointerdown", () => { sliding = j.id; stream.open(); });
1439 range.addEventListener("input", () => { if (sliding === j.id) { intent.set(j.id, Number(range.value), performance.now()); stream.send({ [j.id]: Number(range.value) }); el.pos.textContent = range.value; } });
1474 range.addEventListener("input", () => { if (sliding === j.id) { command(j.id, Number(range.value)); stream.send({ [j.id]: Number(range.value) }); el.pos.textContent = range.value; } });
14401475 const done = () => { if (sliding === j.id) { sliding = null; intent.release(j.id, performance.now()); stream.end(); } };
14411476 range.addEventListener("pointerup", done); range.addEventListener("pointercancel", done); range.addEventListener("change", done);
14421477 for (const b of div.querySelectorAll(".jog button")) jog(b, j.id, () => Number(b.dataset.d));

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs