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
quill/arm:quill/438-netcode-for-the-arm-25-hz-actual-positio → main 9 commits fork
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 | ||
| 137 | 137 | `{"1": ticks, ..., "seq": n, "t": ms}` frames (at most 1 KiB each, or the |
| 138 | 138 | stream closes with 1009; past 200 a second they are dropped, and a stream that |
| 139 | 139 | 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 | |
| 142 | 144 | between them the machine log: every register written on the bus, every change |
| 143 | 145 | of driver, hold and E-stop, and every safety clamp, as it happens. Each joint |
| 144 | 146 | in `/status` also carries its last goal (`target`), its calibrated range |
| @@ -167,7 +169,12 @@ on a LAN or behind Tailscale. | ||
| 167 | 169 | arm (its own URDF and meshes, drag a joint and the arm follows; on a phone one |
| 168 | 170 | finger on a link drags that joint, two fingers turn the view and pinch zooms |
| 169 | 171 | 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 | |
| 171 | 178 | hold-to-jog buttons per joint that stream targets like a leader does, a |
| 172 | 179 | button per saved pose plus save and delete, hold and release, record and |
| 173 | 180 | 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 | ||
| 270 | 277 | into the page (`<script type="application/json" id="boot">`), so the camera |
| 271 | 278 | starts as soon as the script runs. The arm's limits, settings, poses and |
| 272 | 279 | 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 | |
| 275 | 282 | `driver`: `{id, name, owner}` of whoever holds the turn, or `null` when nobody |
| 276 | 283 | does. The owner shows as driving after a take-over. A 503 while the arm link |
| 277 | 284 | is reconnecting has no status and so no `driver`. The terminal (`src/tty.ts`) |
| @@ -318,7 +325,8 @@ or `"source": "simulated: <why>"`. | ||
| 318 | 325 | |
| 319 | 326 | With `ARM_SERVER` unset, the app runs the simulated arm and says |
| 320 | 327 | **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 | |
| 322 | 330 | every viewer's `/status` is answered from that. The Pi is asked once, however |
| 323 | 331 | many people are watching. An arm server from before `/watch` is polled over |
| 324 | 332 | 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"; | ||
| 9 | 9 | import { dirname, join } from "node:path"; |
| 10 | 10 | import { fileURLToPath } from "node:url"; |
| 11 | 11 | |
| 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"]); | |
| 13 | 13 | |
| 14 | 14 | const here = dirname(fileURLToPath(import.meta.url)); |
| 15 | 15 | const cache = new Map<string, { stamp: string; js: string }>(); |
Software/so101/src/bus.test.ts +14
| @@ -38,6 +38,20 @@ describe("FeetechBus", () => { | ||
| 38 | 38 | ]); |
| 39 | 39 | }); |
| 40 | 40 | |
| 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 | ||
| 41 | 55 | test("syncWrite sets every listed servo without waiting for replies", async () => { |
| 42 | 56 | const fake = new FakeBus([1, 2]); |
| 43 | 57 | const bus = new FeetechBus(fake); |
Software/so101/src/bus.ts +18 −1
| @@ -6,7 +6,7 @@ import { | ||
| 6 | 6 | encodeInstruction, |
| 7 | 7 | type StatusPacket, |
| 8 | 8 | } from "./protocol.ts"; |
| 9 | import { STS3215, decodeValue, encodeValue, type Register } from "./sts3215.ts"; | |
| 9 | import { STS3215, decodeAt, decodeValue, encodeValue, type Register } from "./sts3215.ts"; | |
| 10 | 10 | |
| 11 | 11 | export interface OpenOptions { |
| 12 | 12 | /** STS3215s ship at 1 Mbaud; LeRobot keeps them there. */ |
| @@ -191,6 +191,23 @@ export class FeetechBus { | ||
| 191 | 191 | return values; |
| 192 | 192 | } |
| 193 | 193 | |
| 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.(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 | ||
| 194 | 211 | /** Write the same register on many servos in one bus transaction. No status comes back. */ |
| 195 | 212 | async syncWrite(register: Register, values: Iterable<readonly [number, number]>): Promise<void> { |
| 196 | 213 | const list = [...values]; |
Software/so101/src/frame-limit.test.ts +1 −1
| @@ -131,7 +131,7 @@ describe("the relay's /follow", () => { | ||
| 131 | 131 | await Promise.race([new Promise((r) => camera.close(r)), sleep(500)]); |
| 132 | 132 | }); |
| 133 | 133 | 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 }); | |
| 135 | 135 | }); |
| 136 | 136 | afterAll(() => shut(simulation)); |
| 137 | 137 |
Software/so101/src/lobby.test.ts +1 −1
| @@ -726,7 +726,7 @@ describe("lobby", () => { | ||
| 726 | 726 | const alice = await join(port, ALICE); |
| 727 | 727 | await Promise.all([anon.first((m) => m.type === "chat"), alice.first((m) => m.type === "chat")]); |
| 728 | 728 | 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 | |
| 730 | 730 | lobby!.publish({ type: "status", joints: [] }); |
| 731 | 731 | for (const m of await Promise.all(heard)) expect(m.joints).toEqual([]); |
| 732 | 732 | }); |
Software/so101/src/machine-feed.test.ts +3 −3
| @@ -24,7 +24,7 @@ async function fakePi(ids: number[]) { | ||
| 24 | 24 | two.set([123, 1 << 2], 60); // Present_Load: 123 with the sign bit (10) set, -12.3 % |
| 25 | 25 | two[63] = 41; // Present_Temperature |
| 26 | 26 | 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 }); | |
| 28 | 28 | return server; |
| 29 | 29 | } |
| 30 | 30 | |
| @@ -64,7 +64,7 @@ afterEach(async () => { | ||
| 64 | 64 | |
| 65 | 65 | async function rig() { |
| 66 | 66 | 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 }); | |
| 68 | 68 | 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 }); |
| 69 | 69 | return relay; |
| 70 | 70 | } |
| @@ -113,7 +113,7 @@ test("a clamp shows up in the log the moment it is applied, with the command tha | ||
| 113 | 113 | }); |
| 114 | 114 | |
| 115 | 115 | 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 }); | |
| 117 | 117 | relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${sim.port}`, link: FAST }); |
| 118 | 118 | const first = await panel(relay.port); |
| 119 | 119 | 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 = new Map<number, Bounds>(); | |
| 30 | = DEFAULT_MOTION; | |
| 31 | readonly = new Governor(() => this., () => this.); | |
| 32 | /** Per joint, what was drawn less what the profile says, at the last confirmation; it fades to nothing. */ | |
| 33 | readonly = new Map<number, number>(); | |
| 34 | = 0; | |
| 35 | /** When the profile was last advanced; null before the first look. */ | |
| 36 | : 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. = { 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..set(j.id, { min: j.lo, max: j.hi }); | |
| 45 | else if (!this..has(j.id)) this..set(j.id, { min: 0, max: 4095 }); | |
| 46 | const was = drawn.get(j.id); | |
| 47 | this..place(j.id, j.position); | |
| 48 | this..set(j.id, was === undefined ? 0 : was - j.position); | |
| 49 | } | |
| 50 | this. = 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..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..pin(id, position); | |
| 61 | this..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..moving) return true; | |
| 67 | const fade = Math.exp(-(now - this.) / RECONCILE_MS); | |
| 68 | for (const o of this..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. === null ? 0 : Math.max(0, Math.min(1, (now - this.) / 1000)); // a hidden tab's minutes are not replayed | |
| 75 | this. = now; | |
| 76 | while (left > 0) { | |
| 77 | const dt = Math.min(left, MAX_STEP_S); | |
| 78 | this..step(dt); | |
| 79 | left -= dt; | |
| 80 | } | |
| 81 | const fade = Math.exp(-(now - this.) / RECONCILE_MS); | |
| 82 | const out = new Map<number, number>(); | |
| 83 | for (const [id, offset] of this.) { | |
| 84 | const s = this..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 | ||
| 24 | 24 | /** An arm server on a fake bus; `ids` tells the simulation (six joints) and the "real arm" (three) apart. */ |
| 25 | 25 | async function arm(ids: number[]): Promise<RunningServer> { |
| 26 | 26 | 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 }); | |
| 28 | 28 | } |
| 29 | 29 | 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` } : {}) })!; |
| 30 | 30 | const JPEG = Buffer.from([0xff, 0xd8, 0x4a, 0x50, 0x45, 0x47, 0xff, 0xd9]); |
| @@ -401,7 +401,7 @@ describe("relay", () => { | ||
| 401 | 401 | const closed = await new Promise<{ code: number; why: string }>((r) => ws.on("close", (code, why) => r({ code, why: String(why) }))); |
| 402 | 402 | expect(closed).toEqual({ code: 1013, why: "reconnecting to the arm…" }); |
| 403 | 403 | |
| 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 }); | |
| 405 | 405 | await sleep(120); |
| 406 | 406 | expect((await link(base)).mode).toBe("live"); |
| 407 | 407 | expect(await source(base)).toBe(`live 127.0.0.1:${port}`); |
Software/so101/src/relay.ts +24 −7
| @@ -30,7 +30,7 @@ | ||
| 30 | 30 | * |
| 31 | 31 | * The owner's speed cap (src/speed-cap.ts) is applied here too, to moves, poses, grips, playbacks and every stream frame. |
| 32 | 32 | * |
| 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 | |
| 34 | 34 | * WS /machine read-only, for anyone: the arm's raw status, the link's round trip and frame rate, and every |
| 35 | 35 | * command in and out with every safety clamp, pushed as it happens (src/machine-feed.ts) |
| 36 | 36 | * 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> { | ||
| 284 | 284 | open: (url, timeoutMs) => open(url, timeoutMs), |
| 285 | 285 | timings: options.link, |
| 286 | 286 | 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 | }, | |
| 288 | 292 | onMachine: (entries, run) => { |
| 289 | 293 | if (!onArm()) return; |
| 290 | 294 | machine.arm(entries, run); |
| @@ -334,7 +338,11 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 334 | 338 | remote: { http: simulation, ws: simulationWs, host: "simulation" }, |
| 335 | 339 | open: (url, timeoutMs) => open(url, timeoutMs), |
| 336 | 340 | 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 | }, | |
| 338 | 346 | onMachine: (entries, run) => !onArm() && machine.arm(entries, run), |
| 339 | 347 | }); |
| 340 | 348 | }, |
| @@ -467,13 +475,18 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 467 | 475 | |
| 468 | 476 | /** |
| 469 | 477 | * 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"?} | |
| 471 | 481 | */ |
| 472 | const statusEveryMs = options.statusEveryMs ?? 200; | |
| 482 | const statusEveryMs = options.statusEveryMs ?? 40; // the arm's own bus cycle: the simulation is asked this often | |
| 473 | 483 | let pushed = ""; |
| 474 | 484 | 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); | |
| 477 | 490 | if (lobby.watching === 0) return void (pushed = ""); // the next page to open the room hears it at once |
| 478 | 491 | pushing = true; |
| 479 | 492 | let text: string; |
| @@ -487,6 +500,10 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 487 | 500 | } |
| 488 | 501 | if (text !== pushed) lobby.publish(text); |
| 489 | 502 | pushed = text; |
| 503 | if (again) { | |
| 504 | again = false; | |
| 505 | return push(); | |
| 506 | } | |
| 490 | 507 | }; |
| 491 | 508 | |
| 492 | 509 | /** 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 { | ||
| 182 | 182 | return this..get(id)?.goal; |
| 183 | 183 | } |
| 184 | 184 | |
| 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..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..get(id); | |
| 194 | if (t) t.p = p; | |
| 195 | else this.pin(id, p); | |
| 196 | } | |
| 197 | ||
| 185 | 198 | /** Whether any joint has somewhere left to go. */ |
| 186 | 199 | get moving(): boolean { |
| 187 | 200 | for (const t of this..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", () => { | ||
| 106 | 106 | const [a, b] = await Promise.all([watch(), watch()]); |
| 107 | 107 | expect(a.first.joints.map((j) => j.id)).toEqual([1, 2, 6]); |
| 108 | 108 | 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); | |
| 113 | 118 | a.ws.close(); |
| 114 | 119 | b.ws.close(); |
| 115 | 120 | const refused = await new Promise<string>((resolve) => { |
| @@ -150,7 +155,19 @@ describe("viewer page", () => { | ||
| 150 | 155 | expect(module.status).toBe(200); |
| 151 | 156 | expect(html).toContain("intent.target(j.id, j.position"); // the slider row and the model both ask before showing the pose |
| 152 | 157 | 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 | |
| 154 | 171 | expect(html).toContain("explainLock()"); |
| 155 | 172 | }); |
| 156 | 173 |
Software/so101/src/server.ts +33 −8
| @@ -45,7 +45,8 @@ | ||
| 45 | 45 | * "seq" and "t" (src/command-gate.ts): then a stale, replayed or pre-resume one is dropped. |
| 46 | 46 | * A frame over 1 KiB closes the stream; frames past 200 a second are dropped, and a stream that |
| 47 | 47 | * 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; | |
| 49 | 50 | * the hosted app's relay keeps one open, so viewers see the arm without each of them asking the Pi. |
| 50 | 51 | * Between them, {"machine": [entries], "run": "<id>"}: every register written on the bus, every |
| 51 | 52 | * 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 { | ||
| 108 | 109 | easeSpeed?: number; |
| 109 | 110 | /** How often the stream's jam watch samples position and load, ms. */ |
| 110 | 111 | 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). */ | |
| 112 | 113 | watchEveryMs?: number; |
| 114 | /** Longest /watch goes without sending an unchanged status, ms (200): keep it inside the relay link's staleMs. */ | |
| 115 | watchQuietMs?: number; | |
| 113 | 116 | } |
| 114 | 117 | |
| 115 | 118 | export interface RunningServer { |
| @@ -162,6 +165,10 @@ class HttpError extends Error { | ||
| 162 | 165 | const MAX_BODY = 1_000_000; |
| 163 | 166 | const URDF = "so101_new_calib.urdf"; |
| 164 | 167 | /** 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; | |
| 165 | 172 | const JAM_LOAD = 45; |
| 166 | 173 | const JAM_WINDOW = 6; |
| 167 | 174 | /** 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 = {}): | ||
| 199 | 206 | const machine = new MachineLog(); |
| 200 | 207 | const run = randomUUID(); |
| 201 | 208 | 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; | |
| 202 | 212 | bus.onSyncWrite = (register, values) => { |
| 203 | 213 | 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; | |
| 204 | 215 | machine.note("sent", registerName(register), { values: perServo(values) }); |
| 205 | 216 | }; |
| 206 | 217 | /** 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 = {}): | ||
| 561 | 572 | } |
| 562 | 573 | |
| 563 | 574 | 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 | } | |
| 568 | 584 | const b = bounds(); |
| 569 | 585 | const joints = ids.map((id) => ({ |
| 570 | 586 | id, |
| @@ -603,6 +619,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 603 | 619 | estop, |
| 604 | 620 | held: gate.held, |
| 605 | 621 | 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(), | |
| 606 | 624 | }; |
| 607 | 625 | }; |
| 608 | 626 | |
| @@ -1412,17 +1430,24 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1412 | 1430 | // ------------------------------------------------------------------ the watch |
| 1413 | 1431 | // Read-only: the status, pushed. One timer reads the bus while anyone watches; a watcher's messages are ignored. |
| 1414 | 1432 | const watchers = new WebSocketServer({ noServer: true, maxPayload: 1024 }); |
| 1415 | const watchEveryMs = options.watchEveryMs ?? 250; | |
| 1433 | const watchEveryMs = options.watchEveryMs ?? 40; | |
| 1416 | 1434 | let watching: ReturnType<typeof setInterval> | undefined; |
| 1417 | 1435 | 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; | |
| 1418 | 1440 | const machineFrame = (entries: readonly unknown[]) => JSON.stringify({ machine: entries, run }); |
| 1419 | 1441 | const broadcast = async () => { |
| 1420 | 1442 | if (reading) return; // a slow bus read is not stacked behind another: the next tick asks again |
| 1421 | 1443 | reading = true; |
| 1422 | 1444 | try { |
| 1423 | 1445 | 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]; | |
| 1424 | 1449 | for (const w of watchers.clients) { |
| 1425 | if (w.readyState !== w.OPEN) continue; | |
| 1450 | if (w.readyState !== w.OPEN || (same && !fresh.has(w))) continue; | |
| 1426 | 1451 | w.send(text); |
| 1427 | 1452 | // A new watcher's first message is a status; the machine log so far comes right after it. |
| 1428 | 1453 | 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"; | ||
| 9 | 9 | import { FakeBus } from "./fake-bus.ts"; |
| 10 | 10 | import { sign } from "./identity.ts"; |
| 11 | 11 | import { Lobby } from "./lobby.ts"; |
| 12 | import { startRelay, type RunningRelay } from "./relay.ts"; | |
| 12 | import { parseRemote, startRelay, type RunningRelay } from "./relay.ts"; | |
| 13 | 13 | import { startServer, type RunningServer } from "./server.ts"; |
| 14 | 14 | |
| 15 | 15 | const KEY = "test-identity-key"; |
| @@ -91,3 +91,22 @@ test("a non-driver's frame is not applied", async () => { | ||
| 91 | 91 | await sleep(TICK_MS); |
| 92 | 92 | expect(goals(before).filter(([, ticks]) => ticks === 3000)).toEqual([]); |
| 93 | 93 | }); |
| 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 @@ | ||
| 1 | 1 | /** |
| 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 | |
| 3 | 4 | * meshes' geometry, drawn in one translucent material, never hit by a pick, and shown only while the two poses differ. |
| 4 | 5 | * Served at /ui/ and imported by the tests. |
| 5 | 6 | */ |
| @@ -19,13 +20,19 @@ export function apart(shown, real, eps = APART) { | ||
| 19 | 20 | return false; |
| 20 | 21 | } |
| 21 | 22 | |
| 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 | ||
| 22 | 28 | /** |
| 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] | |
| 25 | 32 | */ |
| 26 | export function makeGhost(robot, THREE) { | |
| 33 | export function makeGhost(robot, THREE, tone = ACTUAL) { | |
| 27 | 34 | 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 }); | |
| 29 | 36 | ghost.traverse((/** @type {any} */ o) => { |
| 30 | 37 | o.raycast = () => {}; |
| 31 | 38 | 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 @@ | ||
| 1 | 1 | import { describe, expect, test } from "bun:test"; |
| 2 | import { apart, makeGhost } from "./ghost.js"; | |
| 2 | import { ACTUAL, apart, makeGhost, PREDICTED } from "./ghost.js"; | |
| 3 | 3 | |
| 4 | 4 | describe("apart", () => { |
| 5 | 5 | test("the ghost shows only while the target and the real pose differ", () => { |
| @@ -49,4 +49,12 @@ describe("makeGhost", () => { | ||
| 49 | 49 | ghost.set({ elbow_flex: 1.2 }); ghost.hide(); |
| 50 | 50 | expect(copy.visible).toBe(false); |
| 51 | 51 | }); |
| 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 | }); | |
| 52 | 60 | }); |
Software/so101/src/viewer.html +40 −5
| @@ -1148,7 +1148,7 @@ | ||
| 1148 | 1148 | // ---------------------------------------------------------------- the 3D model |
| 1149 | 1149 | // three.js is most of what the page downloads, so it comes after the first view: the room and the camera work without it. |
| 1150 | 1150 | 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; | |
| 1152 | 1152 | function load3D() { |
| 1153 | 1153 | if (!modelInfo?.ok) return Promise.resolve(false); |
| 1154 | 1154 | return (model3D ??= (async () => { |
| @@ -1156,7 +1156,7 @@ | ||
| 1156 | 1156 | import("three"), import("three/examples/jsm/loaders/STLLoader.js"), import("urdf-loader/manipulator"), import("/ui/__VERSION__/table.js"), |
| 1157 | 1157 | import("/ui/__VERSION__/model-touch.js"), import("/ui/__VERSION__/ghost.js"), |
| 1158 | 1158 | ]); |
| 1159 | THREE = three; tablePlaneZ = plane.tablePlaneZ; makeGhost = ghosts.makeGhost; | |
| 1159 | THREE = three; tablePlaneZ = plane.tablePlaneZ; makeGhost = ghosts.makeGhost; predictedTone = ghosts.PREDICTED; | |
| 1160 | 1160 | if (!customElements.get("urdf-viewer")) customElements.define("urdf-viewer", URDFManipulator); |
| 1161 | 1161 | viewer.loadMeshFunc = (path, manager, material, done) => { |
| 1162 | 1162 | 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 @@ | ||
| 1192 | 1192 | viewer.world.add(table, grid, marker); |
| 1193 | 1193 | viewer.plane.visible = false; // the element's own shadow plane sits under the lowest part, which would be below the table |
| 1194 | 1194 | 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) | |
| 1195 | 1196 | if (status) applyModel(status); |
| 1196 | 1197 | viewer.camera.position.set(0.5, 0.38, 0.62); |
| 1197 | 1198 | viewer.controls.target.set(0.08, 0.1, 0); |
| @@ -1204,7 +1205,7 @@ | ||
| 1204 | 1205 | if (dragging !== e.detail || !viewer.robot) return; |
| 1205 | 1206 | const id = IDS[e.detail]; if (!id || !limits[id]) return; |
| 1206 | 1207 | const ticks = clampTicks(id, toTicks(id, viewer.robot.joints[e.detail].angle)); |
| 1207 | intent.set(id, ticks, performance.now()); | |
| 1208 | command(id, ticks); | |
| 1208 | 1209 | stream.send({ [id]: ticks }); |
| 1209 | 1210 | }); |
| 1210 | 1211 | /** 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 @@ | ||
| 1216 | 1217 | } |
| 1217 | 1218 | function applyModel(d) { |
| 1218 | 1219 | 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 | |
| 1220 | 1221 | const real = {}; |
| 1221 | 1222 | for (const j of d.joints) { |
| 1222 | 1223 | const name = NAMES[j.id]; |
| @@ -1227,6 +1228,7 @@ | ||
| 1227 | 1228 | viewer.setJointValue(name, toRad(j.id, want ?? j.position)); |
| 1228 | 1229 | } |
| 1229 | 1230 | ghost?.set(real); |
| 1231 | predict(d); | |
| 1230 | 1232 | // A stream frame the zone refuses shows as the status's `blocked` in enforce mode; the next clear status ends it. |
| 1231 | 1233 | if (d.blocked && d.zone === "enforce") refused({ error: `${sayViolation(d.blocked)}; back the leader off` }); |
| 1232 | 1234 | else if (!d.blocked && $("refusal").dataset.stream) accepted(); |
| @@ -1417,6 +1419,39 @@ | ||
| 1417 | 1419 | // ---------------------------------------------------------------- live status, sliders and jog |
| 1418 | 1420 | const joints = $("joints"); |
| 1419 | 1421 | 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 | ||
| 1420 | 1455 | /** A phone's compact row has room for a word, not "shoulder_pan". */ |
| 1421 | 1456 | const SHORT = { 1: "Base", 2: "Shoulder", 3: "Elbow", 4: "Wrist", 5: "Roll", 6: "Grip" }; |
| 1422 | 1457 | let sliding = null; |
| @@ -1436,7 +1471,7 @@ | ||
| 1436 | 1471 | const range = div.querySelector("input"); |
| 1437 | 1472 | range.setAttribute("aria-label", j.joint ?? `joint ${j.id}`); |
| 1438 | 1473 | 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; } }); | |
| 1440 | 1475 | const done = () => { if (sliding === j.id) { sliding = null; intent.release(j.id, performance.now()); stream.end(); } }; |
| 1441 | 1476 | range.addEventListener("pointerup", done); range.addEventListener("pointercancel", done); range.addEventListener("change", done); |
| 1442 | 1477 | for (const b of div.querySelectorAll(".jog button")) jog(b, j.id, () => Number(b.dataset.d)); |
new above
offline