Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
`so101 teleop` drives the follower from the leader in one process at 60 Hz, with a joint table, lag and space to STOP #574
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/571-so101-teleop-drives-the-follower-from-th → main 4 commits fork 7 threads · 7 unresolved
Software/so101/README.md +14
| @@ -2,6 +2,20 @@ | ||
| 2 | 2 | |
| 3 | 3 | `bench/fake-serve.ts` is the hosted app (the page, the room, the relay); `so101 serve` is the arm server on the Pi. |
| 4 | 4 | |
| 5 | ## Plug in and play | |
| 6 | ||
| 7 | Two SO-101 arms on USB, calibrated as in https://huggingface.co/docs/lerobot/so101 (the same assembly and calibration guide): | |
| 8 | ||
| 9 | ||
| 10 | bun so101 teleop [--leader /dev/ttyACM0 --follower /dev/ttyACM1] [--hz 60] | |
| 11 | ||
| 12 | ||
| 13 | The leader arm drives the follower in this one process at 60 Hz, with a joint table and the lag on screen. **Space** stops the | |
| 14 | follower (it holds, torque on); space again re-reads where it is and ramps toward the leader a few ticks a frame. **q** quits and | |
| 15 | leaves the follower holding with torque on. Add `--save` once and the two arms are remembered in `~/.config/so101/arms.json` by USB | |
| 16 | serial number; after that `so101 teleop` alone finds them, on whichever ports they are plugged in. Without ports and without | |
| 17 | a saved choice, it says to pass `--leader` and `--follower`. | |
| 18 | ||
| 5 | 19 | ## Open / Closed |
| 6 | 20 | |
| 7 | 21 | The owner can close the whole app when the shop is dark: on `/admin`, **Open / Closed** has **Close the arm** with an optional note for visitors ("Back Saturday 10am CT", at most 140 characters) and an optional "back around" time (the owner's local time); **Open the arm** undoes it. In the terminal: `/close [note]` and `/open`. Over `/live`, the owner sends `{"type": "open", "open": false, "note": "…", "reopenAt": "2026-10-31T10:00-05:00"}`. Every change is in the admin log; the switch is kept in `so101/open.json` under `FORGE_APP_DATA` (src/opening.ts). |
Software/so101/src/arms.test.ts added +55
| @@ -0,0 +1,55 @@ | ||
| 1 | import { 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 { adaptersOf, resolveRoles, rolesOf, loadRoles, saveRoles, savedRoles } from "./arms.ts"; | |
| 6 | ||
| 7 | test("adapters: USB serial only, one per board, macOS callout device", () => { | |
| 8 | const list = [ | |
| 9 | { path: "/dev/tty.usbmodem1", serialNumber: "A" }, | |
| 10 | { path: "/dev/cu.usbmodem1", serialNumber: "A" }, | |
| 11 | { path: "/dev/cu.Bluetooth-Incoming-Port" }, | |
| 12 | { path: "/dev/cu.usbmodem2", serialNumber: "B" }, | |
| 13 | ]; | |
| 14 | expect(adaptersOf(list, "darwin")).toEqual([ | |
| 15 | { path: "/dev/cu.usbmodem1", serial: "A" }, | |
| 16 | { path: "/dev/cu.usbmodem2", serial: "B" }, | |
| 17 | ]); | |
| 18 | }); | |
| 19 | ||
| 20 | test("roles are remembered by serial and found again on other ports", () => { | |
| 21 | const file = join(mkdtempSync(join(tmpdir(), "arms-")), "nested", "arms.json"); | |
| 22 | expect(loadRoles(file)).toBeNull(); | |
| 23 | saveRoles(file, { leader: "B", follower: "A" }); | |
| 24 | const found = savedRoles(loadRoles(file), [{ path: "/dev/x", serial: "A" }, { path: "/dev/y", serial: "B" }]); | |
| 25 | expect(found?.leader.path).toBe("/dev/y"); | |
| 26 | expect(savedRoles(loadRoles(file), [{ path: "/dev/x", serial: "A" }])).toBeNull(); | |
| 27 | }); | |
| 28 | ||
| 29 | test("teleop's ports: the command line wins, else the remembered roles, else a plain error", () => { | |
| 30 | const adapters = [{ path: "/dev/x", serial: "A" }, { path: "/dev/y", serial: "B" }]; | |
| 31 | const saved = { leader: "B", follower: "A" }; | |
| 32 | expect(resolveRoles("/dev/l", "/dev/f", null, [])).toEqual({ leader: "/dev/l", follower: "/dev/f" }); | |
| 33 | expect(resolveRoles(undefined, undefined, saved, adapters)).toEqual({ leader: "/dev/y", follower: "/dev/x" }); | |
| 34 | expect(resolveRoles("/dev/l", undefined, saved, adapters)).toEqual({ leader: "/dev/l", follower: "/dev/x" }); | |
| 35 | expect(() => resolveRoles(undefined, undefined, null, adapters)).toThrow("--save"); | |
| 36 | expect(() => resolveRoles(undefined, undefined, saved, [adapters[0]!])).toThrow("--save"); | |
| 37 | }); | |
| 38 | ||
| 39 | test("--save remembers the serial numbers of the two ports", () => { | |
| 40 | const adapters = [{ path: "/dev/x", serial: "A" }, { path: "/dev/y", serial: "B" }, { path: "/dev/z", serial: null }]; | |
| 41 | expect(rolesOf("/dev/y", "/dev/x", adapters)).toEqual({ leader: "B", follower: "A" }); | |
| 42 | expect(rolesOf("/dev/y", "/dev/z", adapters)).toBeNull(); | |
| 43 | expect(rolesOf("/dev/y", "/dev/y", adapters)).toBeNull(); | |
| 44 | }); | |
| 45 | ||
| 46 | test("--save finds a typed macOS tty path among the cu adapters", () => { | |
| 47 | const adapters = [{ path: "/dev/cu.usbmodem1", serial: "A" }, { path: "/dev/cu.usbmodem2", serial: "B" }]; | |
| 48 | const was = Object.getOwnPropertyDescriptor(process, "platform")!; | |
| 49 | Object.defineProperty(process, "platform", { value: "darwin" }); | |
| 50 | try { | |
| 51 | expect(rolesOf("/dev/tty.usbmodem2", "/dev/tty.usbmodem1", adapters)).toEqual({ leader: "B", follower: "A" }); | |
| 52 | } finally { | |
| 53 | Object.defineProperty(process, "platform", was); | |
| 54 | } | |
| 55 | }); |
Software/so101/src/arms.ts added +81
| @@ -0,0 +1,81 @@ | ||
| 1 | /** Finding the two arms: which USB adapters are arms, which one is the leader, and remembering that by serial number. */ | |
| 2 | ||
| 3 | import { mkdirSync, readFileSync, renameSync, writeFileSync } from "node:fs"; | |
| 4 | import { homedir } from "node:os"; | |
| 5 | import { dirname, join } from "node:path"; | |
| 6 | ||
| 7 | export const ADAPTER = /usbmodem|usbserial|ttyACM|ttyUSB/i; | |
| 8 | ||
| 9 | export interface Adapter { | |
| 10 | path: string; | |
| 11 | /** The USB serial number; stays with the board when it moves to another port. */ | |
| 12 | serial: string | null; | |
| 13 | } | |
| 14 | ||
| 15 | export interface RawPort { | |
| 16 | path: string; | |
| 17 | serialNumber?: string | undefined; | |
| 18 | } | |
| 19 | ||
| 20 | /** macOS exposes each port twice; the cu.* node is the right one for a host-initiated link. */ | |
| 21 | export function calloutDevice(path: string, platform: string = process.platform): string { | |
| 22 | return platform === "darwin" ? path.replace(/^\/dev\/tty\./, "/dev/cu.") : path; | |
| 23 | } | |
| 24 | ||
| 25 | /** The USB serial adapters among `SerialPort.list()`, once each, in a stable order. */ | |
| 26 | export function adaptersOf(list: readonly RawPort[], platform: string = process.platform): Adapter[] { | |
| 27 | const seen = new Map<string, Adapter>(); | |
| 28 | for (const port of list) { | |
| 29 | if (!ADAPTER.test(port.path)) continue; | |
| 30 | const path = calloutDevice(port.path, platform); | |
| 31 | seen.set(path, { path, serial: port.serialNumber ?? null }); | |
| 32 | } | |
| 33 | return [...seen.values()].sort((a, b) => a.path.localeCompare(b.path)); | |
| 34 | } | |
| 35 | ||
| 36 | export interface Roles { | |
| 37 | leader: string; | |
| 38 | follower: string; | |
| 39 | } | |
| 40 | ||
| 41 | /** Serial numbers of the arms, leader and follower; the saved choice by serial, or null when it does not apply to these adapters. */ | |
| 42 | export function savedRoles(saved: Roles | null, adapters: readonly Adapter[]): { leader: Adapter; follower: Adapter } | null { | |
| 43 | if (!saved) return null; | |
| 44 | const leader = adapters.find((a) => a.serial === saved.leader); | |
| 45 | const follower = adapters.find((a) => a.serial === saved.follower); | |
| 46 | return leader && follower && leader !== follower ? { leader, follower } : null; | |
| 47 | } | |
| 48 | ||
| 49 | /** The two ports `so101 teleop` uses: those named on the command line, else the remembered roles found among the adapters. */ | |
| 50 | export function resolveRoles(leaderPath: string | undefined, followerPath: string | undefined, saved: Roles | null, adapters: readonly Adapter[]): { leader: string; follower: string } { | |
| 51 | if (leaderPath && followerPath) return { leader: leaderPath, follower: followerPath }; | |
| 52 | const found = savedRoles(saved, adapters); | |
| 53 | if (!found) throw new Error("I don't know which arm is the leader yet — pass --leader and --follower (add --save to remember them)"); | |
| 54 | return { leader: leaderPath ?? found.leader.path, follower: followerPath ?? found.follower.path }; | |
| 55 | } | |
| 56 | ||
| 57 | /** The serial numbers of the two ports, to remember them; null when either adapter has none. */ | |
| 58 | export function rolesOf(leaderPath: string, followerPath: string, adapters: readonly Adapter[]): Roles | null { | |
| 59 | const leader = adapters.find((a) => a.path === calloutDevice(leaderPath))?.serial; | |
| 60 | const follower = adapters.find((a) => a.path === calloutDevice(followerPath))?.serial; | |
| 61 | return leader && follower && leader !== follower ? { leader, follower } : null; | |
| 62 | } | |
| 63 | ||
| 64 | export function configFile(env: Record<string, string | undefined> = process.env): string { | |
| 65 | return join(env.SO101_HOME ?? join(env.XDG_CONFIG_HOME ?? join(homedir(), ".config"), "so101"), "arms.json"); | |
| 66 | } | |
| 67 | ||
| 68 | export function loadRoles(file: string): Roles | null { | |
The |
||
| 69 | try { | |
| 70 | const stored = JSON.parse(readFileSync(file, "utf8")) as Partial<Roles>; | |
outdated
No production caller: nothing writes arms.json, so the remembered-roles path can never apply. Remove it or add the writer. |
||
| 71 | return typeof stored.leader === "string" && typeof stored.follower === "string" ? { leader: stored.leader, follower: stored.follower } : null; | |
| 72 | } catch { | |
| 73 | return null; | |
| 74 | } | |
| 75 | } | |
| 76 | ||
| 77 | export function saveRoles(file: string, roles: Roles): void { | |
| 78 | mkdirSync(dirname(file), { recursive: true }); | |
| 79 | writeFileSync(`${file}.tmp`, JSON.stringify(roles)); | |
| 80 | renameSync(`${file}.tmp`, file); | |
| 81 | } |
Software/so101/src/cli.ts +14 −1
| @@ -6,7 +6,6 @@ import { describeError, hex } from "./protocol.ts"; | ||
| 6 | 6 | import { MAX_FRAME_STEP, MotionError, moveToPose, name as jointName, playTrajectory, recordTrajectory, sleep } from "./motion.ts"; |
| 7 | 7 | import { streamLeader } from "./leader.ts"; |
| 8 | 8 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; |
| 9 | import { startServer } from "./server.ts"; | |
| 10 | 9 | import { durationSeconds, largestStep, parseTrajectory, serializeTrajectory } from "./trajectory.ts"; |
| 11 | 10 | import { |
| 12 | 11 | BAUD_RATES, |
| @@ -50,6 +49,8 @@ usage: so101 <command> [options] | ||
| 50 | 49 | HTTP API + WebSocket target stream for this arm (run where the follower is) |
| 51 | 50 | leader --to ws://host:8101 [--hz 60] |
| 52 | 51 | read this leader arm and drive a remote follower through its serve |
| 52 | teleop [--leader P --follower P [--save]] [--hz 60] | |
| 53 | the leader arm drives the follower here, space stops it, q quits | |
| 53 | 54 | |
| 54 | 55 | options: |
| 55 | 56 | --port <path> serial port (default: $SO101_PORT, else the only USB adapter) |
| @@ -73,6 +74,9 @@ const OPTIONS = { | ||
| 73 | 74 | torque: { type: "string" }, |
| 74 | 75 | hold: { type: "boolean" }, |
| 75 | 76 | listen: { type: "string" }, |
| 77 | save: { type: "boolean" }, | |
| 78 | leader: { type: "string" }, | |
| 79 | follower: { type: "string" }, | |
| 76 | 80 | to: { type: "string" }, |
| 77 | 81 | model: { type: "string" }, |
| 78 | 82 | } as const; |
| @@ -94,6 +98,9 @@ interface Values { | ||
| 94 | 98 | torque?: string; |
| 95 | 99 | hold?: boolean; |
| 96 | 100 | listen?: string; |
| 101 | save?: boolean; | |
| 102 | leader?: string; | |
| 103 | follower?: string; | |
| 97 | 104 | model?: string; |
| 98 | 105 | to?: string; |
| 99 | 106 | } |
| @@ -498,6 +505,7 @@ const COMMANDS: Record<string, Command> = { | ||
| 498 | 505 | const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":"); |
| 499 | 506 | const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port"); |
| 500 | 507 | return withBus(values, async (bus) => { |
| 508 | const { startServer } = await import("./server.ts"); // lazily: it reaches bun:sqlite, which Node lacks | |
| 501 | 509 | const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model, gainSyncMs: 2000 }); |
| 502 | 510 | console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`); |
| 503 | 511 | console.log("ctrl-c to stop"); |
| @@ -527,6 +535,11 @@ const COMMANDS: Record<string, Command> = { | ||
| 527 | 535 | }); |
| 528 | 536 | }, |
| 529 | 537 | |
| 538 | teleop: async (values) => { | |
| 539 | const { teleop } = await import("./local-teleop-run.ts"); | |
| 540 | await teleop(values.leader, values.follower, bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz"), values.save); | |
| 541 | }, | |
| 542 | ||
| 530 | 543 | "set-id": (values, [currentArg, nextArg]) => { |
| 531 | 544 | if (currentArg === undefined || nextArg === undefined) throw new Error("usage: so101 set-id <current> <new>"); |
| 532 | 545 | const current = servoId(currentArg, "current id"); |
Software/so101/src/keys.test.ts added +30
| @@ -0,0 +1,30 @@ | ||
| 1 | import { expect, test } from "bun:test"; | |
| 2 | import { keyQueue } from "./keys.ts"; | |
| 3 | ||
| 4 | test("a key pressed after a step ends is not lost to that step's waiter", async () => { | |
| 5 | const keys = keyQueue(); | |
| 6 | const step = new AbortController(); | |
| 7 | const stale = keys.next(step.signal); // the step that ended without a key | |
| 8 | step.abort(); | |
| 9 | expect(await stale).toBeNull(); | |
| 10 | keys.push("down"); | |
| 11 | expect(await keys.next()).toBe("down"); | |
| 12 | }); | |
| 13 | ||
| 14 | test("keys pressed while nobody waits are kept in order; an aborted signal returns at once", async () => { | |
| 15 | const keys = keyQueue(); | |
| 16 | keys.push("a"); | |
| 17 | keys.push("b"); | |
| 18 | const done = new AbortController(); | |
| 19 | done.abort(); | |
| 20 | expect(await keys.next(done.signal)).toBe("a"); | |
| 21 | expect(await keys.next()).toBe("b"); | |
| 22 | expect(await keys.next(done.signal)).toBeNull(); | |
| 23 | }); | |
| 24 | ||
| 25 | test("a waiting step gets the key", async () => { | |
| 26 | const keys = keyQueue(); | |
| 27 | const waiting = keys.next(new AbortController().signal); | |
| 28 | keys.push("return"); | |
| 29 | expect(await waiting).toBe("return"); | |
| 30 | }); |
Software/so101/src/keys.ts added +61
| @@ -0,0 +1,61 @@ | ||
| 1 | /** Key presses as a queue that no step can lose a key from: a waiter lives only as long as its signal. */ | |
| 2 | ||
| 3 | import { emitKeypressEvents } from "node:readline"; | |
| 4 | ||
| 5 | export interface Keys { | |
| 6 | /** The next key, or null when `signal` aborts first. A key pressed while nobody waits is kept for the next caller. */ | |
| 7 | next(signal?: AbortSignal): Promise<string | null>; | |
| 8 | close(): void; | |
| 9 | } | |
| 10 | ||
| 11 | export interface KeyQueue extends Keys { | |
| 12 | push(key: string): void; | |
| 13 | } | |
| 14 | ||
| 15 | export function keyQueue(onClose: () => void = () => {}): KeyQueue { | |
| 16 | const queue: string[] = []; | |
| 17 | const waiters = new Set<(key: string) => void>(); | |
| 18 | return { | |
| 19 | push(key) { | |
| 20 | const waiter = waiters.values().next().value; | |
| 21 | if (waiter) { | |
| 22 | waiters.delete(waiter); | |
| 23 | waiter(key); | |
| 24 | } else queue.push(key); | |
| 25 | }, | |
| 26 | next(signal) { | |
| 27 | if (queue.length) return Promise.resolve(queue.shift()!); | |
| 28 | if (signal?.aborted) return Promise.resolve(null); | |
| 29 | return new Promise((resolve) => { | |
| 30 | const waiter = (key: string) => { | |
| 31 | signal?.removeEventListener("abort", abort); | |
| 32 | resolve(key); | |
| 33 | }; | |
| 34 | const abort = () => { | |
| 35 | waiters.delete(waiter); | |
| 36 | resolve(null); | |
| 37 | }; | |
| 38 | waiters.add(waiter); | |
| 39 | signal?.addEventListener("abort", abort, { once: true }); | |
| 40 | }); | |
| 41 | }, | |
| 42 | close: onClose, | |
| 43 | }; | |
| 44 | } | |
| 45 | ||
| 46 | /** Single key presses from the terminal: "up", "down", "return", "space", "q", "ctrl-c" … */ | |
| 47 | export function terminalKeys(): Keys { | |
| 48 | emitKeypressEvents(process.stdin); | |
| 49 | if (process.stdin.isTTY) process.stdin.setRawMode(true); | |
| 50 | process.stdin.resume(); | |
| 51 | const queue = keyQueue(() => { | |
| 52 | process.stdin.off("keypress", on); | |
| 53 | if (process.stdin.isTTY) process.stdin.setRawMode(false); | |
| 54 | process.stdin.pause(); | |
| 55 | }); | |
| 56 | const on = (_: unknown, key: { name?: string; ctrl?: boolean; sequence?: string } | undefined) => { | |
| 57 | queue.push(key?.ctrl && key.name === "c" ? "ctrl-c" : (key?.name ?? key?.sequence ?? "")); | |
| 58 | }; | |
| 59 | process.stdin.on("keypress", on); | |
| 60 | return queue; | |
| 61 | } |
Software/so101/src/local-teleop-run.test.ts added +97
| @@ -0,0 +1,97 @@ | ||
| 1 | import { afterEach, beforeEach, expect, spyOn, test } from "bun:test"; | |
| 2 | import { FeetechBus } from "./bus.ts"; | |
| 3 | import { FakeBus } from "./fake-bus.ts"; | |
| 4 | import { keyQueue } from "./keys.ts"; | |
| 5 | import { IDS, RAMP_STEP, runLocalTeleop } from "./local-teleop.ts"; | |
| 6 | import { driveScreen } from "./local-teleop-run.ts"; | |
| 7 | import { STS3215 } from "./sts3215.ts"; | |
| 8 | ||
| 9 | const arm = () => { | |
| 10 | const fake = new FakeBus([...IDS]); | |
| 11 | return { fake, bus: new FeetechBus(fake) }; | |
| 12 | }; | |
| 13 | const setPresent = (fake: FakeBus, id: number, ticks: number) => { | |
| 14 | const m = fake.servos.get(id)!; | |
| 15 | m[56] = ticks & 0xff; | |
| 16 | m[57] = ticks >> 8; | |
| 17 | }; | |
| 18 | const settle = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 19 | ||
| 20 | let out: ReturnType<typeof spyOn>; | |
| 21 | beforeEach(() => void (out = spyOn(process.stdout, "write").mockImplementation(() => true))); | |
| 22 | afterEach(() => out.mockRestore()); | |
| 23 | ||
| 24 | async function calibrated({ fake, bus }: ReturnType<typeof arm>) { | |
| 25 | for (const id of IDS) { | |
| 26 | await bus.write(id, STS3215.Min_Position_Limit, 1000); | |
| 27 | await bus.write(id, STS3215.Max_Position_Limit, 3000); | |
| 28 | setPresent(fake, id, 2000); | |
| 29 | } | |
| 30 | } | |
| 31 | ||
| 32 | test("the follower follows the leader, holds while stopped, and resumes with a bounded first step", async () => { | |
| 33 | const leader = arm(); | |
| 34 | const follower = arm(); | |
| 35 | await calibrated(leader); | |
| 36 | await calibrated(follower); | |
| 37 | let stopped = false; | |
| 38 | const controller = new AbortController(); | |
Fixed settle(40/60) sleeps at 200 Hz can flake on a loaded machine. Poll until follower.fake.position(1) === 2100, with a deadline. |
||
| 39 | const run = runLocalTeleop(leader.bus, follower.bus, { hz: 200, signal: controller.signal, stopped: () => stopped }); | |
| 40 | await settle(40); | |
| 41 | setPresent(leader.fake, 1, 2100); | |
| 42 | await settle(60); | |
| 43 | expect(follower.fake.position(1)).toBe(2100); | |
| 44 | ||
| 45 | stopped = true; | |
| 46 | await settle(30); | |
| 47 | setPresent(leader.fake, 1, 2900); // moved far while stopped | |
| 48 | await settle(60); | |
| 49 | expect(follower.fake.position(1)).toBe(2100); // STOP holds | |
| 50 | expect(await follower.bus.read(1, STS3215.Torque_Enable)).toBe(1); | |
| 51 | ||
| 52 | const before = follower.fake.writes.length; | |
| 53 | stopped = false; | |
| 54 | await settle(120); | |
| 55 | controller.abort(); | |
| 56 | await run; | |
| 57 | const goals = follower.fake.writes | |
| 58 | .slice(before) | |
| 59 | .filter((w) => w.id === 1 && w.address === STS3215.Goal_Position.address) | |
| 60 | .map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); | |
| 61 | expect(goals.length).toBeGreaterThan(0); | |
| 62 | expect(Math.abs(goals[0]! - 2100)).toBeLessThanOrEqual(RAMP_STEP); | |
| 63 | expect(follower.fake.position(1)).toBeGreaterThan(2100); | |
| 64 | }); | |
| 65 | ||
| 66 | test("driveScreen: space stops, q ends it saying the follower holds, and no key waiter is left behind", async () => { | |
| 67 | const leader = arm(); | |
| 68 | const follower = arm(); | |
| 69 | await calibrated(leader); | |
| 70 | await calibrated(follower); | |
| 71 | const keys = keyQueue(); | |
| 72 | const done = driveScreen(keys, leader.bus, follower.bus, 100); | |
| 73 | await settle(50); | |
| 74 | keys.push("space"); | |
| 75 | await settle(30); | |
| 76 | keys.push("q"); | |
| 77 | await done; | |
| 78 | keys.push("down"); | |
| 79 | expect(await keys.next()).toBe("down"); | |
| 80 | expect(await follower.bus.read(1, STS3215.Torque_Enable)).toBe(1); | |
| 81 | expect(out.mock.calls.map((c: unknown[]) => String(c[0])).join("")).toContain("stays holding with torque on"); | |
| 82 | }); | |
| 83 | ||
| 84 | test("while following, a fast leader jump arrives within one frame; the ramp is for the start and a resume only", async () => { | |
| 85 | const leader = arm(); | |
| 86 | const follower = arm(); | |
| 87 | await calibrated(leader); | |
| 88 | await calibrated(follower); | |
| 89 | const controller = new AbortController(); | |
| 90 | const run = runLocalTeleop(leader.bus, follower.bus, { hz: 50, signal: controller.signal }); | |
| 91 | await settle(100); | |
| 92 | setPresent(leader.fake, 1, 2900); // 900 ticks at once, far past RAMP_STEP | |
| 93 | await settle(60); // a few frames at 20 ms, not the 18 it would take ramped | |
| 94 | controller.abort(); | |
| 95 | await run; | |
| 96 | expect(follower.fake.position(1)).toBe(2900); | |
| 97 | }); |
Software/so101/src/local-teleop-run.ts added +62
| @@ -0,0 +1,62 @@ | ||
| 1 | /** `so101 teleop`: the remembered leader drives the follower, in this one process, until q. */ | |
| 2 | ||
| 3 | import { SerialPort } from "serialport"; | |
| 4 | import { adaptersOf, configFile, loadRoles, resolveRoles, rolesOf, saveRoles } from "./arms.ts"; | |
| 5 | import { FeetechBus } from "./bus.ts"; | |
| 6 | import { terminalKeys, type Keys } from "./keys.ts"; | |
| 7 | import { runLocalTeleop, teleopScreen } from "./local-teleop.ts"; | |
| 8 | ||
| 9 | /** Drive the follower from the leader on screen. Space stops and resumes; q leaves the follower holding with torque on. */ | |
| 10 | export async function driveScreen(key: Keys, leader: FeetechBus, follower: FeetechBus, hz = 60): Promise<void> { | |
| 11 | const controller = new AbortController(); | |
| 12 | let stopped = false; | |
| 13 | let last = 0; | |
| 14 | const listening = (async () => { | |
| 15 | for (;;) { | |
| 16 | const k = await key.next(controller.signal); | |
| 17 | if (k === null) return; | |
| 18 | if (k === "space") stopped = !stopped; | |
| 19 | else if (k === "q" || k === "ctrl-c") return controller.abort(); | |
| 20 | } | |
| 21 | })(); | |
| 22 | try { | |
| 23 | await runLocalTeleop(leader, follower, { | |
| 24 | hz, | |
| 25 | signal: controller.signal, | |
| 26 | stopped: () => stopped, | |
| 27 | onTick: (s) => { | |
| 28 | if (performance.now() - last < 100) return; | |
| 29 | last = performance.now(); | |
| 30 | process.stdout.write(`\x1b[2J\x1b[H${teleopScreen(s).join("\n")}\n`); | |
| 31 | }, | |
| 32 | }); | |
| 33 | } finally { | |
| 34 | controller.abort(); | |
| 35 | await listening; | |
| 36 | } | |
| 37 | process.stdout.write("Stopped. The follower stays holding with torque on; `so101 torque off` releases it.\n"); | |
| 38 | } | |
| 39 | ||
| 40 | /** Both ports are opened before the terminal goes raw; one `finally` closes all three. */ | |
| 41 | export async function teleop(leaderPath: string | undefined, followerPath: string | undefined, hz: number, save = false): Promise<void> { | |
| 42 | const adapters = leaderPath && followerPath && !save ? [] : adaptersOf(await SerialPort.list()); | |
| 43 | const ports = resolveRoles(leaderPath, followerPath, loadRoles(configFile()), adapters); | |
| 44 | let key: Keys | undefined; | |
| 45 | let leader: FeetechBus | undefined; | |
| 46 | let follower: FeetechBus | undefined; | |
| 47 | try { | |
| 48 | leader = await FeetechBus.open(ports.leader); | |
| 49 | follower = await FeetechBus.open(ports.follower); | |
| 50 | if (save) { | |
| 51 | const roles = rolesOf(ports.leader, ports.follower, adapters); | |
| 52 | if (!roles) throw new Error("--save needs both ports to be USB adapters with a serial number"); | |
| 53 | saveRoles(configFile(), roles); | |
| 54 | } | |
| 55 | key = terminalKeys(); | |
| 56 | await driveScreen(key, leader, follower, hz); | |
| 57 | } finally { | |
| 58 | key?.close(); | |
| 59 | await leader?.close(); | |
| 60 | await follower?.close(); | |
| 61 | } | |
| 62 | } |
Software/so101/src/local-teleop.test.ts added +24
| @@ -0,0 +1,24 @@ | ||
| 1 | import { expect, test } from "bun:test"; | |
| 2 | import { percentile, teleopScreen } from "./local-teleop.ts"; | |
| 3 | ||
| 4 | const live = { sent: 3, lagMs: { p50: 1, p95: 2 }, leader: new Map([[1, 2000], [2, 1500]]), target: { "1": 2010 }, stopped: false }; | |
| 5 | ||
| 6 | test("percentile of a sorted list", () => { | |
| 7 | expect(percentile([1, 2, 3, 4], 0.5)).toBe(3); | |
| 8 | expect(percentile([], 0.95)).toBe(0); | |
| 9 | }); | |
| 10 | ||
| 11 | test("the screen has a row per joint, the lag, and STOP while live", () => { | |
| 12 | const lines = teleopScreen(live); | |
| 13 | expect(lines.filter((l) => l.includes("leader") && l.includes("target"))).toHaveLength(6); | |
| 14 | expect(lines.find((l) => l.includes("leader 2000"))).toMatch(/target 2010$/); | |
| 15 | expect(lines.find((l) => l.includes("leader 1500"))).toMatch(/target -$/); | |
| 16 | expect(lines.join("\n")).toContain("lag 1.0 ms typical, 2.0 ms worst frames 3"); | |
| 17 | expect(lines.join("\n")).toContain("press SPACE to STOP"); | |
| 18 | }); | |
| 19 | ||
| 20 | test("stopped, the screen says the follower holds and how to go on", () => { | |
| 21 | const text = teleopScreen({ ...live, stopped: true }).join("\n"); | |
| 22 | expect(text).toContain("STOPPED — the follower holds. Press space to go on, q to quit (torque stays on)."); | |
| 23 | expect(text).not.toContain("LIVE"); | |
| 24 | }); |
Software/so101/src/local-teleop.ts added +105
| @@ -0,0 +1,105 @@ | ||
| 1 | /** The leader drives the follower directly, in this one process: no serve, no network. */ | |
| 2 | ||
| 3 | import type { FeetechBus } from "./bus.ts"; | |
| 4 | import { MAX_FRAME_STEP } from "./motion.ts"; | |
| 5 | import { mapFrame, type Range } from "./mapping.ts"; | |
| 6 | import { JOINTS, STS3215 } from "./sts3215.ts"; | |
| 7 | ||
| 8 | export const IDS = [1, 2, 3, 4, 5, 6] as const; | |
| 9 | const GRIPPER = 6; | |
| 10 | /** Most ticks a goal moves per frame while catching up with the leader after a start or a resume (never while following): 400 at once is the most motion.ts allows between frames; this is gentler at 60 Hz. */ | |
| 11 | export const RAMP_STEP = Math.round(MAX_FRAME_STEP / 8); | |
| 12 | ||
This clamp is applied on every frame, so it caps normal following speed as well as the resume. Use it only until the goal catches up with the leader after a stop. |
||
| 13 | export interface TeleopSnapshot { | |
| 14 | sent: number; | |
| 15 | /** Leader reading → follower target written, ms, over the last second or so. */ | |
| 16 | lagMs: { p50: number; p95: number }; | |
| 17 | leader: ReadonlyMap<number, number>; | |
| 18 | target: Record<string, number>; | |
| 19 | stopped: boolean; | |
| 20 | } | |
| 21 | ||
| 22 | export interface LocalTeleopOptions { | |
| 23 | hz?: number; | |
| 24 | signal?: AbortSignal; | |
| 25 | /** While true the follower is not sent anything and holds where it is. */ | |
| 26 | stopped?: () => boolean; | |
| 27 | onTick?: (snapshot: TeleopSnapshot) => void; | |
| 28 | } | |
| 29 | ||
| 30 | export async function readLimits(bus: FeetechBus, ids: readonly number[] = IDS): Promise<Map<number, Range>> { | |
| 31 | const limits = new Map<number, Range>(); | |
| 32 | for (const id of ids) limits.set(id, { min: await bus.read(id, STS3215.Min_Position_Limit), max: await bus.read(id, STS3215.Max_Position_Limit) }); | |
| 33 | return limits; | |
| 34 | } | |
| 35 | ||
| 36 | export function percentile(sorted: readonly number[], p: number): number { | |
| 37 | return sorted.length ? sorted[Math.min(sorted.length - 1, Math.floor(p * sorted.length))]! : 0; | |
| 38 | } | |
| 39 | ||
| 40 | /** Runs until the signal aborts; returns the frames sent. Torque on the follower stays on, so it holds where it ended. */ | |
| 41 | export async function runLocalTeleop(leader: FeetechBus, follower: FeetechBus, { hz = 60, signal, stopped = () => false, onTick }: LocalTeleopOptions = {}): Promise<number> { | |
| 42 | const leaderLimits = await readLimits(leader); | |
| 43 | const followerLimits = Object.fromEntries([...(await readLimits(follower))].map(([id, r]) => [String(id), r])); | |
| 44 | await leader.syncWrite(STS3215.Torque_Enable, IDS.map((id) => [id, 0] as const)); // the leader is moved by hand | |
| 45 | // Start from where the follower is, so the first frame does not snap it across the room. | |
| 46 | const here = await follower.syncRead(IDS, STS3215.Present_Position); | |
| 47 | await follower.syncWrite(STS3215.Goal_Position, here); | |
| 48 | await follower.syncWrite(STS3215.Torque_Enable, IDS.map((id) => [id, 1] as const)); | |
| 49 | const period = 1000 / hz; | |
| 50 | const lags: number[] = []; | |
| 51 | const started = performance.now(); | |
| 52 | let sent = 0; | |
| 53 | let loops = 0; | |
| 54 | // After a stop the leader may be far from where the follower waits: walk the goal there in bounded steps, never jump. | |
| 55 | let goal = new Map<number, number>(here); | |
| 56 | let wasStopped = false; | |
| 57 | let ramping = true; // from the start, the follower may be far from the leader | |
| 58 | while (!signal?.aborted) { | |
| 59 | const t0 = performance.now(); | |
| 60 | const reading = await leader.syncRead(IDS, STS3215.Present_Position); | |
| 61 | const frame = mapFrame(reading, leaderLimits, followerLimits, GRIPPER); | |
| 62 | const hold = stopped(); | |
| 63 | if (hold) wasStopped = true; | |
| 64 | else if (Object.keys(frame).length) { | |
| 65 | if (wasStopped) { | |
| 66 | goal = new Map(await follower.syncRead(IDS, STS3215.Present_Position)); // re-seed from where the follower really is | |
| 67 | wasStopped = false; | |
| 68 | ramping = true; | |
| 69 | } | |
| 70 | const next = Object.entries(frame).map(([id, ticks]) => { | |
| 71 | const from = goal.get(Number(id)) ?? ticks; | |
| 72 | const to = ramping ? from + Math.max(-RAMP_STEP, Math.min(RAMP_STEP, ticks - from)) : ticks; | |
| 73 | goal.set(Number(id), to); | |
| 74 | return [Number(id), to] as const; | |
| 75 | }); | |
| 76 | await follower.syncWrite(STS3215.Goal_Position, next); | |
| 77 | if (ramping && next.every(([id, to]) => to === frame[String(id)])) ramping = false; // caught up: from now on the leader goes straight through | |
| 78 | sent++; | |
| 79 | } | |
| 80 | lags.push(performance.now() - t0); | |
| 81 | if (lags.length > hz) lags.shift(); | |
| 82 | const sorted = [...lags].sort((a, b) => a - b); | |
| 83 | onTick?.({ sent, lagMs: { p50: percentile(sorted, 0.5), p95: percentile(sorted, 0.95) }, leader: reading, target: frame, stopped: hold }); | |
| 84 | loops++; | |
While stopped, |
||
| 85 | const wait = started + loops * period - performance.now(); | |
| 86 | if (wait > 0) await new Promise((r) => setTimeout(r, wait)); | |
| 87 | } | |
| 88 | return sent; | |
| 89 | } | |
| 90 | ||
| 91 | export function teleopScreen(s: TeleopSnapshot): string[] { | |
| 92 | const rows = IDS.map((id) => { | |
| 93 | const target = s.target[String(id)]; | |
| 94 | return ` ${(JOINTS[id] ?? "").padEnd(14)} leader ${String(s.leader.get(id) ?? "-").padStart(5)} → target ${String(target ?? "-").padStart(5)}`; | |
| 95 | }); | |
| 96 | return [ | |
| 97 | "so101 teleop — the leader arm drives the follower", | |
| 98 | "", | |
| 99 | ...rows, | |
| 100 | "", | |
| 101 | ` lag ${s.lagMs.p50.toFixed(1)} ms typical, ${s.lagMs.p95.toFixed(1)} ms worst frames ${s.sent}`, | |
| 102 | "", | |
| 103 | s.stopped ? " STOPPED — the follower holds. Press space to go on, q to quit (torque stays on)." : " ● LIVE press SPACE to STOP, q to quit (q leaves the follower holding, torque on)", | |
| 104 | ]; | |
| 105 | } |
The typed path is compared with the adapter paths unchanged. On macOS
/dev/tty.usbmodemXwill not match/dev/cu.usbmodemX, so--savefails with a misleading error. ApplycalloutDeviceto both paths before comparing.