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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

`so101 setup` finds both arms, asks you to wiggle the leader, reuses existing calibration, then starts teleop #575

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/572-so101-setup-finds-both-arms-asks-you-to → main 4 commits fork 3 threads · 3 unresolved

  • Merged. smith/572-so101-setup-finds-both-arms-asks-you-to was merged into main as f44d21f.

Review

Sign in to review this change.
10 files changed +544 −8
Software/so101/README.md +8 −7
@@ -4,17 +4,18 @@
44
55 ## Plug in and play
66
7 Two SO-101 arms on USB, calibrated as in https://huggingface.co/docs/lerobot/so101 (the same assembly and calibration guide):
7 Two SO-101 arms on USB, Node 22.6+ and Bun, nothing else:
88
99 ```
10 bun so101 teleop [--leader /dev/ttyACM0 --follower /dev/ttyACM1] [--hz 60]
10 bun install && bun so101 setup
1111 ```
1212
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`.
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 teleop --leader P --follower P [--save]` names the ports, and `--save` remembers them). `bun so101 setup --dry-run` shows the plan without touching an arm. Assembly and calibration background:
18 https://huggingface.co/docs/lerobot/so101
1819
1920 ## Open / Closed
2021
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 +29
@@ -0,0 +1,29 @@
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, offset: 5 }]]);
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, offset: 5 } });
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 });
24
25 test("a joint with a missing or out-of-bounds homing_offset is not trusted", () => {
26 const joint = { id: 1, range_min: 800, range_max: 3200 };
27 for (const homing_offset of [undefined, 2048, -2048, 1.5, "5"]) expect(parseLerobot({ shoulder_pan: { ...joint, homing_offset } }).size).toBe(0);
28 expect(parseLerobot({ shoulder_pan: { ...joint, homing_offset: -2047 } }).get(1)?.offset).toBe(-2047);
29 });
Software/so101/src/arm-calibration.ts added +79
@@ -0,0 +1,79 @@
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 /** A range only means something with the Homing_Offset it was measured under (LeRobot's ticks are offset-corrected). */
18 export type Calibration = Range & { offset: number };
19 export const MAX_OFFSET = 2047;
20
21 export type Verdict =
22 | { id: number; state: "ok" }
23 | { id: number; state: "restore"; range: Calibration } // the servo forgot; LeRobot's file knows
24 | { id: number; state: "recalibrate"; why: string };
25
26 export function lerobotDirs(kind: "robots" | "teleoperators", home: string = homedir(), env: Record<string, string | undefined> = process.env): string {
27 const cache = env.HF_HOME ? join(env.HF_HOME, "lerobot") : join(env.XDG_CACHE_HOME ?? join(home, ".cache"), "huggingface", "lerobot");
28 return join(cache, "calibration", kind);
29 }
30
31 /** LeRobot's calibration JSON: { joint_name: { id, homing_offset, range_min, range_max, … } } → servo id → range and offset. Bad entries are skipped. */
32 export function parseLerobot(body: unknown): Map<number, Calibration> {
33 const out = new Map<number, Calibration>();
34 if (typeof body !== "object" || body === null) return out;
35 for (const [name, joint] of Object.entries(body)) {
36 const j = joint as { id?: unknown; range_min?: unknown; range_max?: unknown; homing_offset?: unknown } | null;
outdated

homing_offset is discarded here, but range_min/range_max only mean anything together with it. Return { min, max, offset } and validate offset as an integer within ±2047.

37 const id = Object.entries(JOINTS).find(([, joint]) => joint === name)?.[0];
38 const tick = (v: unknown): v is number => typeof v === "number" && Number.isInteger(v) && v >= 0 && v <= FULL_TURN;
39 if (id === undefined || !j || !tick(j.range_min) || !tick(j.range_max) || j.range_max - j.range_min < MIN_RANGE) continue;
40 const offset = j.homing_offset;
41 if (typeof offset !== "number" || !Number.isInteger(offset) || Math.abs(offset) > MAX_OFFSET) continue;
42 out.set(Number(id), { min: j.range_min, max: j.range_max, offset });
43 }
44 return out;
45 }
46
47 /** The newest calibration file in `dir`, parsed; an empty map when there is none. */
48 export function newestLerobot(dir: string): Map<number, Calibration> {
49 let best: { file: string; at: number } | null = null;
50 try {
51 for (const name of readdirSync(dir)) {
52 if (!name.endsWith(".json")) continue;
53 const file = join(dir, name);
54 const at = statSync(file).mtimeMs;
55 if (!best || at > best.at) best = { file, at };
56 }
57 return best ? parseLerobot(JSON.parse(readFileSync(best.file, "utf8"))) : new Map();
58 } catch {
59 return new Map();
60 }
61 }
62
63 /** 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. */
64 export function judge(limits: ReadonlyMap<number, Range>, present: ReadonlyMap<number, number>, reference: ReadonlyMap<number, Calibration> = new Map()): Verdict[] {
65 const verdicts: Verdict[] = [];
66 for (const [id, range] of limits) {
67 const ref = reference.get(id);
68 const set = range.max - range.min >= MIN_RANGE && !(range.min === 0 && range.max === FULL_TURN);
69 const at = present.get(id);
70 if (!set) {
71 verdicts.push(ref ? { id, state: "restore", range: ref } : { id, state: "recalibrate", why: "its range was never set" });
72 } else if (at !== undefined && (at < range.min - 20 || at > range.max + 20)) {
73 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}` });
74 } else {
75 verdicts.push({ id, state: "ok" });
76 }
77 }
78 return verdicts;
79 }
Software/so101/src/arms.test.ts +7 −1
@@ -2,7 +2,7 @@ import { expect, test } from "bun:test";
22 import { mkdtempSync } from "node:fs";
33 import { tmpdir } from "node:os";
44 import { join } from "node:path";
5 import { adaptersOf, resolveRoles, rolesOf, loadRoles, saveRoles, savedRoles } from "./arms.ts";
5 import { adaptersOf, movement, WIGGLE_TICKS, resolveRoles, rolesOf, loadRoles, saveRoles, savedRoles } from "./arms.ts";
66
77 test("adapters: USB serial only, one per board, macOS callout device", () => {
88 const list = [
@@ -53,3 +53,9 @@ test("--save finds a typed macOS tty path among the cu adapters", () => {
5353 Object.defineProperty(process, "platform", was);
5454 }
5555 });
56
57 test("movement adds up the ticks each joint moved and ignores joints missing from either reading", () => {
58 expect(movement(new Map([[1, 100], [2, 200]]), new Map([[1, 150], [2, 150], [3, 9]]))).toBe(100);
59 expect(movement(new Map(), new Map([[1, 5]]))).toBe(0);
60 expect(WIGGLE_TICKS).toBeGreaterThan(0);
61 });
Software/so101/src/arms.ts +19
@@ -33,6 +33,25 @@ export function adaptersOf(list: readonly RawPort[], platform: string = process.
3333 return [...seen.values()].sort((a, b) => a.path.localeCompare(b.path));
3434 }
3535
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
3655 export interface Roles {
3756 leader: string;
3857 follower: string;
Software/so101/src/cli-node.test.ts +7
@@ -39,3 +39,10 @@ test("the CLI's static imports never reach a bun: module", () => {
3939 expect(seen.size).toBeGreaterThan(3);
4040 expect(offenders).toEqual([]);
4141 });
42
43 test.skipIf(!strips)("setup --dry-run runs under Node and touches no servo", () => {
44 const dry = so101("setup", "--dry-run");
45 expect(dry.stderr).toBe("");
46 expect(dry.stdout).toContain("so101 setup (dry run)");
47 expect(dry.status).toBe(0);
48 });
Software/so101/src/cli.ts +9
@@ -49,6 +49,7 @@ usage: so101 <command> [options]
4949 HTTP API + WebSocket target stream for this arm (run where the follower is)
5050 leader --to ws://host:8101 [--hz 60]
5151 read this leader arm and drive a remote follower through its serve
52 setup [--dry-run] guided: find both arms, reuse calibration, then drive
5253 teleop [--leader P --follower P [--save]] [--hz 60]
5354 the leader arm drives the follower here, space stops it, q quits
5455
@@ -75,6 +76,7 @@ const OPTIONS = {
7576 hold: { type: "boolean" },
7677 listen: { type: "string" },
7778 save: { type: "boolean" },
79 "dry-run": { type: "boolean" },
7880 leader: { type: "string" },
7981 follower: { type: "string" },
8082 to: { type: "string" },
@@ -99,6 +101,7 @@ interface Values {
99101 hold?: boolean;
100102 listen?: string;
101103 save?: boolean;
104 "dry-run"?: boolean;
102105 leader?: string;
103106 follower?: string;
104107 model?: string;
@@ -535,6 +538,12 @@ const COMMANDS: Record<string, Command> = {
535538 });
536539 },
537540
541 setup: async (values) => {
542 const { dryRun, setup } = await import("./setup.ts");
543 if (values["dry-run"]) return dryRun();
544 await setup({ hz: bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz") });
545 },
546
538547 teleop: async (values) => {
539548 const { teleop } = await import("./local-teleop-run.ts");
540549 await teleop(values.leader, values.follower, bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz"), values.save);
Software/so101/src/setup.test.ts added +182
@@ -0,0 +1,182 @@
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 } from "./local-teleop.ts";
9 import { foundSentence } from "./arms.ts";
10 import { ensureCalibrated, progress, recalibrateJoint, whoIsTheLeader } from "./setup.ts";
11 import { STS3215 } from "./sts3215.ts";
12
13 const arm = () => {
14 const fake = new FakeBus([...IDS]);
15 return { fake, bus: new FeetechBus(fake) };
16 };
17 const setPresent = (fake: FakeBus, id: number, ticks: number) => {
18 const m = fake.servos.get(id)!;
19 m[56] = ticks & 0xff;
20 m[57] = ticks >> 8;
21 };
22 const limits = async (bus: FeetechBus, id: number) => [await bus.read(id, STS3215.Min_Position_Limit), await bus.read(id, STS3215.Max_Position_Limit)];
23 const settle = (ms: number) => new Promise((r) => setTimeout(r, ms));
24
25 let out: ReturnType<typeof spyOn>;
26 beforeEach(() => void (out = spyOn(process.stdout, "write").mockImplementation(() => true)));
27 afterEach(() => out.mockRestore());
28
29 async function calibrated(fake: FakeBus, bus: FeetechBus) {
30 for (const id of IDS) {
31 await bus.write(id, STS3215.Min_Position_Limit, 1000);
32 await bus.write(id, STS3215.Max_Position_Limit, 3000);
33 setPresent(fake, id, 2000);
34 }
35 }
36
37 test("recalibrateJoint: writes the swept range of just that joint when enter is pressed", async () => {
38 const { fake, bus } = arm();
39 const keys = keyQueue();
40 const done = recalibrateJoint(keys, bus, 2, "test");
41 for (const at of [1500, 400, 3500, 2000]) {
42 setPresent(fake, 2, at);
43 await settle(80);
44 }
45 keys.push("return");
46 await done;
47 expect(await limits(bus, 2)).toEqual([400, 3500]);
48 expect(await limits(bus, 3)).toEqual([0, 4095]);
49 });
50
51 test("recalibrateJoint: refuses a sweep that crosses the 4095→0 wrap", async () => {
52 const { fake, bus } = arm();
53 const keys = keyQueue();
54 const done = recalibrateJoint(keys, bus, 2, "test");
55 for (const at of [20, 4080, 2000]) {
56 setPresent(fake, 2, at);
57 await settle(80);
58 }
59 keys.push("return");
60 await expect(done).rejects.toThrow("wrap");
61 });
62
63 test("recalibrateJoint: refuses a sweep that barely moved", async () => {
64 const { bus } = arm();
65 const keys = keyQueue();
66 const done = recalibrateJoint(keys, bus, 2, "test");
67 await settle(120);
68 keys.push("return");
69 await expect(done).rejects.toThrow("only moved");
70 });
71
72 test("ensureCalibrated: a forgotten range comes back from LeRobot's file; a good one is left alone", async () => {
73 const home = mkdtempSync(join(tmpdir(), "lerobot-"));
74 const dir = join(home, "lerobot", "calibration", "robots");
75 mkdirSync(dir, { recursive: true });
76 writeFileSync(join(dir, "so101.json"), JSON.stringify({ shoulder_pan: { id: 1, homing_offset: -123, range_min: 800, range_max: 3200 } }));
77 const old = process.env.HF_HOME;
78 process.env.HF_HOME = home;
79 try {
80 const { fake, bus } = arm();
81 await bus.write(2, STS3215.Min_Position_Limit, 700);
82 await bus.write(2, STS3215.Max_Position_Limit, 3300);
83 for (const id of [3, 4, 5, 6]) {
84 await bus.write(id, STS3215.Min_Position_Limit, 700);
85 await bus.write(id, STS3215.Max_Position_Limit, 3300);
86 }
87 await ensureCalibrated(keyQueue(), bus, "robots", "follower");
88 expect(await limits(bus, 1)).toEqual([800, 3200]);
89 expect(await limits(bus, 2)).toEqual([700, 3300]);
90 expect(await bus.read(1, STS3215.Homing_Offset)).toBe(-123);
91 expect(fake.position(1)).toBe(2048);
92 } finally {
93 if (old === undefined) delete process.env.HF_HOME;
94 else process.env.HF_HOME = old;
95 }
96 });
97
98 test("whoIsTheLeader: closes the first port when the second will not open", async () => {
99 const first = new FakeBus([...IDS]);
100 let closed = false;
101 const close = first.close.bind(first);
102 first.close = (callback) => {
103 closed = true;
104 close(callback);
105 };
106 let calls = 0;
107 const open = spyOn(FeetechBus, "open").mockImplementation(async () => {
108 if (++calls === 2) throw new Error("no such port");
109 return new FeetechBus(first);
110 });
111 try {
112 await expect(whoIsTheLeader(keyQueue(), [{ path: "/dev/a", serial: "1" }, { path: "/dev/b", serial: "2" }])).rejects.toThrow("no such port");
113 expect(closed).toBe(true);
114 } finally {
115 open.mockRestore();
116 }
117 });
118
119 const twoArms = () => {
120 const a = arm();
121 const b = arm();
122 const open = spyOn(FeetechBus, "open").mockImplementation(async (path: string) => (path === "/dev/a" ? a.bus : b.bus));
123 return { a, b, open };
124 };
125 const adapters = [{ path: "/dev/a", serial: "1" }, { path: "/dev/b", serial: "2" }];
126
127 test("whoIsTheLeader: the arm that is moved by hand is the leader", async () => {
128 const { a, b, open } = twoArms();
129 try {
130 const found = whoIsTheLeader(keyQueue(), adapters);
131 await settle(50);
132 setPresent(b.fake, 1, 2048 + 400);
133 expect((await found).leader.path).toBe("/dev/b");
134 } finally {
135 open.mockRestore();
136 }
137 void a;
138 });
139
140 test("whoIsTheLeader: a small jiggle of both arms decides nothing, and an unrelated key does not quit", async () => {
141 const { a, b, open } = twoArms();
142 const keys = keyQueue();
143 try {
144 let result = "pending";
145 const found = whoIsTheLeader(keys, adapters).then(() => (result = "decided"), () => (result = "quit"));
146 await settle(50);
147 setPresent(a.fake, 1, 2048 + 150);
148 setPresent(b.fake, 1, 2048 + 150);
149 keys.push("x");
150 await settle(300);
151 expect(result).toBe("pending");
152 keys.push("q");
153 await found;
154 expect(result).toBe("quit");
155 } finally {
156 open.mockRestore();
157 }
158 });
159
160 test("recalibrateJoint: only enter ends the sweep", async () => {
161 const { fake, bus } = arm();
162 const keys = keyQueue();
163 const done = recalibrateJoint(keys, bus, 2, "test");
164 setPresent(fake, 2, 500);
165 await settle(80);
166 setPresent(fake, 2, 3000);
167 await settle(80);
168 keys.push("x");
169 await settle(100);
170 expect(await limits(bus, 2)).toEqual([0, 4095]);
171 keys.push("return");
172 await done;
173 expect(await limits(bus, 2)).toEqual([500, 3000]);
174 });
175
176 test("progress marks the current and finished steps; foundSentence reads 0, 1 and 2 arms", () => {
177 expect(progress(1)).toContain("✓1 Find the arms");
178 expect(progress(1)).toContain("[2 Which is the leader]");
179 expect(foundSentence(0)).toContain("no arms");
180 expect(foundSentence(1)).toContain("I see 1 arm. Plug in the other one");
181 expect(foundSentence(2)).toBe("I see 2 arms.");
182 });
Software/so101/src/setup.ts added +203
@@ -0,0 +1,203 @@
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, configFile, foundSentence, loadRoles, movement, saveRoles, savedRoles, WIGGLE_TICKS, type Adapter } from "./arms.ts";
5 import { FULL_TURN, MIN_RANGE, judge, lerobotDirs, newestLerobot, type Verdict } from "./arm-calibration.ts";
6 import { FeetechBus } from "./bus.ts";
7 import { IDS, readLimits } from "./local-teleop.ts";
8 import { driveScreen } from "./local-teleop-run.ts";
9 import { sleep } from "./motion.ts";
10 import { JOINTS, STS3215 } from "./sts3215.ts";
11 import { terminalKeys, type Keys } from "./keys.ts";
12
13 const STEPS = ["Find the arms", "Which is the leader", "Calibration", "Drive"];
14
15 export function progress(step: number): string {
16 return STEPS.map((name, i) => (i === step ? `[${i + 1} ${name}]` : `${i < step ? "✓" : " "}${i + 1} ${name}`)).join(" › ");
17 }
18
19 function show(step: number, lines: string[]): void {
20 const frame = ["so101 setup", progress(step), "", ...lines, ""].join("\n");
21 process.stdout.write(process.stdout.isTTY ? `\x1b[2J\x1b[H${frame}\n` : `${frame}\n`);
22 }
23
24 /** The dry run: what setup would do on this computer, touching no servo. */
25 export async function dryRun(write: (line: string) => void = console.log): Promise<void> {
26 const adapters = adaptersOf(await SerialPort.list().catch(() => []));
27 const saved = loadRoles(configFile());
28 write("so101 setup (dry run) — nothing is moved or written");
29 write(progress(0));
30 write(` 1. I see ${adapters.length} arm${adapters.length === 1 ? "" : "s"}: ${adapters.map((a) => `${a.path}${a.serial ? ` (#${a.serial})` : ""}`).join(", ")}`);
31 write(` 2. ${savedRoles(saved, adapters) ? "leader remembered by serial number" : "you wiggle the arm you will hold; its serial number is remembered"} (${configFile()})`);
32 for (const kind of ["robots", "teleoperators"] as const) {
33 const found = newestLerobot(lerobotDirs(kind));
34 write(` 3. LeRobot ${kind} calibration: ${found.size ? `${found.size} joints found, reused where the servos disagree` : "none found"}`);
35 }
36 write(" each servo's own limits are checked; only a joint out of range is calibrated again, one at a time");
37 write(" 4. so101 teleop drives the follower from the leader at 60 Hz; space stops it");
38 }
39
40 async function choose(key: Keys, step: number, title: string, options: string[]): Promise<number> {
41 let at = 0;
42 for (;;) {
43 show(step, [title, "", ...options.map((o, i) => ` ${i === at ? "›" : " "} ${o}`), "", " ↑/↓ to choose, enter to confirm, q to quit"]);
44 const k = await key.next();
45 if (k === "up") at = (at + options.length - 1) % options.length;
46 else if (k === "down") at = (at + 1) % options.length;
47 else if (k === "return") return at;
48 else if (k === "q" || k === "ctrl-c") throw new Quit();
49 }
50 }
51
52 class Quit extends Error {}
53
54 /** Call `hit` when `accept` likes a key; other keys are read and ignored until the signal aborts. */
55 function onKey(key: Keys, signal: AbortSignal, accept: (k: string) => boolean, hit: () => void): void {
56 void (async () => {
57 for (;;) {
58 const k = await key.next(signal);
59 if (k === null) return;
60 if (accept(k)) return hit();
61 }
62 })();
63 }
64
65 async function waitForArms(key: Keys): Promise<Adapter[]> {
66 for (;;) {
67 const found = adaptersOf(await SerialPort.list());
68 show(0, [foundSentence(found.length), ...found.map((a) => ` • ${a.path}${a.serial ? ` #${a.serial}` : ""}`), "", " q to quit"]);
69 if (found.length >= 2) return found;
70 const step = new AbortController();
71 const timer = setTimeout(() => step.abort(), 1000);
72 const waited = await key.next(step.signal);
73 clearTimeout(timer);
74 step.abort();
75 if (waited === "q" || waited === "ctrl-c") throw new Quit();
76 }
77 }
78
79 /** Watch both buses until one is moved by hand; that one is the leader. */
80 export async function whoIsTheLeader(key: Keys, adapters: Adapter[]): Promise<{ leader: Adapter; follower: Adapter }> {
81 const [a, b] = adapters as [Adapter, Adapter];
82 const first = await FeetechBus.open(a.path);
83 let second: FeetechBus;
84 try {
85 second = await FeetechBus.open(b.path);
86 } catch (err) {
87 await first.close(); // the second arm would not open: do not leave the first port held
88 throw err;
89 }
90 const buses = [first, second] as const;
91 try {
92 const base = await Promise.all(buses.map((bus) => bus.syncRead(IDS, STS3215.Present_Position)));
93 const step = new AbortController();
94 let stop = false;
95 onKey(key, step.signal, (k) => k === "q" || k === "ctrl-c", () => (stop = true));
96 try {
97 while (!stop) {
98 show(1, ["Wiggle the arm you'll hold (the leader).", "", ` watching ${a.path} and ${b.path}…`, "", " q to quit"]);
99 await sleep(100);
100 const now = await Promise.all(buses.map((bus) => bus.syncRead(IDS, STS3215.Present_Position)));
101 const moved = now.map((m, i) => movement(base[i]!, m));
102 const [ma = 0, mb = 0] = moved;
103 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 };
104 }
105 throw new Quit();
106 } finally {
107 step.abort();
108 }
109 } finally {
110 await Promise.all(buses.map((bus) => bus.close()));
111 }
112 }
113
114 /** A sweep that touches both ends of the dial this closely wrapped around 4095→0. */
115 const WRAP_EDGE = 50;
116
117 /** One joint, one live bar: release the servo, watch the range the person sweeps, write it when they press enter. */
118 export async function recalibrateJoint(key: Keys, bus: FeetechBus, id: number, why: string): Promise<void> {
119 await bus.write(id, STS3215.Torque_Enable, 0);
120 let lo = 4095;
121 let hi = 0;
122 let done = false;
123 const step = new AbortController();
124 onKey(key, step.signal, (k) => k === "return", () => (done = true));
125 try {
outdated

Medium: a travel that crosses the 4095→0 wrap gives lo≈0, hi≈4095. That passes MIN_RANGE and is written as a full-turn range, which judge() flags as unset next run. Refuse a sweep that touches both ends, and test it.

126 while (!done) {
127 const at = await bus.read(id, STS3215.Present_Position);
128 lo = Math.min(lo, at);
129 hi = Math.max(hi, at);
130 const width = 40;
131 const cell = (t: number) => Math.min(width - 1, Math.floor((t / 4096) * width));
132 const barCells = Array.from({ length: width }, (_, i) => (i === cell(at) ? "█" : i >= cell(lo) && i <= cell(hi) ? "▒" : "·"));
133 show(2, [
134 `${JOINTS[id]} (servo ${id}) needs calibrating: ${why}.`,
135 "",
136 ` Move only the ${JOINTS[id]?.replace("_", " ")} through its whole travel, both ends.`,
137 "",
138 ` [${barCells.join("")}]`,
139 ` now ${at} low ${lo} high ${hi}`,
140 "",
141 " enter when you have reached both ends",
142 ]);
143 await sleep(50);
144 }
145 } finally {
146 step.abort();
147 }
148 if (lo < WRAP_EDGE && hi > FULL_TURN - WRAP_EDGE) throw new Error(`${JOINTS[id]} read both ends of the dial (${lo}-${hi}): its travel crosses the 4095→0 wrap; run setup again and sweep it from a different starting position`);
149 if (hi - lo < MIN_RANGE) throw new Error(`${JOINTS[id]} only moved ${hi - lo} ticks; run setup again and sweep its whole travel`);
150 await bus.write(id, STS3215.Min_Position_Limit, lo);
151 await bus.write(id, STS3215.Max_Position_Limit, hi);
152 }
153
154 export async function ensureCalibrated(key: Keys, bus: FeetechBus, kind: "robots" | "teleoperators", who: string): Promise<void> {
outdated

High: write STS3215.Homing_Offset from the file before the limits, or the restored range sits in the wrong frame on a servo whose offset was reset. Add a test asserting the offset register after ensureCalibrated.

155 const present = await bus.syncRead(IDS, STS3215.Present_Position);
156 const verdicts: Verdict[] = judge(await readLimits(bus), present, newestLerobot(lerobotDirs(kind)));
157 for (const v of verdicts) {
158 if (v.state === "restore") {
159 await bus.write(v.id, STS3215.Homing_Offset, v.range.offset);
160 await bus.write(v.id, STS3215.Min_Position_Limit, v.range.min);
161 await bus.write(v.id, STS3215.Max_Position_Limit, v.range.max);
162 }
163 else if (v.state === "recalibrate") await recalibrateJoint(key, bus, v.id, `${who}: ${v.why}`);
164 }
165 const fixed = verdicts.filter((v) => v.state !== "ok").length;
166 show(2, [`${who}: ${fixed ? `${fixed} joint${fixed === 1 ? "" : "s"} fixed, the rest kept as calibrated` : "calibration is fine, kept as it is"}.`]);
167 await sleep(700);
168 }
169
170 export interface SetupOptions {
171 hz?: number;
172 }
173
174 export async function setup(options: SetupOptions = {}): Promise<void> {
175 const key = terminalKeys();
176 let leader: FeetechBus | undefined;
177 let follower: FeetechBus | undefined;
178 try {
179 const adapters = await waitForArms(key);
180 const file = configFile();
181 let roles = savedRoles(loadRoles(file), adapters);
182 if (roles) {
183 const again = await choose(key, 1, `I remember the leader (#${roles.leader.serial}).`, ["Use it", "Tell the arms apart again"]);
184 if (again === 1) roles = null;
185 }
186 if (!roles) {
187 roles = await whoIsTheLeader(key, adapters);
188 if (roles.leader.serial && roles.follower.serial) saveRoles(file, { leader: roles.leader.serial, follower: roles.follower.serial });
189 }
190 leader = await FeetechBus.open(roles.leader.path);
191 follower = await FeetechBus.open(roles.follower.path);
192 await ensureCalibrated(key, leader, "teleoperators", "leader");
193 await ensureCalibrated(key, follower, "robots", "follower");
194 await driveScreen(key, leader, follower, options.hz ?? 60);
195 } catch (err) {
196 if (!(err instanceof Quit)) throw err;
197 show(3, ["Bye."]);
198 } finally {
199 key.close();
200 await leader?.close();
201 await follower?.close();
202 }
203 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs