Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Leader teleop: with the leader connected in Chrome, moving it moves the real arm - live, during your turn #65
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
quill/arm:quill/56-leader-teleop-with-the-leader-connected → main 2 commits fork 2 threads · 2 unresolved
7 files
changed +727
−80
Software/README.md +16 −5
| @@ -254,11 +254,22 @@ has Web Serial, not which browser it is. | ||
| 254 | 254 | - Before your turn, the 3D model and the joint rows mirror the leader |
| 255 | 255 | (`leader 2048` beside each joint, in follower ticks), so you can line it up |
| 256 | 256 | with the arm while you wait. |
| 257 | - During your turn, **Drive with leader** streams its pose at 60 Hz over the | |
| 258 | same `/follow` stream the sliders use. The arm first eases over to the | |
| 259 | leader, and the server applies the same limits, safe zone and jam | |
| 260 | watch as to any other stream. **■ Stop leader**, **Hand back**, or the end of | |
| 261 | your turn stop it, and the arm holds. | |
| 257 | - **Teleop** (the switch beside Connect leader arm, or the button in | |
| 258 | Controls) makes the arm follow the leader live during your turn. The | |
| 259 | leader is read at 60 Hz and its pose, gripper included, goes out at most | |
| 260 | 30 times a second over the same `/follow` stream the sliders use. Only the | |
| 261 | latest reading is sent and nothing queues behind it (`src/teleop.ts`). | |
| 262 | The arm first eases over to the leader instead of snapping. The server | |
| 263 | applies the same limits, safe zone and jam watch as to any other stream. | |
| 264 | The owner's switch takes control at once. Anyone else's switch waits for | |
| 265 | their turn and sends nothing before it. | |
| 266 | - The arm stops and holds when you switch Teleop off, hide the tab, unplug | |
| 267 | the leader, hand back, or your turn ends. If the connection drops, nothing | |
| 268 | is sent and teleop reconnects by itself. | |
| 269 | - While teleop drives, Controls draws each joint's leader target (ring) and | |
| 270 | the follower's reported position (dot) on one track, so you can see the | |
| 271 | lag. The leader→follower lag p50/p95 is shown there and on the metrics | |
| 272 | page, as this browser measures it. It turns amber above 200 ms at p95. | |
| 262 | 273 | - Frames that arrive after your turn ends are dropped at the relay, even |
| 263 | 274 | before your stream has closed. |
| 264 | 275 | - iPhone and iPad browsers can't open USB ports (WebKit has no Web Serial). |
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", "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"]); | |
| 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/fake-serial.ts added +48
| @@ -0,0 +1,48 @@ | ||
| 1 | /** | |
| 2 | * A Web Serial port with a leader arm's servos behind it, for the tests of the leader in the browser | |
| 3 | * (src/leader-arm.ts) and of teleop (src/teleop.ts). | |
| 4 | */ | |
| 5 | ||
| 6 | import { FakeBus, type FakeOptions } from "./fake-bus.ts"; | |
| 7 | import type { SerialPortLike } from "./leader-arm.ts"; | |
| 8 | ||
| 9 | /** A Web Serial port with a leader arm's servos behind it, as Chrome hands one to the page. */ | |
| 10 | export class FakeSerialPort implements SerialPortLike { | |
| 11 | readable: ReadableStream<Uint8Array> | null = null; | |
| 12 | writable: WritableStream<Uint8Array> | null = null; | |
| 13 | opened: number | null = null; | |
| 14 | closed = false; | |
| 15 | : ReadableStreamDefaultController<Uint8Array> | null = null; | |
| 16 | = false; | |
| 17 | readonly servos: FakeBus; | |
| 18 | constructor(servos: FakeBus) { | |
| 19 | this.servos = servos; | |
| 20 | } | |
| 21 | async open({ baudRate }: { baudRate: number }) { | |
| 22 | this.opened = baudRate; | |
| 23 | this. = false; | |
| 24 | // Cancelled (the page closed the leader) or errored (unplugged), the stream takes no more bytes. | |
| 25 | this.readable = new ReadableStream({ start: (c) => void (this. = c), cancel: () => void (this. = true) }); | |
| 26 | this.servos.on("data", (chunk: Uint8Array) => { | |
| 27 | if (!this.) this.?.enqueue(chunk); | |
| 28 | }); | |
| 29 | this.writable = new WritableStream({ write: (chunk) => this.servos.write(chunk, () => undefined) }); | |
| 30 | } | |
| 31 | async close() { | |
| 32 | this.closed = true; | |
| 33 | this. = true; | |
| 34 | } | |
| 35 | unplug() { | |
| 36 | this. = true; // a lost device answers nothing more | |
| 37 | this.?.error(new Error("The device has been lost.")); | |
| 38 | } | |
| 39 | } | |
| 40 | ||
| 41 | /** A calibrated leader: servo id → [min, max, present]. */ | |
| 42 | export function leaderServos(arm: Record<number, [number, number, number]>, options: FakeOptions = {}): FakeBus { | |
| 43 | const fake = new FakeBus(Object.keys(arm).map(Number), options); | |
| 44 | for (const [id, [min, max]] of Object.entries(arm)) fake.servos.get(Number(id))!.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9); | |
| 45 | for (const [id, [, , at]] of Object.entries(arm)) place(fake, Number(id), at); | |
| 46 | return fake; | |
| 47 | } | |
| 48 | export const place = (fake: FakeBus, id: number, ticks: number) => fake.servos.get(id)!.set([ticks & 0xff, ticks >> 8], 56); |
Software/so101/src/leader-arm.test.ts +3 −36
| @@ -5,9 +5,10 @@ import { join } from "node:path"; | ||
| 5 | 5 | import WebSocket from "ws"; |
| 6 | 6 | import { BROWSER_MODULES } from "./browser-modules.ts"; |
| 7 | 7 | import { FeetechBus } from "./bus.ts"; |
| 8 | import { FakeBus, type FakeOptions } from "./fake-bus.ts"; | |
| 8 | import { FakeBus } from "./fake-bus.ts"; | |
| 9 | import { FakeSerialPort, leaderServos, place } from "./fake-serial.ts"; | |
| 9 | 10 | import { sign } from "./identity.ts"; |
| 10 | import { LeaderArm, leaderSupport, type SerialPortLike } from "./leader-arm.ts"; | |
| 11 | import { LeaderArm, leaderSupport } from "./leader-arm.ts"; | |
| 11 | 12 | import { Lobby } from "./lobby.ts"; |
| 12 | 13 | import { startRelay, type RunningRelay } from "./relay.ts"; |
| 13 | 14 | import { startServer, type RunningServer } from "./server.ts"; |
| @@ -21,40 +22,6 @@ const ALICE = (() => { | ||
| 21 | 22 | return { "x-forge-identity": identity, "x-forge-signature": sign(identity, KEY) }; |
| 22 | 23 | })(); |
| 23 | 24 | |
| 24 | /** A Web Serial port with a leader arm's servos behind it, as Chrome hands one to the page. */ | |
| 25 | class FakeSerialPort implements SerialPortLike { | |
| 26 | readable: ReadableStream<Uint8Array> | null = null; | |
| 27 | writable: WritableStream<Uint8Array> | null = null; | |
| 28 | opened: number | null = null; | |
| 29 | closed = false; | |
| 30 | : ReadableStreamDefaultController<Uint8Array> | null = null; | |
| 31 | readonly servos: FakeBus; | |
| 32 | constructor(servos: FakeBus) { | |
| 33 | this.servos = servos; | |
| 34 | } | |
| 35 | async open({ baudRate }: { baudRate: number }) { | |
| 36 | this.opened = baudRate; | |
| 37 | this.readable = new ReadableStream({ start: (c) => void (this. = c) }); | |
| 38 | this.servos.on("data", (chunk: Uint8Array) => this.?.enqueue(chunk)); | |
| 39 | this.writable = new WritableStream({ write: (chunk) => this.servos.write(chunk, () => undefined) }); | |
| 40 | } | |
| 41 | async close() { | |
| 42 | this.closed = true; | |
| 43 | } | |
| 44 | unplug() { | |
| 45 | this.?.error(new Error("The device has been lost.")); | |
| 46 | } | |
| 47 | } | |
| 48 | ||
| 49 | /** A calibrated leader: servo id → [min, max, present]. */ | |
| 50 | function leaderServos(arm: Record<number, [number, number, number]>, options: FakeOptions = {}): FakeBus { | |
| 51 | const fake = new FakeBus(Object.keys(arm).map(Number), options); | |
| 52 | for (const [id, [min, max]] of Object.entries(arm)) fake.servos.get(Number(id))!.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9); | |
| 53 | for (const [id, [, , at]] of Object.entries(arm)) place(fake, Number(id), at); | |
| 54 | return fake; | |
| 55 | } | |
| 56 | const place = (fake: FakeBus, id: number, ticks: number) => fake.servos.get(id)!.set([ticks & 0xff, ticks >> 8], 56); | |
| 57 | ||
| 58 | 25 | describe("the leader arm over Web Serial", () => { |
| 59 | 26 | test("opens at 1 Mbaud, reads each range, lets the arm go limp, and reads its pose", async () => { |
| 60 | 27 | const servos = leaderServos({ 1: [1000, 3000, 2500], 2: [900, 3100, 1200], 6: [2000, 3000, 2250] }); |
Software/so101/src/teleop.test.ts added +289
| @@ -0,0 +1,289 @@ | ||
| 1 | import { afterAll, afterEach, beforeAll, describe, expect, test } from "bun:test"; | |
| 2 | import { mkdtempSync } from "node:fs"; | |
| 3 | import { tmpdir } from "node:os"; | |
| 4 | import { join } from "node:path"; | |
| 5 | import WebSocket from "ws"; | |
| 6 | import { FeetechBus } from "./bus.ts"; | |
| 7 | import { FakeBus } from "./fake-bus.ts"; | |
| 8 | import { FakeSerialPort, leaderServos, place } from "./fake-serial.ts"; | |
| 9 | import { sign } from "./identity.ts"; | |
| 10 | import { LeaderArm } from "./leader-arm.ts"; | |
| 11 | import { Lobby } from "./lobby.ts"; | |
| 12 | import { startRelay, type RunningRelay } from "./relay.ts"; | |
| 13 | import { startServer, type RunningServer } from "./server.ts"; | |
| 14 | import { lagSample, percentile, Teleop, type SocketLike, type TeleopState } from "./teleop.ts"; | |
| 15 | ||
| 16 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 17 | ||
| 18 | /** A /follow socket that opens at once and records what is sent and how it was closed. */ | |
| 19 | class FakeSocket implements SocketLike { | |
| 20 | readyState = 0; | |
| 21 | sent: Array<Record<string, number>> = []; | |
| 22 | closedWith: number | null = null; | |
| 23 | : Array<() => void> = []; | |
| 24 | : Array<(e: { code: number; reason: string }) => void> = []; | |
| 25 | constructor() { | |
| 26 | queueMicrotask(() => { | |
| 27 | if (this.readyState !== 0) return; | |
| 28 | this.readyState = 1; | |
| 29 | for (const f of this.) f(); | |
| 30 | }); | |
| 31 | } | |
| 32 | send(data: string) { | |
| 33 | this.sent.push(JSON.parse(data)); | |
| 34 | } | |
| 35 | close(code = 1000) { | |
| 36 | this.closedWith = code; | |
| 37 | this.readyState = 3; | |
| 38 | } | |
| 39 | /** The server ends it, or the network drops it. */ | |
| 40 | end(code: number, reason = "") { | |
| 41 | this.readyState = 3; | |
| 42 | for (const f of this.) f({ code, reason }); | |
| 43 | } | |
| 44 | addEventListener(type: "open" | "close", listener: never) { | |
| 45 | (type === "open" ? this. : this.).push(listener); | |
| 46 | } | |
| 47 | } | |
| 48 | ||
| 49 | function rig(options: { may?: boolean; visible?: boolean } = {}) { | |
| 50 | const env = { may: options.may ?? true, visible: options.visible ?? true }; | |
| 51 | const sockets: FakeSocket[] = []; | |
| 52 | const states: Array<[TeleopState, string | null]> = []; | |
| 53 | const teleop = new Teleop({ | |
| 54 | connect: () => { | |
| 55 | const s = new FakeSocket(); | |
| 56 | sockets.push(s); | |
| 57 | return s; | |
| 58 | }, | |
| 59 | mayDrive: () => env.may, | |
| 60 | visible: () => env.visible, | |
| 61 | retryMs: 50, | |
| 62 | onChange: (state, note) => states.push([state, note]), | |
| 63 | }); | |
| 64 | teleop.setLeader(true); | |
| 65 | return { env, sockets, states, teleop, sent: () => sockets.flatMap((s) => s.sent) }; | |
| 66 | } | |
| 67 | ||
| 68 | /** Readings at `hz` for `ms`, the base joint counting up from 2000. */ | |
| 69 | async function readings(teleop: Teleop, hz: number, ms: number, from = 2000): Promise<number> { | |
| 70 | const until = performance.now() + ms; | |
| 71 | let t = from; | |
| 72 | while (performance.now() < until) { | |
| 73 | teleop.pose({ 1: t++, 6: 2500 }); | |
| 74 | await sleep(1000 / hz); | |
| 75 | } | |
| 76 | return t - 1; | |
| 77 | } | |
| 78 | ||
| 79 | describe("teleop", () => { | |
| 80 | test("sends the leader's pose at 30 Hz at most, the latest reading winning, gripper and all", async () => { | |
| 81 | const { teleop, sockets, sent } = rig(); | |
| 82 | teleop.set(true); | |
| 83 | await sleep(5); | |
| 84 | expect(teleop.state).toBe("live"); | |
| 85 | const last = await readings(teleop, 120, 500); | |
| 86 | await sleep(60); | |
| 87 | const frames = sent(); | |
| 88 | expect(frames.length).toBeGreaterThanOrEqual(10); | |
| 89 | expect(frames.length).toBeLessThanOrEqual(17); // 500 ms at 30 Hz, never the 60 readings it was handed | |
| 90 | expect(frames.at(-1)).toEqual({ 1: last, 6: 2500 }); // nothing older is left behind to go out later | |
| 91 | expect(new Set(frames.map((f) => f[1])).size).toBe(frames.length); // a reading is never sent twice | |
| 92 | expect(sockets.length).toBe(1); | |
| 93 | }); | |
| 94 | ||
| 95 | test("outside your turn nothing is sent and no stream is opened", async () => { | |
| 96 | const { teleop, sockets, sent } = rig({ may: false }); | |
| 97 | teleop.set(true); | |
| 98 | await readings(teleop, 60, 150); | |
| 99 | expect(teleop.state).toBe("waiting"); | |
| 100 | expect(sockets.length).toBe(0); | |
| 101 | expect(sent()).toEqual([]); | |
| 102 | }); | |
| 103 | ||
| 104 | test("the owner's switch waits for the take-over, then drives; the turn ending switches it off and closes the stream", async () => { | |
| 105 | const { teleop, sockets, env, states } = rig({ may: false }); | |
| 106 | teleop.set(true); | |
| 107 | expect(teleop.state).toBe("waiting"); | |
| 108 | env.may = true; // the take-over arrived | |
| 109 | teleop.update(); | |
| 110 | await sleep(5); | |
| 111 | expect(teleop.state).toBe("live"); | |
| 112 | env.may = false; | |
| 113 | teleop.update(); | |
| 114 | expect(sockets[0]!.closedWith).toBe(1000); // on purpose: the arm server holds | |
| 115 | expect(teleop.on).toBe(false); | |
| 116 | expect(states.at(-1)).toEqual(["off", "your turn ended"]); | |
| 117 | env.may = true; // a later turn does not start the leader driving by itself | |
| 118 | teleop.update(); | |
| 119 | expect(sockets.length).toBe(1); | |
| 120 | }); | |
| 121 | ||
| 122 | test("a hidden tab stops sending at once, and teleop comes back when it is shown", async () => { | |
| 123 | const { teleop, sockets, env, sent } = rig(); | |
| 124 | teleop.set(true); | |
| 125 | await sleep(5); | |
| 126 | await readings(teleop, 60, 100); | |
| 127 | env.visible = false; | |
| 128 | teleop.update(); | |
| 129 | expect(sockets[0]!.closedWith).toBe(1000); | |
| 130 | expect(teleop.state).toBe("paused"); | |
| 131 | const before = sent().length; | |
| 132 | await readings(teleop, 60, 100); | |
| 133 | expect(sent().length).toBe(before); | |
| 134 | env.visible = true; | |
| 135 | teleop.update(); | |
| 136 | await sleep(5); | |
| 137 | expect(teleop.state).toBe("live"); | |
| 138 | await readings(teleop, 60, 100); | |
| 139 | expect(sockets[1]!.sent.length).toBeGreaterThan(0); | |
| 140 | }); | |
| 141 | ||
| 142 | test("a lost socket sends nothing and reconnects; one the server ends turns teleop off with its reason", async () => { | |
| 143 | const { teleop, sockets, states } = rig(); | |
| 144 | teleop.set(true); | |
| 145 | await sleep(5); | |
| 146 | sockets[0]!.end(1006); | |
| 147 | expect(teleop.state).toBe("paused"); | |
| 148 | teleop.pose({ 1: 2100 }); | |
| 149 | await sleep(80); | |
| 150 | expect(sockets.length).toBe(2); | |
| 151 | expect(teleop.state).toBe("live"); | |
| 152 | sockets[1]!.end(1000, "your turn ended"); | |
| 153 | expect(teleop.state).toBe("off"); | |
| 154 | expect(states.at(-1)).toEqual(["off", "your turn ended"]); | |
| 155 | await sleep(80); | |
| 156 | expect(sockets.length).toBe(2); | |
| 157 | }); | |
| 158 | ||
| 159 | test("an unplugged leader switches teleop off and closes the stream before another frame goes", async () => { | |
| 160 | const { teleop, sockets, sent } = rig(); | |
| 161 | teleop.set(true); | |
| 162 | await sleep(5); | |
| 163 | teleop.pose({ 1: 2000 }); | |
| 164 | await sleep(40); | |
| 165 | const before = sent().length; | |
| 166 | teleop.pose({ 1: 2400 }); | |
| 167 | teleop.setLeader(false, "the leader was unplugged"); | |
| 168 | await sleep(80); | |
| 169 | expect(sockets[0]!.closedWith).toBe(1000); | |
| 170 | expect(sent().length).toBe(before); | |
| 171 | expect(teleop.on).toBe(false); | |
| 172 | }); | |
| 173 | }); | |
| 174 | ||
| 175 | describe("leader→follower lag", () => { | |
| 176 | const sent = [0, 33, 66, 100, 133].map((at, i) => ({ at, frame: { 1: 2000 + i * 50, 2: 1000 } })); | |
| 177 | test("is how long ago the leader was where the follower is now", () => { | |
| 178 | expect(lagSample(sent, { 1: 2100, 2: 1004 }, 166)).toBe(100); | |
| 179 | expect(lagSample(sent, { 1: 2005, 2: 1000 }, 140)).toBe(140); | |
| 180 | }); | |
| 181 | test("says nothing when the follower has caught up, is nowhere near, or the leader barely moved", () => { | |
| 182 | expect(lagSample(sent, { 1: 2200, 2: 1000 }, 150)).toBeNull(); | |
| 183 | expect(lagSample(sent, { 1: 3000, 2: 1000 }, 150)).toBeNull(); | |
| 184 | const creeping = [0, 33, 66, 100].map((at, i) => ({ at, frame: { 1: 2000 + i * 10 } })); | |
| 185 | expect(lagSample(creeping, { 1: 2005 }, 120)).toBeNull(); // within two tolerances of the latest: too close to tell | |
| 186 | expect(lagSample([], { 1: 2000 }, 0)).toBeNull(); | |
| 187 | }); | |
| 188 | test("p50 and p95 by nearest rank", () => { | |
| 189 | const v = Array.from({ length: 100 }, (_, i) => i + 1); | |
| 190 | expect(percentile(v, 50)).toBe(50); | |
| 191 | expect(percentile(v, 95)).toBe(95); | |
| 192 | expect(percentile([7], 95)).toBe(7); | |
| 193 | }); | |
| 194 | }); | |
| 195 | ||
| 196 | describe("a fake serial leader through the same path as the page", () => { | |
| 197 | const KEY = "test-identity-key"; | |
| 198 | const identity = `u-alice|alice|user|${Date.now() + 3_600_000}`; | |
| 199 | const ALICE = { "x-forge-identity": identity, "x-forge-signature": sign(identity, KEY) }; | |
| 200 | let simulation: RunningServer; | |
| 201 | let relay: RunningRelay; | |
| 202 | let lobby: Lobby; | |
| 203 | const shut = (s: { close(): Promise<void> } | undefined) => (s ? Promise.race([s.close(), sleep(500)]) : undefined); | |
| 204 | beforeAll(async () => { | |
| 205 | const sim = new FakeBus([1, 2, 6]); | |
| 206 | sim.servos.get(1)!.set([1000 & 0xff, 1000 >> 8, 3000 & 0xff, 3000 >> 8], 9); | |
| 207 | simulation = await startServer(new FeetechBus(sim), { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-teleop-")), "poses.json"), watchdogMs: 2000 }); | |
| 208 | lobby = new Lobby({ identityKey: KEY }); | |
| 209 | relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${simulation.port}`, lobby }); | |
| 210 | }); | |
| 211 | afterAll(async () => { | |
| 212 | await shut(relay); | |
| 213 | lobby.close(); | |
| 214 | await shut(simulation); | |
| 215 | }); | |
| 216 | let leader: LeaderArm | undefined; | |
| 217 | let teleop: Teleop | undefined; | |
| 218 | afterEach(async () => { | |
| 219 | teleop?.dispose(); | |
| 220 | await leader?.close().catch(() => undefined); | |
| 221 | lobby.turns.end("u-alice"); | |
| 222 | await sleep(150); | |
| 223 | }); | |
| 224 | const followerAt = async () => { | |
| 225 | const s = (await (await fetch(`http://127.0.0.1:${relay.port}/status`)).json()) as { joints: Array<{ id: number; position: number }> }; | |
| 226 | return Object.fromEntries(s.joints.map((j) => [j.id, j.position])); | |
| 227 | }; | |
| 228 | const until = async (check: () => Promise<boolean>, ms = 3000) => { | |
| 229 | const deadline = Date.now() + ms; | |
| 230 | while (Date.now() < deadline) if (await check()) return true; | |
| 231 | else await sleep(20); | |
| 232 | return false; | |
| 233 | }; | |
| 234 | /** The page's wiring: the leader read at 60 Hz, mapped, handed to teleop; an unplug tells teleop the leader is gone. */ | |
| 235 | const drive = async (servos: FakeBus, port: FakeSerialPort) => { | |
| 236 | leader = await LeaderArm.open(port, { ids: [1, 2, 6] }); | |
| 237 | const limits = (await (await fetch(`http://127.0.0.1:${relay.port}/limits`)).json()) as Record<string, { min: number; max: number }>; | |
| 238 | const t = (teleop = new Teleop({ | |
| 239 | connect: () => new WebSocket(`ws://127.0.0.1:${relay.port}/follow`, { headers: ALICE }) as unknown as SocketLike, | |
| 240 | mayDrive: () => lobby.turns.isDriver("u-alice"), | |
| 241 | })); | |
| 242 | t.setLeader(true); | |
| 243 | const l = leader; | |
| 244 | const reading = l.follow((pose) => t.pose(l.targets(pose, limits)), { hz: 60 }).catch((err: Error) => t.setLeader(false, err.message)); | |
| 245 | return { teleop: t, reading, servos }; | |
| 246 | }; | |
| 247 | ||
| 248 | test("moves the simulated follower, the gripper too, and nothing is sent outside the turn", async () => { | |
| 249 | const servos = leaderServos({ 1: [1000, 3000, 2600], 2: [1000, 3000, 2100], 6: [2000, 3000, 2500] }); | |
| 250 | const port = new FakeSerialPort(servos); | |
| 251 | const before = await followerAt(); | |
| 252 | const { teleop } = await drive(servos, port); | |
| 253 | teleop.set(true); | |
| 254 | await sleep(300); | |
| 255 | expect(teleop.state).toBe("waiting"); | |
| 256 | expect(await followerAt()).toEqual(before); | |
| 257 | ||
| 258 | lobby.turns.join({ id: "u-alice", name: "alice", owner: false }); | |
| 259 | teleop.update(); | |
| 260 | expect(await until(async () => (await followerAt())[1] === 2600)).toBe(true); | |
| 261 | expect(await followerAt()).toEqual({ 1: 2600, 2: 2148, 6: 2048 }); | |
| 262 | place(servos, 6, 3000); // the trigger squeezed all the way: the gripper follows to its end | |
| 263 | expect(await until(async () => (await followerAt())[6]! > 4000)).toBe(true); | |
| 264 | expect(teleop.frames).toBeGreaterThan(0); | |
| 265 | }); | |
| 266 | ||
| 267 | test("unplugging the leader mid-move stops the follower within one frame, and it holds", async () => { | |
| 268 | const servos = leaderServos({ 1: [1000, 3000, 2000], 2: [1000, 3000, 2100], 6: [2000, 3000, 2500] }); | |
| 269 | const port = new FakeSerialPort(servos); | |
| 270 | lobby.turns.join({ id: "u-alice", name: "alice", owner: false }); | |
| 271 | const { teleop, reading } = await drive(servos, port); | |
| 272 | teleop.set(true); | |
| 273 | expect(await until(async () => (await followerAt())[1] === 2000)).toBe(true); | |
| 274 | // The leader swings faster than the follower may turn, so the follower is still on its way when the cable comes out. | |
| 275 | for (let t = 2000; t <= 2900; t += 60) { | |
| 276 | place(servos, 1, t); | |
| 277 | await sleep(16); | |
| 278 | } | |
| 279 | port.unplug(); | |
| 280 | await reading; | |
| 281 | expect(teleop.on).toBe(false); | |
| 282 | await sleep(1000 / 30 + 50); // one frame, and the stream's close reaching the arm server | |
| 283 | const stopped = (await followerAt())[1]!; | |
| 284 | await sleep(400); | |
| 285 | const later = (await followerAt())[1]!; | |
| 286 | expect(later).toBe(stopped); | |
| 287 | expect(later).toBeLessThan(2900); | |
| 288 | }); | |
| 289 | }); |
Software/so101/src/teleop.ts added +271
| @@ -0,0 +1,271 @@ | ||
| 1 | /** | |
| 2 | * Teleop: the connected leader arm drives the follower live. The page imports this module itself (served by | |
| 3 | * src/browser-modules.ts) and hands it what it cannot know on its own: how to open the /follow stream, whether it is | |
| 4 | * this person's turn, and whether the tab is on screen. It touches no DOM. | |
| 5 | * | |
| 6 | * The leader is read at 60 Hz or faster and every reading is handed to `pose`; frames go out at most `hz` times a | |
| 7 | * second (30 by default), the latest reading winning, never queued. Sending is allowed only while all of these hold: | |
| 8 | * the switch is on, a leader is connected, it is your turn and the tab is visible. The moment one fails, the stream is | |
| 9 | * closed on purpose and the arm server holds where the arm is (a clean close holds; only a lost one parks). Teleop | |
| 10 | * comes back by itself when the tab is shown again or a dropped socket reconnects, and the server eases the arm over to | |
| 11 | * the leader's pose instead of snapping. A turn that ends, an unplugged leader, or a stream the server ended turns the | |
| 12 | * switch off. | |
| 13 | * | |
| 14 | * Lag: each frame sent remembers when its leader reading was taken. When the follower's reported position matches an | |
| 15 | * earlier frame but not the latest one, the follower is that far behind the leader; `lag()` summarises the samples. | |
| 16 | */ | |
| 17 | ||
| 18 | /** Servo id → follower target, ticks. */ | |
| 19 | export type Frame = Record<string, number>; | |
| 20 | ||
| 21 | /** The part of a browser WebSocket teleop uses; a test hands in the `ws` package's, or a fake. */ | |
| 22 | export interface SocketLike { | |
| 23 | readonly readyState: number; | |
| 24 | send(data: string): void; | |
| 25 | close(code?: number, reason?: string): void; | |
| 26 | addEventListener(type: "open", listener: () => void): void; | |
| 27 | addEventListener(type: "close", listener: (event: { code: number; reason: string }) => void): void; | |
| 28 | } | |
| 29 | ||
| 30 | /** off: the switch is off. waiting: on, but it is not your turn yet. paused: on, your turn, the tab is hidden. */ | |
| 31 | export type TeleopState = "off" | "waiting" | "paused" | "connecting" | "live"; | |
| 32 | ||
| 33 | export interface TeleopOptions { | |
| 34 | /** Open the /follow stream. */ | |
| 35 | connect(): SocketLike; | |
| 36 | /** Whether this person may drive now: their turn, or a run with nobody to sign in as. */ | |
| 37 | mayDrive(): boolean; | |
| 38 | /** Whether the page is on screen; hidden, nothing is sent. Always visible by default. */ | |
| 39 | visible?(): boolean; | |
| 40 | /** Frames per second at most; latest wins. */ | |
| 41 | hz?: number; | |
| 42 | /** How long after a dropped socket teleop reconnects, ms. */ | |
| 43 | retryMs?: number; | |
| 44 | now?(): number; | |
| 45 | /** Told every change of state, with why it changed when that is worth saying. */ | |
| 46 | onChange?(state: TeleopState, note: string | null): void; | |
| 47 | } | |
| 48 | ||
| 49 | export interface Lag { | |
| 50 | p50: number; | |
| 51 | p95: number; | |
| 52 | n: number; | |
| 53 | } | |
| 54 | ||
| 55 | interface Sent { | |
| 56 | at: number; | |
| 57 | frame: Frame; | |
| 58 | } | |
| 59 | ||
| 60 | const OPEN = 1; | |
| 61 | /** How far back a follower position is matched against what was sent, ms. */ | |
| 62 | const WINDOW_MS = 2000; | |
| 63 | /** Ticks within which a follower joint counts as at a frame's target. */ | |
| 64 | export const TOLERANCE = 20; | |
| 65 | const SAMPLES = 300; | |
| 66 | ||
| 67 | /** The largest difference between two poses over the joints both name, ticks; Infinity when they share none. */ | |
| 68 | function gap(a: Frame, b: Frame): number { | |
| 69 | let worst = -1; | |
| 70 | for (const [id, t] of Object.entries(a)) { | |
| 71 | const u = b[id]; | |
| 72 | if (typeof u === "number") worst = Math.max(worst, Math.abs(t - u)); | |
| 73 | } | |
| 74 | return worst < 0 ? Infinity : worst; | |
| 75 | } | |
| 76 | ||
| 77 | /** | |
| 78 | * One leader→follower lag reading, ms, or null when this position says nothing: the follower has caught up with the | |
| 79 | * latest frame, matches none, or the leader has not moved far enough since the frame it matches to tell them apart. | |
| 80 | * @param sent frames in the order they were sent, each with when its leader reading was taken | |
| 81 | * @param follower where the follower was, read at `at` | |
| 82 | */ | |
| 83 | export function lagSample(sent: readonly Sent[], follower: Frame, at: number, tolerance = TOLERANCE): number | null { | |
| 84 | const latest = sent[sent.length - 1]; | |
| 85 | if (!latest) return null; | |
| 86 | for (let i = sent.length - 1; i >= 0; i--) { | |
| 87 | const s = sent[i]!; | |
| 88 | if (at - s.at > WINDOW_MS) break; | |
| 89 | if (gap(s.frame, follower) > tolerance) continue; | |
| 90 | if (s === latest || gap(s.frame, latest.frame) <= 2 * tolerance) return null; | |
| 91 | return Math.max(0, at - s.at); | |
| 92 | } | |
| 93 | return null; | |
| 94 | } | |
| 95 | ||
| 96 | /** The p-th percentile (0-100) of `values`, nearest rank. */ | |
| 97 | export function percentile(values: readonly number[], p: number): number { | |
| 98 | if (!values.length) return NaN; | |
| 99 | const sorted = [...values].sort((a, b) => a - b); | |
| 100 | return sorted[Math.min(sorted.length - 1, Math.max(0, Math.ceil((p / 100) * sorted.length) - 1))]!; | |
| 101 | } | |
| 102 | ||
| 103 | export class Teleop { | |
| 104 | /** Frames sent since teleop last went live. */ | |
| 105 | frames = 0; | |
| 106 | : TeleopState = "off"; | |
| 107 | = false; | |
| 108 | = false; | |
| 109 | /** This switch-on has driven: losing the turn then turns it off, rather than waiting for the next turn. */ | |
| 110 | = false; | |
| 111 | : SocketLike | null = null; | |
| 112 | : Sent | null = null; | |
| 113 | = -Infinity; | |
| 114 | = -Infinity; | |
| 115 | : ReturnType<typeof setTimeout> | undefined; | |
| 116 | : ReturnType<typeof setTimeout> | undefined; | |
| 117 | : Sent[] = []; | |
| 118 | : number[] = []; | |
| 119 | readonly : TeleopOptions; | |
| 120 | readonly : number; | |
| 121 | readonly : () => number; | |
| 122 | ||
| 123 | constructor(options: TeleopOptions) { | |
| 124 | this. = options; | |
| 125 | this. = 1000 / (options.hz ?? 30); | |
| 126 | this. = options.now ?? (() => performance.now()); | |
| 127 | } | |
| 128 | ||
| 129 | get state(): TeleopState { | |
| 130 | return this.; | |
| 131 | } | |
| 132 | ||
| 133 | /** Whether the switch is on (live or not). */ | |
| 134 | get on(): boolean { | |
| 135 | return this.; | |
| 136 | } | |
| 137 | ||
| 138 | /** Whether a stream is open or opening: the arm is the leader's. */ | |
| 139 | get driving(): boolean { | |
| 140 | return this. === "live" || this. === "connecting"; | |
| 141 | } | |
| 142 | ||
| 143 | /** The switch. On outside your turn waits for it; the page decides who may ask for one. */ | |
| 144 | set(on: boolean, note: string | null = null): void { | |
| 145 | if (on === this.) return this.update(); | |
| 146 | this. = on; | |
| 147 | this. = false; | |
| 148 | this.update(note); | |
| 149 | } | |
| 150 | ||
| 151 | /** A leader was connected or went away (unplugged, stopped answering, disconnected). */ | |
| 152 | setLeader(connected: boolean, note: string | null = null): void { | |
| 153 | this. = connected; | |
| 154 | if (!connected) { | |
| 155 | this. = null; | |
| 156 | if (this.) return this.set(false, note); | |
| 157 | } | |
| 158 | this.update(); | |
| 159 | } | |
| 160 | ||
| 161 | /** A fresh leader reading, mapped onto the follower. Sent at the next slot if teleop is live; otherwise only kept. */ | |
| 162 | pose(frame: Frame, at = this.()): void { | |
| 163 | this. = { at, frame }; | |
| 164 | if (this. === "live") this.(); | |
| 165 | } | |
| 166 | ||
| 167 | /** Where the follower is, read at `at` (the middle of the status request): one more lag sample when it says something. */ | |
| 168 | observe(follower: Frame, at = this.()): number | null { | |
| 169 | const lag = this.driving ? lagSample(this., follower, at) : null; | |
| 170 | if (lag !== null) { | |
| 171 | this..push(lag); | |
| 172 | if (this..length > SAMPLES) this..shift(); | |
| 173 | } | |
| 174 | return lag; | |
| 175 | } | |
| 176 | ||
| 177 | lag(): Lag | null { | |
| 178 | if (!this..length) return null; | |
| 179 | return { p50: Math.round(percentile(this., 50)), p95: Math.round(percentile(this., 95)), n: this..length }; | |
| 180 | } | |
| 181 | ||
| 182 | /** Look again at the turn and the tab: call it whenever either may have changed. */ | |
| 183 | update(note: string | null = null): void { | |
| 184 | const may = this..mayDrive(); | |
| 185 | if (this. && !may && this.) { | |
| 186 | this. = false; | |
| 187 | note ??= "your turn ended"; | |
| 188 | } | |
| 189 | const allowed = this. && this. && may && (this..visible?.() ?? true); | |
| 190 | if (!allowed) { | |
| 191 | this.(); | |
| 192 | return this.(!this. ? "off" : !this. ? "off" : !may ? "waiting" : "paused", note); | |
| 193 | } | |
| 194 | this. = true; | |
| 195 | if (!this.) this.(); | |
| 196 | else if (note) this..onChange?.(this., note); | |
| 197 | } | |
| 198 | ||
| 199 | (): void { | |
| 200 | clearTimeout(this.); | |
| 201 | this. = undefined; | |
| 202 | const ws = this..connect(); | |
| 203 | this. = ws; | |
| 204 | this.frames = 0; | |
| 205 | this.("connecting", null); | |
| 206 | ws.addEventListener("open", () => { | |
| 207 | if (this. !== ws) return; | |
| 208 | this.("live", null); | |
| 209 | this.(); | |
| 210 | }); | |
| 211 | ws.addEventListener("close", ({ code, reason }) => { | |
| 212 | if (this. !== ws) return; | |
| 213 | this. = null; | |
| 214 | clearTimeout(this.); | |
| 215 | this. = undefined; | |
| 216 | // Ended by the server on purpose (the turn, a stop, an emergency stop, a busy arm): off, with its reason. | |
| 217 | if (code === 1000 || code === 1013 || code >= 4000) return this.set(false, reason || "the arm server ended teleop"); | |
| 218 | // Lost: nothing is sent, the arm server holds or parks, and teleop tries again shortly. | |
| 219 | this.("paused", `the connection to the arm dropped${reason ? ` (${reason})` : ""}; trying again`); | |
| 220 | this. = setTimeout(() => { | |
| 221 | this. = undefined; | |
| 222 | this.update(); | |
| 223 | }, this..retryMs ?? 1000); | |
| 224 | }); | |
| 225 | } | |
| 226 | ||
| 227 | /** Stop sending at once and close the stream on purpose: the arm server holds where the arm is. */ | |
| 228 | (): void { | |
| 229 | clearTimeout(this.); | |
| 230 | this. = undefined; | |
| 231 | clearTimeout(this.); | |
| 232 | this. = undefined; | |
| 233 | const ws = this.; | |
| 234 | this. = null; | |
| 235 | ws?.close(1000, "teleop stopped"); | |
| 236 | this. = []; | |
| 237 | } | |
| 238 | ||
| 239 | (): void { | |
| 240 | if (this. !== undefined || !this.) return; | |
| 241 | const wait = Math.max(0, this. + this. - this.()); | |
| 242 | this. = setTimeout(() => this.(), wait); | |
| 243 | } | |
| 244 | ||
| 245 | (): void { | |
| 246 | this. = undefined; | |
| 247 | const ws = this.; | |
| 248 | const latest = this.; | |
| 249 | if (!ws || ws.readyState !== OPEN || this. !== "live" || !latest || latest.at <= this.) return; | |
| 250 | if (!Object.keys(latest.frame).length) return; | |
| 251 | ws.send(JSON.stringify(latest.frame)); | |
| 252 | this.frames++; | |
| 253 | this. = this.(); | |
| 254 | this. = latest.at; | |
| 255 | this..push(latest); | |
| 256 | while (this..length && latest.at - this.[0]!.at > WINDOW_MS) this..shift(); | |
| 257 | } | |
| 258 | ||
| 259 | (state: TeleopState, note: string | null): void { | |
| 260 | const changed = state !== this.; | |
| 261 | this. = state; | |
| 262 | if (changed || note) this..onChange?.(state, note); | |
| 263 | } | |
| 264 | ||
| 265 | /** Off for good: the page is going away. */ | |
| 266 | dispose(): void { | |
| 267 | this. = false; | |
| 268 | this.(); | |
| 269 | this. = "off"; | |
| 270 | } | |
| 271 | } |
Software/so101/src/viewer.html +99 −38
| @@ -104,6 +104,18 @@ | ||
| 104 | 104 | .tab { flex: 1; min-height: 0; overflow: auto; overscroll-behavior: contain; padding: 12px calc(12px + var()) 16px calc(12px + var()); display: flex; flex-direction: column; gap: 12px; } |
| 105 | 105 | .tab h2 { margin: 4px 0 0; font-size: 13px; font-weight: 650; color: var(); text-transform: uppercase; letter-spacing: .06em; } |
| 106 | 106 | .tab p.note { margin: 0; color: var(); font-size: 13px; } |
| 107 | /* teleop: a switch beside the leader, and while it drives, each joint's leader (ring) and follower (dot) on one track */ | |
| 108 | label.switch { display: inline-flex; gap: 10px; align-items: center; min-height: var(); cursor: pointer; font-weight: 600; } | |
| 109 | label.switch input { appearance: none; margin: 0; width: 44px; height: 26px; border-radius: 13px; background: var(); border: 1px solid var(); position: relative; cursor: pointer; flex: none; } | |
| 110 | label.switch input::after { content: ""; position: absolute; top: 2px; left: 2px; width: 20px; height: 20px; border-radius: 50%; background: var(); transition: transform .15s, background .15s; } | |
| 111 | label.switch input:checked { background: var(); border-color: var(); } label.switch input:checked::after { transform: translateX(18px); background: var(); } | |
| 112 | label.switch input:disabled { opacity: .4; cursor: default; } | |
| 113 | @media (prefers-reduced-motion: reduce) { label.switch input::after { transition: none; } } | |
| 114 | .lag { color: var(); font-size: 13px; font-variant-numeric: tabular-nums; } .lag.slow { color: var(); } | |
| 115 | #teleopLive { display: grid; grid-template-columns: max-content 1fr; gap: 6px 10px; align-items: center; font-size: 12px; color: var(); margin: 10px 0; } | |
| 116 | #teleopLive .track { position: relative; height: 14px; border-radius: 7px; background: var(); border: 1px solid var(); overflow: hidden; } | |
| 117 | #teleopLive .track i { position: absolute; top: 2px; left: calc(var(, .5) * 100% - 4px); width: 8px; height: 8px; border-radius: 50%; } | |
| 118 | #teleopLive .track i.l { border: 2px solid var(); top: 1px; width: 10px; height: 10px; left: calc(var(, .5) * 100% - 5px); } #teleopLive .track i.f { background: var(); } | |
| 107 | 119 | #tabs { flex: none; display: flex; border-top: 1px solid var(); background: var(); padding: 0 calc(var() + var()) var() var(); } |
| 108 | 120 | #tabs button { flex: 1; min-width: 0; min-height: 56px; border: 0; border-radius: 0; background: none; color: var(); font-size: 14px; font-weight: 600; position: relative; padding: 0 4px; } |
| 109 | 121 | #tabs button[aria-selected=true] { color: var(); } |
| @@ -280,8 +292,9 @@ | ||
| 280 | 292 | <ol id="queue" aria-label="Queue"></ol> |
| 281 | 293 | <p class="note">A turn lasts a few minutes and ends early when the arm sits idle. When yours comes, the Controls tab appears.</p> |
| 282 | 294 | <h2>Leader arm</h2> |
| 283 | <div class="row"><button id="leaderBtn" title="read a leader arm plugged into this computer or phone (Chrome, Edge)">Connect leader arm</button><button id="leaderCal" hidden title="teach this page your own leader arm: once, kept with your name">Calibrate leader</button></div> | |
| 284 | <p class="note">Plug an SO-101 leader into this computer or Android phone. The 3D model mirrors it while you wait; on your turn, Drive with leader in Controls moves the arm with it.</p> | |
| 295 | <div class="row"><button id="leaderBtn" title="read a leader arm plugged into this computer or phone (Chrome, Edge)">Connect leader arm</button><label class="switch" id="teleopRow" hidden title="on your turn, the arm follows the leader live"><input type="checkbox" role="switch" id="teleop"> Teleop</label><button id="leaderCal" hidden title="teach this page your own leader arm: once, kept with your name">Calibrate leader</button></div> | |
| 296 | <p class="lag" id="teleopLag" hidden></p> | |
| 297 | <p class="note">Plug an SO-101 leader into this computer or Android phone. The 3D model mirrors it while you wait; switch Teleop on and, on your turn, the arm follows it live. Hiding this tab or unplugging the leader stops the arm where it is.</p> | |
| 285 | 298 | </div> |
| 286 | 299 | <div class="tab" id="tab-controls" role="tabpanel" aria-labelledby="t-controls" hidden> |
| 287 | 300 | <div id="reach" hidden> |
| @@ -302,8 +315,10 @@ | ||
| 302 | 315 | <button id="releaseBtn" class="danger" title="torque off">Release</button> |
| 303 | 316 | <button id="record" class="rec">● Record</button> |
| 304 | 317 | <button id="play">▶ Play…</button> |
| 305 | <button id="leaderDrive" class="go" hidden title="stream the leader arm's pose to the arm during your turn">Drive with leader</button> | |
| 318 | <button id="leaderDrive" class="go" hidden aria-pressed="false" title="teleop: the arm follows the leader live during your turn">Teleop</button> | |
| 306 | 319 | </div> |
| 320 | <div id="teleopLive" hidden aria-label="Leader (ring) and follower (dot) per joint"></div> | |
| 321 | <p class="lag" id="teleopLag2" hidden></p> | |
| 307 | 322 | <div id="readout"></div> |
| 308 | 323 | </div> |
| 309 | 324 | <div class="tab" id="tab-admin" role="tabpanel" aria-labelledby="t-admin" hidden> |
| @@ -785,7 +800,7 @@ | ||
| 785 | 800 | const r = await api("/estop", {}); |
| 786 | 801 | msg(r.ok ? "EMERGENCY STOP: the arm holds; an admin must clear it" : r.error, true); |
| 787 | 802 | if (stream.ws) stream.ws.close(); |
| 788 | if (leaderStreaming()) stopLeaderDrive(); | |
| 803 | stopLeaderDrive("emergency stop"); | |
| 789 | 804 | } |
| 790 | 805 | async function clearEStop() { |
| 791 | 806 | if (!confirm("Clear the emergency stop? The arm stays where it is; the driver can move it again.")) return; |
| @@ -876,10 +891,12 @@ | ||
| 876 | 891 | |
| 877 | 892 | // ---------------------------------------------------------------- the leader arm, plugged into this computer or phone |
| 878 | 893 | // src/leader-arm.ts, served at /app/: the CLI's own bus code over Web Serial. Connected, the leader is mirrored by the 3D model |
| 879 | // and the joint rows; during your turn "Drive with leader" streams its pose like any stream, through the server's limits and zone. | |
| 880 | let leader = null; // { arm, port, stop, pose, targets, ws, frames, calibration } | |
| 894 | // and the joint rows. With Teleop on, during your turn, src/teleop.ts streams its pose at 30 Hz, latest wins, through the | |
| 895 | // server's limits and zone; the turn ending, a hidden tab, an unplugged leader or a lost socket stop it and the arm holds. | |
| 896 | let leader = null; // { arm, port, stop, pose, targets, calibration } | |
| 897 | let teleop = null; // src/teleop.ts, made when the first leader connects | |
| 881 | 898 | let calib = null; // the calibration card, while it is open |
| 882 | const leaderStreaming = () => Boolean(leader?.ws); | |
| 899 | const leaderStreaming = () => Boolean(teleop?.driving); | |
| 883 | 900 | const leaderMirrored = () => Object.keys(leader?.targets ?? {}).length > 0 && !leaderStreaming(); |
| 884 | 901 | const explain = (text) => { $("leaderWhy").textContent = text; $("leaderHelp").showModal(); }; |
| 885 | 902 | function renderLeader() { |
| @@ -889,15 +906,50 @@ | ||
| 889 | 906 | for (const [id, ticks] of Object.entries(t)) if (NAMES[id] && limits[id] && NAMES[id] !== dragging) viewer.setJointValue(NAMES[id], toRad(Number(id), ticks)); |
| 890 | 907 | } |
| 891 | 908 | function renderLeaderButtons() { |
| 909 | const on = Boolean(teleop?.on); | |
| 892 | 910 | $("leaderBtn").textContent = leader ? "Disconnect leader" : "Connect leader arm"; |
| 893 | $("leaderDrive").hidden = !leader; | |
| 911 | $("leaderDrive").hidden = $("teleopRow").hidden = !leader; | |
| 894 | 912 | $("leaderCal").hidden = !leader; |
| 895 | $("leaderDrive").textContent = leaderStreaming() ? "■ Stop leader" : "Drive with leader"; | |
| 896 | $("tag3").textContent = leader && !leaderStreaming() ? "3D · mirroring your leader arm" : "3D · drag a joint to move it · drag around it to orbit"; | |
| 913 | $("teleop").checked = on; | |
| 914 | $("leaderDrive").textContent = on ? "■ Teleop off" : "Teleop"; | |
| 915 | $("leaderDrive").setAttribute("aria-pressed", String(on)); | |
| 916 | $("teleopLive").hidden = !leaderStreaming(); | |
| 917 | $("tag3").textContent = leaderStreaming() ? "3D · the arm, following your leader" : leader ? "3D · mirroring your leader arm" : "3D · drag a joint to move it · drag around it to orbit"; | |
| 918 | renderLag(); | |
| 919 | } | |
| 920 | /** Leader→follower lag as this browser measures it (src/teleop.ts): the follower's reported pose against the leader readings sent. */ | |
| 921 | function renderLag() { | |
| 922 | const lag = teleop?.lag(), text = lag ? `leader → follower lag p50 ${lag.p50} ms · p95 ${lag.p95} ms (${lag.n})` : leaderStreaming() ? "leader → follower lag: move the leader to measure" : ""; | |
| 923 | for (const id of ["teleopLag", "teleopLag2"]) { $(id).textContent = text; $(id).hidden = !text; $(id).classList.toggle("slow", (lag?.p95 ?? 0) > 200); } | |
| 924 | } | |
| 925 | /** While teleop drives: each joint's leader target (ring) and the follower's reported position (dot) on its range, so lag shows. */ | |
| 926 | const teleopTracks = new Map(); | |
| 927 | function renderTracks() { | |
| 928 | if (!leaderStreaming()) return; | |
| 929 | const t = leader?.targets ?? {}, at = Object.fromEntries((status?.joints ?? []).map((j) => [j.id, j.position])); | |
| 930 | for (const id of Object.keys(limits)) { | |
| 931 | if (!teleopTracks.has(id)) { | |
| 932 | const l = el("i", { className: "l" }), f = el("i", { className: "f" }), track = el("div", { className: "track" }, l, f); | |
| 933 | $("teleopLive").append(el("span", { textContent: NAMES[id] ?? `servo ${id}` }), track); | |
| 934 | teleopTracks.set(id, { l, f }); | |
| 935 | } | |
| 936 | const { min, max } = limits[id], row = teleopTracks.get(id), pct = (v) => String(Math.min(1, Math.max(0, (v - min) / (max - min)))); | |
| 937 | row.l.hidden = t[id] === undefined; if (t[id] !== undefined) row.l.style.setProperty("--at", pct(t[id])); | |
| 938 | row.f.hidden = typeof at[id] !== "number"; if (typeof at[id] === "number") row.f.style.setProperty("--at", pct(at[id])); | |
| 939 | } | |
| 940 | } | |
| 941 | function teleopChanged(state, note) { | |
| 942 | renderLeaderButtons(); | |
| 943 | if (state === "live") msg("teleop: the arm follows your leader. CAUTION: it first eases over to the leader's pose."); | |
| 944 | else if (state === "waiting") msg(note ?? "teleop is on: it drives the arm once you have control"); | |
| 945 | else if (state === "paused") msg(note ?? "teleop paused while this page is hidden; the arm holds", true); | |
| 946 | else if (state === "off") msg(note ? `teleop off: ${note}; the arm holds` : `teleop off after ${teleop.frames} frames; the arm holds`, Boolean(note)); | |
| 947 | if (status) applyModel(status); // driving, the model shows the arm; not, it mirrors the leader again | |
| 948 | renderLeader(); | |
| 897 | 949 | } |
| 898 | 950 | async function connectLeader() { |
| 899 | let lib; | |
| 900 | try { lib = await import("/app/leader-arm.ts"); } | |
| 951 | let lib, tele; | |
| 952 | try { [lib, tele] = await Promise.all([import("/app/leader-arm.ts"), import("/app/teleop.ts")]); } | |
| 901 | 953 | catch { return explain("This server could not hand the page the leader arm's code. Meanwhile you can drive with the sliders, the − and + buttons, or by dragging a joint of the 3D model."); } |
| 902 | 954 | const support = lib.leaderSupport({ serial: "serial" in navigator, secure: isSecureContext, userAgent: navigator.userAgent, touchPoints: navigator.maxTouchPoints }); |
| 903 | 955 | if (!support.ok) return explain(support.why); |
| @@ -909,22 +961,29 @@ | ||
| 909 | 961 | catch (err) { return msg(`leader: ${err.message}`, true); } |
| 910 | 962 | // The driver's own calibration when they have one; otherwise the leader servos' stored ranges, LeRobot's way. |
| 911 | 963 | const mine = await api("/leader/calibration").catch(() => null); |
| 912 | const l = { arm, port, stop: new AbortController(), pose: null, targets: null, ws: null, frames: 0, calibration: mine?.ok && mine.joints ? mine : null }; | |
| 964 | const l = { arm, port, stop: new AbortController(), pose: null, targets: null, calibration: mine?.ok && mine.joints ? mine : null }; | |
| 913 | 965 | leader = l; |
| 966 | teleop ??= new tele.Teleop({ | |
| 967 | connect: () => new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`), | |
| 968 | mayDrive, visible: () => document.visibilityState === "visible", onChange: teleopChanged, | |
| 969 | }); | |
| 970 | teleop.setLeader(true); | |
| 914 | 971 | renderLeaderButtons(); |
| 915 | 972 | const how = l.calibration ? ", with your calibration" : arm.limits.size ? "; Calibrate leader for a clean mapping" : "; it has no stored ranges, so Calibrate leader before it can drive"; |
| 916 | msg(`leader connected and mirrored in 3D${how}. ${mayDrive() ? "Press Drive with leader to move the arm with it." : "It drives the arm once you have control."}`); | |
| 973 | msg(`leader connected and mirrored in 3D${how}. Switch Teleop on and the arm follows it ${mayDrive() ? "now" : "on your turn"}.`); | |
| 974 | let frame = 0; | |
| 917 | 975 | l.arm.follow((pose) => { |
| 918 | 976 | l.pose = pose; |
| 919 | 977 | l.targets = l.arm.targets(pose, limits, l.calibration); |
| 920 | 978 | if (calib) return calibReading(pose); |
| 921 | if (l.ws?.readyState === 1 && Object.keys(l.targets).length) { l.ws.send(JSON.stringify(l.targets)); l.frames++; } | |
| 979 | teleop.pose(l.targets); | |
| 980 | if (frame++ % 4 === 0) renderTracks(); // the leader is read at 60 Hz; the tracks need no more than 15 | |
| 922 | 981 | renderLeader(); |
| 923 | 982 | }, { hz: 60, signal: l.stop.signal }).catch((err) => disconnectLeader(`the leader stopped answering (${err.message})`)); |
| 924 | 983 | } |
| 925 | 984 | async function disconnectLeader(reason) { |
| 926 | 985 | const l = leader; if (!l) return; |
| 927 | stopLeaderDrive(); | |
| 986 | teleop?.setLeader(false, reason ?? "the leader was disconnected"); // first: the stream closes before anything else, and the arm holds | |
| 928 | 987 | closeCalib(); |
| 929 | 988 | leader = null; |
| 930 | 989 | l.stop.abort(); |
| @@ -933,20 +992,17 @@ | ||
| 933 | 992 | if (status) applyModel(status); |
| 934 | 993 | msg(`leader disconnected${reason ? `: ${reason}` : ""}`, Boolean(reason)); |
| 935 | 994 | } |
| 995 | /** Teleop on. The owner takes control at once (ending whoever drives); anyone else teleops on their own turn only. */ | |
| 936 | 996 | function driveLeader() { |
Non-owners are refused here when it is not their turn, while the README and teleop.ts say the switch waits for the turn. Pick one behaviour and make the docs match. |
||
| 937 | const l = leader; if (!l || l.ws) return; | |
| 938 | if (calib) return msg("finish or cancel the leader calibration first", true); | |
| 939 | if (!mayDrive()) return msg("take control first: the leader drives the arm only during your turn", true); | |
| 940 | const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`); | |
| 941 | l.ws = ws; l.frames = 0; | |
| 942 | ws.onopen = () => msg("driving with the leader. CAUTION: the arm first eases over to the leader's pose."); | |
| 943 | ws.onclose = (e) => { | |
| 944 | if (l.ws !== ws) return; | |
| 945 | l.ws = null; | |
| 946 | renderLeaderButtons(); | |
| 947 | msg(`the leader stopped driving${e.reason ? `: ${e.reason}` : ""}; the arm holds, and the 3D model mirrors the leader again`, e.code !== 1000); | |
| 948 | }; | |
| 949 | renderLeaderButtons(); | |
| 997 | const l = leader; if (!l || !teleop || teleop.on) return renderLeaderButtons(); | |
| 998 | const refuse = (text) => { msg(text, true); renderLeaderButtons(); }; | |
| 999 | if (calib) return refuse("finish or cancel the leader calibration first"); | |
| 1000 | if (!Object.keys(l.targets ?? {}).length) return refuse("the leader maps onto no joint yet: Calibrate leader first"); | |
| 1001 | if (!mayDrive()) { | |
| 1002 | if (!isOwner()) return refuse("teleop drives the arm on your turn: take control to join the line, then switch it on"); | |
| 1003 | say({ type: "join" }); // the owner takes control now; teleop starts as soon as the turn is theirs | |
| 1004 | } | |
| 1005 | teleop.set(true); | |
| 950 | 1006 | } |
| 951 | 1007 | // ---------------------------------------------------------------- leader calibration (src/leader-map.js) |
| 952 | 1008 | // Once per driver: the leader in the follower's middle pose, then each joint at each end the model shows. The ends |
| @@ -1046,15 +1102,14 @@ | ||
| 1046 | 1102 | |
| 1047 | 1103 | $("leaderCal").onclick = () => (calib ? undefined : void openCalib()); |
| 1048 | 1104 | |
| 1049 | function stopLeaderDrive() { | |
| 1050 | const ws = leader?.ws; if (!ws) return; | |
| 1051 | leader.ws = null; | |
| 1052 | ws.close(); | |
| 1053 | renderLeaderButtons(); | |
| 1054 | msg(`the leader stopped driving after ${leader.frames} frames; the arm holds`); | |
| 1105 | function stopLeaderDrive(note) { | |
| 1106 | if (teleop?.on) teleop.set(false, note ?? null); | |
| 1055 | 1107 | } |
| 1056 | 1108 | $("leaderBtn").onclick = () => (leader ? disconnectLeader() : connectLeader()); |
| 1057 | $("leaderDrive").onclick = () => (leaderStreaming() ? stopLeaderDrive() : driveLeader()); | |
| 1109 | $("leaderDrive").onclick = () => (teleop?.on ? stopLeaderDrive() : driveLeader()); | |
| 1110 | $("teleop").onchange = () => ($("teleop").checked ? driveLeader() : stopLeaderDrive()); | |
| 1111 | // A hidden page sends nothing: the stream closes and the arm holds; shown again, teleop picks up where the leader is. | |
| 1112 | document.addEventListener("visibilitychange", () => teleop?.update()); | |
| 1058 | 1113 | navigator.serial?.addEventListener("disconnect", (e) => { if (e.target === leader?.port) disconnectLeader("the leader was unplugged"); }); |
| 1059 | 1114 | |
| 1060 | 1115 | // ---------------------------------------------------------------- the 3D model |
| @@ -1267,6 +1322,7 @@ | ||
| 1267 | 1322 | ["since", (m.since ?? "").replace("T", " ").slice(0, 19)], |
| 1268 | 1323 | ["jams", c.jam], ["zone blocks", c.block], ["stalls on moves", c.stall], ["lost servos", c.missing], ["hot joints", c.hot], |
| 1269 | 1324 | ["sessions", `${c.stream} streams · ${c.move} moves · ${c.play} playbacks · ${c.record} recordings`], |
| 1325 | ["teleop lag, this browser", (() => { const lag = teleop?.lag(); return lag ? `p50 ${lag.p50} ms · p95 ${lag.p95} ms over ${lag.n} samples` : "—"; })()], | |
| 1270 | 1326 | ["closest to the table", m.closest ? `${(m.closest.z * 1000).toFixed(0)} mm (${m.closest.link.replace(/_link$/, "")}, ${m.closest.at.slice(11, 19)})` : "—"], |
| 1271 | 1327 | ].map(([k, v]) => `<b>${k}</b><span>${v}</span>`).join(""); |
| 1272 | 1328 | $("peaks").innerHTML = "<tr><th>joint</th><th>peak load</th><th>peak temperature</th></tr>" + Object.entries(m.peaks).map(([id, p]) => `<tr><td>${NAMES[id] ?? id}</td><td>${p.load.toFixed(0)} %</td><td>${p.temperature} °C</td></tr>`).join(""); |
| @@ -1303,7 +1359,7 @@ | ||
| 1303 | 1359 | for (const id of ["zoneRow", "trims", "safety", "jointLimits"]) $(id).inert = !owner; |
| 1304 | 1360 | viewer.toggleAttribute("disable-dragging", !can); |
| 1305 | 1361 | if (!can) reachEnd(); |
| 1306 | if (!can && leaderStreaming()) stopLeaderDrive(); // the server closes it too; the page need not wait for that | |
| 1362 | teleop?.update(); // the turn ended: teleop stops at once (the server closes it too; the page need not wait for that) | |
| 1307 | 1363 | // Your turn just came: open the controls. It just ended: back to the chat. |
| 1308 | 1364 | if (mayDrive() !== drove) { drove = mayDrive(); if (relayed) showTab(drove ? "controls" : "chat"); } |
| 1309 | 1365 | else if ($(`t-${tab}`).hidden) showTab(tab); |
| @@ -1645,7 +1701,12 @@ | ||
| 1645 | 1701 | status = d; |
| 1646 | 1702 | if (Boolean(d.estop) !== wasStopped) applyLock(); |
| 1647 | 1703 | $("dot").className = d.reconnecting ? "dot warn" : "dot on"; |
| 1648 | $("state").textContent = d.reconnecting ? "reconnecting…" : d.driving === null ? "idle" : d.driving === "record" ? `recording ${d.recording}` : leaderStreaming() ? `leader · ${leader.frames} frames` : d.driving; | |
| 1704 | $("state").textContent = d.reconnecting ? "reconnecting…" : d.driving === null ? "idle" : d.driving === "record" ? `recording ${d.recording}` : leaderStreaming() ? `teleop · ${teleop.frames} frames` : d.driving; | |
| 1705 | if (leaderStreaming()) { | |
| 1706 | // The follower's pose was read somewhere between asking and the answer: the middle is the best guess of when. | |
| 1707 | teleop.observe(Object.fromEntries(d.joints.filter((j) => typeof j.position === "number").map((j) => [j.id, j.position])), (started + performance.now()) / 2); | |
| 1708 | renderTracks(); renderLag(); | |
| 1709 | } | |
| 1649 | 1710 | $("estop").classList.toggle("on", Boolean(d.estop)); $("estop").textContent = d.estop ? "Stopped" : "Stop"; |
| 1650 | 1711 | $("record").textContent = d.driving === "record" ? "■ Stop recording" : "● Record"; |
| 1651 | 1712 | for (const id of ["hold", "releaseBtn", "play"]) $(id).disabled = d.driving !== null; |
new above
offline
aria-label on a div with no role is not announced. Add role="group" and a visually hidden text summary (e.g. 'base: leader 2600, follower 2540'), updated at a low rate.