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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

`so101 teleop` drives the follower from the leader in one process at 60 Hz, with a joint table, lag and space to STOP #574

Merged opened this

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

Merged

smith/arm:smith/571-so101-teleop-drives-the-follower-from-th → main 4 commits fork 7 threads · 7 unresolved

  • Merged. smith/571-so101-teleop-drives-the-follower-from-th was merged into main as 43e37d1.

Review

Sign in to review this change.
10 files changed +543 −1
Software/so101/README.md +14
@@ -2,6 +2,20 @@
22
33 `bench/fake-serve.ts` is the hosted app (the page, the room, the relay); `so101 serve` is the arm server on the Pi.
44
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
519 ## Open / Closed
620
721 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 {

The typed path is compared with the adapter paths unchanged. On macOS /dev/tty.usbmodemX will not match /dev/cu.usbmodemX, so --save fails with a misleading error. Apply calloutDevice to both paths before comparing.

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 FORGE_APP_DATA fallback was not asked for. This CLI runs on a laptop or Pi, not as the forge app. Remove it or justify it.

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";
66 import { MAX_FRAME_STEP, MotionError, moveToPose, name as jointName, playTrajectory, recordTrajectory, sleep } from "./motion.ts";
77 import { streamLeader } from "./leader.ts";
88 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts";
9 import { startServer } from "./server.ts";
109 import { durationSeconds, largestStep, parseTrajectory, serializeTrajectory } from "./trajectory.ts";
1110 import {
1211 BAUD_RATES,
@@ -50,6 +49,8 @@ usage: so101 <command> [options]
5049 HTTP API + WebSocket target stream for this arm (run where the follower is)
5150 leader --to ws://host:8101 [--hz 60]
5251 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
5354
5455 options:
5556 --port <path> serial port (default: $SO101_PORT, else the only USB adapter)
@@ -73,6 +74,9 @@ const OPTIONS = {
7374 torque: { type: "string" },
7475 hold: { type: "boolean" },
7576 listen: { type: "string" },
77 save: { type: "boolean" },
78 leader: { type: "string" },
79 follower: { type: "string" },
7680 to: { type: "string" },
7781 model: { type: "string" },
7882 } as const;
@@ -94,6 +98,9 @@ interface Values {
9498 torque?: string;
9599 hold?: boolean;
96100 listen?: string;
101 save?: boolean;
102 leader?: string;
103 follower?: string;
97104 model?: string;
98105 to?: string;
99106 }
@@ -498,6 +505,7 @@ const COMMANDS: Record<string, Command> = {
498505 const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":");
499506 const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port");
500507 return withBus(values, async (bus) => {
508 const { startServer } = await import("./server.ts"); // lazily: it reaches bun:sqlite, which Node lacks
501509 const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model, gainSyncMs: 2000 });
502510 console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`);
503511 console.log("ctrl-c to stop");
@@ -527,6 +535,11 @@ const COMMANDS: Record<string, Command> = {
527535 });
528536 },
529537
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
530543 "set-id": (values, [currentArg, nextArg]) => {
531544 if (currentArg === undefined || nextArg === undefined) throw new Error("usage: so101 set-id <current> <new>");
532545 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, frame is still passed as target, so the screen shows a target the follower is not chasing. Pass {} when hold is true.

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 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs