trevato/arm

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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine
More
Mapg aCommitsg mBranchesg bRunsg rEventsFlows

Clone, one command, drive: a guided setup TUI that finds both arms, reuses existing calibration, and starts local teleop #566

Closed smithcrew opened this in answer to trevatoowner
ConversationFiles
Closed without merging

smith/arm:smith/564-clone-one-command-drive-a-guided-setup-t → main 5 commits fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
14 files changed +856 −1
Software/so101/README.md +15
@@ -2,6 +2,21 @@
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, Node 22.6+ and Bun, nothing else:
8
9 ```
10 bun install && bun so101 setup
11 ```
12
13 Setup is full-screen: it finds both arms (and waits if only one shows up), asks you to wiggle the one you will hold
14 (remembered by USB serial number in `~/.config/so101/arms.json`), checks each servo's calibrated range, and reuses LeRobot's
15 files in `~/.cache/huggingface/lerobot/calibration/` when a servo has forgotten its range. Only a joint that is out of range
16 is calibrated again, one at a time. Then the leader drives the follower, 60 Hz, space to STOP. Later, `bun so101 teleop`
17 skips the questions. `bun so101 setup --dry-run` shows the plan without touching an arm. Assembly and calibration background:
18 https://huggingface.co/docs/lerobot/so101
19
520 ## Open / Closed
621
722 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/package.json +1
@@ -8,6 +8,7 @@
88 "so101": "bin/so101"
99 },
1010 "scripts": {
11 "so101": "node bin/so101",
1112 "typecheck": "tsc --noEmit",
1213 "test": "bun test"
1314 },
Software/so101/src/arm-calibration.test.ts added +23
@@ -0,0 +1,23 @@
1 import { expect, test } from "bun:test";
2 import { judge, parseLerobot } from "./arm-calibration.ts";
3
4 const lerobot = { shoulder_pan: { id: 1, drive_mode: 0, homing_offset: 5, range_min: 800, range_max: 3200 }, gripper: { id: 6, range_min: 5, range_max: 6 }, bogus: {} };
5
6 test("LeRobot's file maps joint names to servo ranges and skips junk", () => {
7 expect([...parseLerobot(lerobot)]).toEqual([[1, { min: 800, max: 3200 }]]);
8 expect(parseLerobot("x").size).toBe(0);
9 });
10
11 test("a calibrated joint is kept; a forgotten one comes back from LeRobot's file; without a file it is recalibrated", () => {
12 const limits = new Map([[1, { min: 0, max: 4095 }], [2, { min: 700, max: 3300 }], [3, { min: 0, max: 4095 }]]);
13 const present = new Map([[1, 2000], [2, 2000], [3, 2000]]);
14 const verdicts = judge(limits, present, parseLerobot(lerobot));
15 expect(verdicts[0]).toEqual({ id: 1, state: "restore", range: { min: 800, max: 3200 } });
16 expect(verdicts[1]).toEqual({ id: 2, state: "ok" });
17 expect(verdicts[2]?.state).toBe("recalibrate");
18 });
19
20 test("a joint sitting outside its range is flagged", () => {
21 const verdicts = judge(new Map([[2, { min: 700, max: 3300 }]]), new Map([[2, 4000]]));
22 expect(verdicts[0]).toMatchObject({ state: "recalibrate" });
23 });
Software/so101/src/arm-calibration.ts added +73
@@ -0,0 +1,73 @@
1 /**
2 * Does an arm still carry a usable calibration? The servos keep their own range limits in EEPROM (that is what
3 * LeRobot's calibration writes), so the question is answered by reading them back, and by comparing with the
4 * calibration files LeRobot left on the computer.
5 */
6
7 import { readdirSync, readFileSync, statSync } from "node:fs";
8 import { homedir } from "node:os";
9 import { join } from "node:path";
10 import type { Range } from "./mapping.ts";
11 import { JOINTS } from "./sts3215.ts";
12
13 /** A joint whose range is narrower than this was never calibrated (factory is 0-4095, or 0-0). */
14 export const MIN_RANGE = 200;
15 export const FULL_TURN = 4095;
16
17 export type Verdict =
18 | { id: number; state: "ok" }
19 | { id: number; state: "restore"; range: Range } // the servo forgot; LeRobot's file knows
20 | { id: number; state: "recalibrate"; why: string };
21
22 export function lerobotDirs(kind: "robots" | "teleoperators", home: string = homedir(), env: Record<string, string | undefined> = process.env): string {
23 const cache = env.HF_HOME ? join(env.HF_HOME, "lerobot") : join(env.XDG_CACHE_HOME ?? join(home, ".cache"), "huggingface", "lerobot");
24 return join(cache, "calibration", kind);
25 }
26
27 /** LeRobot's calibration JSON: { joint_name: { id, range_min, range_max, … } } → servo id → range. Bad entries are skipped. */
28 export function parseLerobot(body: unknown): Map<number, Range> {
29 const out = new Map<number, Range>();
30 if (typeof body !== "object" || body === null) return out;
31 for (const [name, joint] of Object.entries(body)) {
32 const j = joint as { id?: unknown; range_min?: unknown; range_max?: unknown } | null;
33 const id = Object.entries(JOINTS).find(([, joint]) => joint === name)?.[0];
34 const tick = (v: unknown): v is number => typeof v === "number" && Number.isInteger(v) && v >= 0 && v <= FULL_TURN;
35 if (id === undefined || !j || !tick(j.range_min) || !tick(j.range_max) || j.range_max - j.range_min < MIN_RANGE) continue;
36 out.set(Number(id), { min: j.range_min, max: j.range_max });
37 }
38 return out;
39 }
40
41 /** The newest calibration file in `dir`, parsed; an empty map when there is none. */
42 export function newestLerobot(dir: string): Map<number, Range> {
43 let best: { file: string; at: number } | null = null;
44 try {
45 for (const name of readdirSync(dir)) {
46 if (!name.endsWith(".json")) continue;
47 const file = join(dir, name);
48 const at = statSync(file).mtimeMs;
49 if (!best || at > best.at) best = { file, at };
50 }
51 return best ? parseLerobot(JSON.parse(readFileSync(best.file, "utf8"))) : new Map();
52 } catch {
53 return new Map();
54 }
55 }
56
57 /** Check every joint: the servo's limits against being set at all, the arm's present position against them, LeRobot's file as the way back. */
58 export function judge(limits: ReadonlyMap<number, Range>, present: ReadonlyMap<number, number>, reference: ReadonlyMap<number, Range> = new Map()): Verdict[] {
59 const verdicts: Verdict[] = [];
60 for (const [id, range] of limits) {
61 const ref = reference.get(id);
62 const set = range.max - range.min >= MIN_RANGE && !(range.min === 0 && range.max === FULL_TURN);
63 const at = present.get(id);
64 if (!set) {
65 verdicts.push(ref ? { id, state: "restore", range: ref } : { id, state: "recalibrate", why: "its range was never set" });
66 } else if (at !== undefined && (at < range.min - 20 || at > range.max + 20)) {
67 verdicts.push(ref && at >= ref.min - 20 && at <= ref.max + 20 ? { id, state: "restore", range: ref } : { id, state: "recalibrate", why: `it sits at ${at}, outside ${range.min}-${range.max}` });
68 } else {
69 verdicts.push({ id, state: "ok" });
70 }
71 }
72 return verdicts;
73 }
Software/so101/src/arms.test.ts added +45
@@ -0,0 +1,45 @@
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, foundSentence, loadRoles, movement, 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("the sentence for one arm says what to do", () => {
21 expect(foundSentence(1)).toBe("I see 1 arm. Plug in the other one, or check its power. Watching…");
22 });
23
24 test("movement sums ticks over joints both readings share", () => {
25 expect(movement(new Map([[1, 100], [2, 200]]), new Map([[1, 150], [2, 180], [3, 9]]))).toBe(70);
26 });
27
28 test("roles are remembered by serial and found again on other ports", () => {
29 const file = join(mkdtempSync(join(tmpdir(), "arms-")), "nested", "arms.json");
30 expect(loadRoles(file)).toBeNull();
31 saveRoles(file, { leader: "B", follower: "A" });
32 const found = savedRoles(loadRoles(file), [{ path: "/dev/x", serial: "A" }, { path: "/dev/y", serial: "B" }]);
33 expect(found?.leader.path).toBe("/dev/y");
34 expect(savedRoles(loadRoles(file), [{ path: "/dev/x", serial: "A" }])).toBeNull();
35 });
36
37 test("teleop's ports: the command line wins, else the remembered roles, else a plain error", () => {
38 const adapters = [{ path: "/dev/x", serial: "A" }, { path: "/dev/y", serial: "B" }];
39 const saved = { leader: "B", follower: "A" };
40 expect(resolveRoles("/dev/l", "/dev/f", null, [])).toEqual({ leader: "/dev/l", follower: "/dev/f" });
41 expect(resolveRoles(undefined, undefined, saved, adapters)).toEqual({ leader: "/dev/y", follower: "/dev/x" });
42 expect(resolveRoles("/dev/l", undefined, saved, adapters)).toEqual({ leader: "/dev/l", follower: "/dev/x" });
43 expect(() => resolveRoles(undefined, undefined, null, adapters)).toThrow("run: so101 setup");
44 expect(() => resolveRoles(undefined, undefined, saved, [adapters[0]!])).toThrow("run: so101 setup");
45 });
Software/so101/src/arms.ts added +93
@@ -0,0 +1,93 @@
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 function foundSentence(count: number): string {
37 if (count >= 2) return `I see ${count} arms.`;
38 if (count === 1) return "I see 1 arm. Plug in the other one, or check its power. Watching…";
39 return "I see no arms. Plug them in and check their power. Watching…";
40 }
41
42 /** Total ticks the joints moved between two readings; a joint missing from one is ignored. */
43 export function movement(before: ReadonlyMap<number, number>, after: ReadonlyMap<number, number>): number {
44 let total = 0;
45 for (const [id, ticks] of after) {
46 const was = before.get(id);
47 if (was !== undefined) total += Math.abs(ticks - was);
48 }
49 return total;
50 }
51
52 /** Ticks of total movement (about 20°) that mean "this is the arm being held". */
53 export const WIGGLE_TICKS = 220;
54
55 export interface Roles {
56 leader: string;
57 follower: string;
58 }
59
60 /** Serial numbers of the arms, leader and follower; the saved choice by serial, or null when it does not apply to these adapters. */
61 export function savedRoles(saved: Roles | null, adapters: readonly Adapter[]): { leader: Adapter; follower: Adapter } | null {
62 if (!saved) return null;
63 const leader = adapters.find((a) => a.serial === saved.leader);
64 const follower = adapters.find((a) => a.serial === saved.follower);
65 return leader && follower && leader !== follower ? { leader, follower } : null;
66 }
67
68 /** The two ports `so101 teleop` uses: those named on the command line, else the remembered roles found among the adapters. */
69 export function resolveRoles(leaderPath: string | undefined, followerPath: string | undefined, saved: Roles | null, adapters: readonly Adapter[]): { leader: string; follower: string } {
70 if (leaderPath && followerPath) return { leader: leaderPath, follower: followerPath };
71 const found = savedRoles(saved, adapters);
72 if (!found) throw new Error("I don't know which arm is the leader yet — run: so101 setup (or pass --leader-port and --follower-port)");
73 return { leader: leaderPath ?? found.leader.path, follower: followerPath ?? found.follower.path };
74 }
75
76 export function configFile(env: Record<string, string | undefined> = process.env): string {
77 return join(env.SO101_HOME ?? env.FORGE_APP_DATA ?? join(env.XDG_CONFIG_HOME ?? join(homedir(), ".config"), "so101"), "arms.json");
78 }
79
80 export function loadRoles(file: string): Roles | null {
81 try {
82 const stored = JSON.parse(readFileSync(file, "utf8")) as Partial<Roles>;
83 return typeof stored.leader === "string" && typeof stored.follower === "string" ? { leader: stored.leader, follower: stored.follower } : null;
84 } catch {
85 return null;
86 }
87 }
88
89 export function saveRoles(file: string, roles: Roles): void {
90 mkdirSync(dirname(file), { recursive: true });
91 writeFileSync(`${file}.tmp`, JSON.stringify(roles));
92 renameSync(`${file}.tmp`, file);
93 }
Software/so101/src/cli-node.test.ts added +25
@@ -0,0 +1,25 @@
1 import { expect, test } from "bun:test";
2 import { spawnSync } from "node:child_process";
3
4 // The README promises a fresh clone runs under Node 22.6+ (types stripped at load). A Bun-only import reached from the CLI
5 // (bun:sqlite, say) crashes it on a laptop while every Bun test passes.
6 const version = spawnSync("node", ["--version"], { encoding: "utf8" });
7 const [major = 0, minor = 0] = (version.stdout ?? "").replace(/^v/, "").split(".").map(Number);
8 const strips = version.status === 0 && (major > 22 || (major === 22 && minor >= 6));
9 const cli = new URL("../bin/so101", import.meta.url).pathname;
10
11 function so101(...args: string[]) {
12 return spawnSync("node", ["--experimental-strip-types", "--no-warnings", cli, ...args], { encoding: "utf8", timeout: 30_000, env: { ...process.env, SO101_HOME: process.env.TMPDIR ?? "/tmp" } });
13 }
14
15 test.skipIf(!strips)("--help, ports and setup --dry-run run under Node", () => {
16 const help = so101("--help");
17 expect(help.stderr).toBe("");
18 expect(help.stdout).toContain("setup");
19 expect(help.status).toBe(0);
20 expect(so101("ports").status).toBe(0);
21 const dry = so101("setup", "--dry-run");
22 expect(dry.stderr).not.toMatch(/ERR_/);
23 expect(dry.stdout).toContain("dry run");
24 expect(dry.status).toBe(0);
25 });
Software/so101/src/cli.ts +21 −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,9 @@ 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 setup [--dry-run] guided: find both arms, reuse calibration, then drive
53 teleop [--leader-port p --follower-port p] [--hz 60]
54 the leader arm drives the follower here, space stops it
5355
5456 options:
5557 --port <path> serial port (default: $SO101_PORT, else the only USB adapter)
@@ -75,6 +77,9 @@ const OPTIONS = {
7577 listen: { type: "string" },
7678 to: { type: "string" },
7779 model: { type: "string" },
80 "dry-run": { type: "boolean" },
81 "leader-port": { type: "string" },
82 "follower-port": { type: "string" },
7883 } as const;
7984
8085 interface Values {
@@ -96,6 +101,9 @@ interface Values {
96101 listen?: string;
97102 model?: string;
98103 to?: string;
104 "dry-run"?: boolean;
105 "leader-port"?: string;
106 "follower-port"?: string;
99107 }
100108
101109 type Command = (values: Values, positionals: string[]) => Promise<void>;
@@ -498,6 +506,7 @@ const COMMANDS: Record<string, Command> = {
498506 const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":");
499507 const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port");
500508 return withBus(values, async (bus) => {
509 const { startServer } = await import("./server.ts"); // lazily: it reaches bun:sqlite, which Node lacks
501510 const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model, gainSyncMs: 2000 });
502511 console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`);
503512 console.log("ctrl-c to stop");
@@ -527,6 +536,17 @@ const COMMANDS: Record<string, Command> = {
527536 });
528537 },
529538
539 setup: async (values) => {
540 const { dryRun, setup } = await import("./setup.ts");
541 if (values["dry-run"]) return dryRun();
542 await setup({ hz: bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz") });
543 },
544
545 teleop: async (values) => {
546 const { teleop } = await import("./setup.ts");
547 await teleop(values["leader-port"], values["follower-port"], bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz"));
548 },
549
530550 "set-id": (values, [currentArg, nextArg]) => {
531551 if (currentArg === undefined || nextArg === undefined) throw new Error("usage: so101 set-id <current> <new>");
532552 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.test.ts added +13
@@ -0,0 +1,13 @@
1 import { expect, test } from "bun:test";
2 import { percentile, teleopScreen } from "./local-teleop.ts";
3
4 test("percentile of a sorted list", () => {
5 expect(percentile([1, 2, 3, 4], 0.5)).toBe(3);
6 expect(percentile([], 0.95)).toBe(0);
7 });
8
9 test("the screen shows STOP while live and says held when stopped", () => {
10 const s = { sent: 3, lagMs: { p50: 1, p95: 2 }, leader: new Map([[1, 2000]]), target: { "1": 2010 }, stopped: false };
11 expect(teleopScreen(s).join("\n")).toContain("press SPACE to STOP");
12 expect(teleopScreen({ ...s, stopped: true }).join("\n")).toContain("STOPPED");
13 });
Software/so101/src/local-teleop.ts added +102
@@ -0,0 +1,102 @@
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: 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
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 while (!signal?.aborted) {
58 const t0 = performance.now();
59 const reading = await leader.syncRead(IDS, STS3215.Present_Position);
60 const frame = mapFrame(reading, leaderLimits, followerLimits, GRIPPER);
61 const hold = stopped();
62 if (hold) wasStopped = true;
63 else if (Object.keys(frame).length) {
64 if (wasStopped) {
65 goal = new Map(await follower.syncRead(IDS, STS3215.Present_Position)); // re-seed from where the follower really is
66 wasStopped = false;
67 }
68 const next = Object.entries(frame).map(([id, ticks]) => {
69 const from = goal.get(Number(id)) ?? ticks;
70 const to = from + Math.max(-RAMP_STEP, Math.min(RAMP_STEP, ticks - from));
71 goal.set(Number(id), to);
72 return [Number(id), to] as const;
73 });
74 await follower.syncWrite(STS3215.Goal_Position, next);
75 sent++;
76 }
77 lags.push(performance.now() - t0);
78 if (lags.length > hz) lags.shift();
79 const sorted = [...lags].sort((a, b) => a - b);
80 onTick?.({ sent, lagMs: { p50: percentile(sorted, 0.5), p95: percentile(sorted, 0.95) }, leader: reading, target: frame, stopped: hold });
81 loops++;
82 const wait = started + loops * period - performance.now();
83 if (wait > 0) await new Promise((r) => setTimeout(r, wait));
84 }
85 return sent;
86 }
87
88 export function teleopScreen(s: TeleopSnapshot): string[] {
89 const rows = IDS.map((id) => {
90 const target = s.target[String(id)];
91 return ` ${(JOINTS[id] ?? "").padEnd(14)} leader ${String(s.leader.get(id) ?? "-").padStart(5)} → follower ${String(target ?? "-").padStart(5)}`;
92 });
93 return [
94 "so101 teleop — the leader arm drives the follower",
95 "",
96 ...rows,
97 "",
98 ` lag ${s.lagMs.p50.toFixed(1)} ms typical, ${s.lagMs.p95.toFixed(1)} ms worst frames ${s.sent}`,
99 "",
100 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)",
101 ];
102 }
Software/so101/src/setup.test.ts added +130
@@ -0,0 +1,130 @@
1 import { afterEach, beforeEach, expect, spyOn, test } from "bun:test";
2 import { mkdirSync, mkdtempSync, writeFileSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import { FeetechBus } from "./bus.ts";
6 import { FakeBus } from "./fake-bus.ts";
7 import { keyQueue } from "./keys.ts";
8 import { IDS, RAMP_STEP, runLocalTeleop } from "./local-teleop.ts";
9 import { driveScreen, ensureCalibrated, recalibrateJoint } from "./setup.ts";
10 import { STS3215 } from "./sts3215.ts";
11
12 const arm = () => {
13 const fake = new FakeBus([...IDS]);
14 return { fake, bus: new FeetechBus(fake) };
15 };
16 const setPresent = (fake: FakeBus, id: number, ticks: number) => {
17 const m = fake.servos.get(id)!;
18 m[56] = ticks & 0xff;
19 m[57] = ticks >> 8;
20 };
21 const limits = async (bus: FeetechBus, id: number) => [await bus.read(id, STS3215.Min_Position_Limit), await bus.read(id, STS3215.Max_Position_Limit)];
22 const settle = (ms: number) => new Promise((r) => setTimeout(r, ms));
23
24 let out: ReturnType<typeof spyOn>;
25 beforeEach(() => void (out = spyOn(process.stdout, "write").mockImplementation(() => true)));
26 afterEach(() => out.mockRestore());
27
28 async function calibrated(fake: FakeBus, bus: FeetechBus) {
29 for (const id of IDS) {
30 await bus.write(id, STS3215.Min_Position_Limit, 1000);
31 await bus.write(id, STS3215.Max_Position_Limit, 3000);
32 setPresent(fake, id, 2000);
33 }
34 }
35
36 test("teleop: the follower follows the leader, holds while stopped, and resumes without a jump", async () => {
37 const leader = arm();
38 const follower = arm();
39 await calibrated(leader.fake, leader.bus);
40 await calibrated(follower.fake, follower.bus);
41 let stopped = false;
42 const controller = new AbortController();
43 const run = runLocalTeleop(leader.bus, follower.bus, { hz: 200, signal: controller.signal, stopped: () => stopped });
44 await settle(40);
45 setPresent(leader.fake, 1, 2100);
46 await settle(60);
47 expect(follower.fake.position(1)).toBe(2100);
48
49 stopped = true;
50 await settle(30);
51 setPresent(leader.fake, 1, 2900); // moved far while stopped
52 await settle(60);
53 expect(follower.fake.position(1)).toBe(2100); // STOP holds
54
55 const goals: number[] = [];
56 const before = follower.fake.writes.length;
57 stopped = false;
58 await settle(120);
59 controller.abort();
60 await run;
61 for (const w of follower.fake.writes.slice(before)) if (w.address === STS3215.Goal_Position.address) goals.push(...[w.bytes[0]! | (w.bytes[1]! << 8)]);
62 expect(goals.length).toBeGreaterThan(0);
63 expect(Math.abs(goals[0]! - 2100)).toBeLessThanOrEqual(RAMP_STEP); // the first goal after resume is one bounded step away
64 expect(follower.fake.position(1)).toBeGreaterThan(2100);
65 });
66
67 test("recalibrateJoint: writes the swept range of just that joint when enter is pressed", async () => {
68 const { fake, bus } = arm();
69 const keys = keyQueue();
70 const done = recalibrateJoint(keys, bus, 2, "test");
71 for (const at of [1500, 400, 3500, 2000]) {
72 setPresent(fake, 2, at);
73 await settle(80);
74 }
75 keys.push("return");
76 await done;
77 expect(await limits(bus, 2)).toEqual([400, 3500]);
78 expect(await limits(bus, 3)).toEqual([0, 4095]);
79 });
80
81 test("recalibrateJoint: refuses a sweep that barely moved", async () => {
82 const { bus } = arm();
83 const keys = keyQueue();
84 const done = recalibrateJoint(keys, bus, 2, "test");
85 await settle(120);
86 keys.push("return");
87 await expect(done).rejects.toThrow("only moved");
88 });
89
90 test("ensureCalibrated: a forgotten range comes back from LeRobot's file; a good one is left alone", async () => {
91 const home = mkdtempSync(join(tmpdir(), "lerobot-"));
92 const dir = join(home, "lerobot", "calibration", "robots");
93 mkdirSync(dir, { recursive: true });
94 writeFileSync(join(dir, "so101.json"), JSON.stringify({ shoulder_pan: { id: 1, range_min: 800, range_max: 3200 } }));
95 const old = process.env.HF_HOME;
96 process.env.HF_HOME = home;
97 try {
98 const { fake, bus } = arm();
99 await bus.write(2, STS3215.Min_Position_Limit, 700);
100 await bus.write(2, STS3215.Max_Position_Limit, 3300);
101 for (const id of [3, 4, 5, 6]) {
102 await bus.write(id, STS3215.Min_Position_Limit, 700);
103 await bus.write(id, STS3215.Max_Position_Limit, 3300);
104 }
105 await ensureCalibrated(keyQueue(), bus, "robots", "follower");
106 expect(await limits(bus, 1)).toEqual([800, 3200]);
107 expect(await limits(bus, 2)).toEqual([700, 3300]);
108 expect(fake.position(1)).toBe(2048);
109 } finally {
110 if (old === undefined) delete process.env.HF_HOME;
111 else process.env.HF_HOME = old;
112 }
113 });
114
115 test("driveScreen: space stops, q ends it, and no key waiter is left behind", async () => {
116 const leader = arm();
117 const follower = arm();
118 await calibrated(leader.fake, leader.bus);
119 await calibrated(follower.fake, follower.bus);
120 const keys = keyQueue();
121 const done = driveScreen(keys, leader.bus, follower.bus, 100);
122 await settle(50);
123 keys.push("space");
124 await settle(30);
125 keys.push("q");
126 await done;
127 keys.push("down"); // the next step's key
128 expect(await keys.next()).toBe("down");
129 expect(await follower.bus.read(1, STS3215.Torque_Enable)).toBe(1); // q leaves torque on
130 });
Software/so101/src/setup.ts added +224
@@ -0,0 +1,224 @@
1 /** `so101 setup`: a guided, full-screen walk from two plugged-in arms to driving one with the other. */
2
3 import { SerialPort } from "serialport";
4 import { adaptersOf, resolveRoles, configFile, foundSentence, loadRoles, movement, saveRoles, savedRoles, WIGGLE_TICKS, type Adapter } from "./arms.ts";
5 import { judge, lerobotDirs, newestLerobot, type Verdict } from "./arm-calibration.ts";
6 import { FeetechBus } from "./bus.ts";
7 import { IDS, readLimits, runLocalTeleop, teleopScreen } from "./local-teleop.ts";
8 import { sleep } from "./motion.ts";
9 import { JOINTS, STS3215 } from "./sts3215.ts";
10 import { terminalKeys, type Keys } from "./keys.ts";
11
12 const STEPS = ["Find the arms", "Which is the leader", "Calibration", "Drive"];
13
14 export function progress(step: number): string {
15 return STEPS.map((name, i) => (i === step ? `[${i + 1} ${name}]` : `${i < step ? "✓" : " "}${i + 1} ${name}`)).join(" › ");
16 }
17
18 function show(step: number, lines: string[]): void {
19 const frame = ["so101 setup", progress(step), "", ...lines, ""].join("\n");
20 process.stdout.write(process.stdout.isTTY ? `\x1b[2J\x1b[H${frame}\n` : `${frame}\n`);
21 }
22
23 /** The dry run: what setup would do on this computer, touching no servo. */
24 export async function dryRun(write: (line: string) => void = console.log): Promise<void> {
25 const adapters = adaptersOf(await SerialPort.list());
26 const saved = loadRoles(configFile());
27 write("so101 setup (dry run) — nothing is moved or written");
28 write(progress(0));
29 write(` 1. ${foundSentence(adapters.length)} ${adapters.map((a) => `${a.path}${a.serial ? ` (#${a.serial})` : ""}`).join(", ")}`);
30 write(` 2. ${savedRoles(saved, adapters) ? "leader remembered by serial number" : "you wiggle the arm you will hold; its serial number is remembered"} (${configFile()})`);
31 for (const kind of ["robots", "teleoperators"] as const) {
32 const found = newestLerobot(lerobotDirs(kind));
33 write(` 3. LeRobot ${kind} calibration: ${found.size ? `${found.size} joints found, reused where the servos disagree` : "none found"}`);
34 }
35 write(" each servo's own limits are checked; only a joint out of range is calibrated again, one at a time");
36 write(" 4. so101 teleop drives the follower from the leader at 60 Hz; space stops it");
37 }
38
39 async function choose(key: Keys, step: number, title: string, options: string[]): Promise<number> {
40 let at = 0;
41 for (;;) {
42 show(step, [title, "", ...options.map((o, i) => ` ${i === at ? "›" : " "} ${o}`), "", " ↑/↓ to choose, enter to confirm, q to quit"]);
43 const k = await key.next();
44 if (k === "up") at = (at + options.length - 1) % options.length;
45 else if (k === "down") at = (at + 1) % options.length;
46 else if (k === "return") return at;
47 else if (k === "q" || k === "ctrl-c") throw new Quit();
48 }
49 }
50
51 class Quit extends Error {}
52
53 async function waitForArms(key: Keys): Promise<Adapter[]> {
54 for (;;) {
55 const found = adaptersOf(await SerialPort.list());
56 show(0, [foundSentence(found.length), ...found.map((a) => ` • ${a.path}${a.serial ? ` #${a.serial}` : ""}`), "", " q to quit"]);
57 if (found.length >= 2) return found;
58 const step = new AbortController();
59 const timer = setTimeout(() => step.abort(), 1000);
60 const waited = await key.next(step.signal);
61 clearTimeout(timer);
62 step.abort();
63 if (waited === "q" || waited === "ctrl-c") throw new Quit();
64 }
65 }
66
67 /** Watch both buses until one is moved by hand; that one is the leader. */
68 async function whoIsTheLeader(key: Keys, adapters: Adapter[]): Promise<{ leader: Adapter; follower: Adapter }> {
69 const [a, b] = adapters as [Adapter, Adapter];
70 const buses = [await FeetechBus.open(a.path), await FeetechBus.open(b.path)] as const;
71 try {
72 const base = await Promise.all(buses.map((bus) => bus.syncRead(IDS, STS3215.Present_Position)));
73 const step = new AbortController();
74 let stop = false;
75 void key.next(step.signal).then((k) => (stop = k !== null));
76 try {
77 while (!stop) {
78 show(1, ["Wiggle the arm you'll hold (the leader).", "", ` watching ${a.path} and ${b.path}…`, "", " q to quit"]);
79 await sleep(100);
80 const now = await Promise.all(buses.map((bus) => bus.syncRead(IDS, STS3215.Present_Position)));
81 const moved = now.map((m, i) => movement(base[i]!, m));
82 const [ma = 0, mb = 0] = moved;
83 if (Math.max(ma, mb) >= WIGGLE_TICKS && Math.abs(ma - mb) >= WIGGLE_TICKS / 2) return ma > mb ? { leader: a, follower: b } : { leader: b, follower: a };
84 }
85 throw new Quit();
86 } finally {
87 step.abort();
88 }
89 } finally {
90 await Promise.all(buses.map((bus) => bus.close()));
91 }
92 }
93
94 /** One joint, one live bar: release the servo, watch the range the person sweeps, write it when they press enter. */
95 export async function recalibrateJoint(key: Keys, bus: FeetechBus, id: number, why: string): Promise<void> {
96 await bus.write(id, STS3215.Torque_Enable, 0);
97 let lo = 4095;
98 let hi = 0;
99 let done = false;
100 const step = new AbortController();
101 void key.next(step.signal).then((k) => (done = k !== null));
102 try {
103 while (!done) {
104 const at = await bus.read(id, STS3215.Present_Position);
105 lo = Math.min(lo, at);
106 hi = Math.max(hi, at);
107 const width = 40;
108 const cell = (t: number) => Math.min(width - 1, Math.floor((t / 4096) * width));
109 const barCells = Array.from({ length: width }, (_, i) => (i === cell(at) ? "█" : i >= cell(lo) && i <= cell(hi) ? "▒" : "·"));
110 show(2, [
111 `${JOINTS[id]} (servo ${id}) needs calibrating: ${why}.`,
112 "",
113 ` Move only the ${JOINTS[id]?.replace("_", " ")} through its whole travel, both ends.`,
114 "",
115 ` [${barCells.join("")}]`,
116 ` now ${at} low ${lo} high ${hi}`,
117 "",
118 " enter when you have reached both ends",
119 ]);
120 await sleep(50);
121 }
122 } finally {
123 step.abort();
124 }
125 if (hi - lo < 200) throw new Error(`${JOINTS[id]} only moved ${hi - lo} ticks; run setup again and sweep its whole travel`);
126 await bus.write(id, STS3215.Min_Position_Limit, lo);
127 await bus.write(id, STS3215.Max_Position_Limit, hi);
128 }
129
130 export async function ensureCalibrated(key: Keys, bus: FeetechBus, kind: "robots" | "teleoperators", who: string): Promise<void> {
131 const present = await bus.syncRead(IDS, STS3215.Present_Position);
132 const verdicts: Verdict[] = judge(await readLimits(bus), present, newestLerobot(lerobotDirs(kind)));
133 for (const v of verdicts) {
134 if (v.state === "restore") await bus.write(v.id, STS3215.Min_Position_Limit, v.range.min).then(() => bus.write(v.id, STS3215.Max_Position_Limit, v.range.max));
135 else if (v.state === "recalibrate") await recalibrateJoint(key, bus, v.id, `${who}: ${v.why}`);
136 }
137 const fixed = verdicts.filter((v) => v.state !== "ok").length;
138 show(2, [`${who}: ${fixed ? `${fixed} joint${fixed === 1 ? "" : "s"} fixed, the rest kept as calibrated` : "calibration is fine, kept as it is"}.`]);
139 await sleep(700);
140 }
141
142 export interface SetupOptions {
143 hz?: number;
144 }
145
146 /** Drive the follower from the leader on screen until q. Space stops and resumes; q leaves the follower holding with torque on. */
147 export async function driveScreen(key: Keys, leader: FeetechBus, follower: FeetechBus, hz = 60): Promise<void> {
148 const controller = new AbortController();
149 let stopped = false;
150 let last = 0;
151 const listening = (async () => {
152 for (;;) {
153 const k = await key.next(controller.signal);
154 if (k === null) return;
155 if (k === "space") stopped = !stopped;
156 else if (k === "q" || k === "ctrl-c") return controller.abort();
157 }
158 })();
159 try {
160 await runLocalTeleop(leader, follower, {
161 hz,
162 signal: controller.signal,
163 stopped: () => stopped,
164 onTick: (s) => {
165 if (performance.now() - last < 100) return;
166 last = performance.now();
167 process.stdout.write(`\x1b[2J\x1b[H${teleopScreen(s).join("\n")}\n`);
168 },
169 });
170 } finally {
171 controller.abort();
172 await listening;
173 }
174 }
175
176 export async function setup(options: SetupOptions = {}): Promise<void> {
177 const key = terminalKeys();
178 let leader: FeetechBus | undefined;
179 let follower: FeetechBus | undefined;
180 try {
181 const adapters = await waitForArms(key);
182 const file = configFile();
183 let roles = savedRoles(loadRoles(file), adapters);
184 if (roles) {
185 const again = await choose(key, 1, `I remember the leader (#${roles.leader.serial}).`, ["Use it", "Tell the arms apart again"]);
186 if (again === 1) roles = null;
187 }
188 if (!roles) {
189 roles = await whoIsTheLeader(key, adapters);
190 if (roles.leader.serial && roles.follower.serial) saveRoles(file, { leader: roles.leader.serial, follower: roles.follower.serial });
191 }
192 leader = await FeetechBus.open(roles.leader.path);
193 follower = await FeetechBus.open(roles.follower.path);
194 await ensureCalibrated(key, leader, "teleoperators", "leader");
195 await ensureCalibrated(key, follower, "robots", "follower");
196 await driveScreen(key, leader, follower, options.hz ?? 60);
197 show(3, ["Stopped. The follower is still holding with torque on; `so101 torque off` releases it."]);
198 } catch (err) {
199 if (!(err instanceof Quit)) throw err;
200 show(3, ["Bye."]);
201 } finally {
202 key.close();
203 await leader?.close();
204 await follower?.close();
205 }
206 }
207
208 /** `so101 teleop`: skip the questions, use the remembered arms. */
209 export async function teleop(leaderPath: string | undefined, followerPath: string | undefined, hz: number): Promise<void> {
210 const ports = resolveRoles(leaderPath, followerPath, loadRoles(configFile()), leaderPath && followerPath ? [] : adaptersOf(await SerialPort.list()));
211 let key: Keys | undefined;
212 let leader: FeetechBus | undefined;
213 let follower: FeetechBus | undefined;
214 try {
215 leader = await FeetechBus.open(ports.leader);
216 follower = await FeetechBus.open(ports.follower);
217 key = terminalKeys(); // raw mode only once both ports are open
218 await driveScreen(key, leader, follower, hz);
219 } finally {
220 key?.close();
221 await leader?.close();
222 await follower?.close();
223 }
224 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs