Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
`so101 setup` finds both arms, asks you to wiggle the leader, reuses existing calibration, then starts teleop #575
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/572-so101-setup-finds-both-arms-asks-you-to → main 4 commits fork 3 threads · 3 unresolved
Software/so101/README.md +8 −7
| @@ -4,17 +4,18 @@ | ||
| 4 | 4 | |
| 5 | 5 | ## Plug in and play |
| 6 | 6 | |
| 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: | |
| 8 | 8 | |
| 9 | 9 | |
| 10 | bun so101 teleop [--leader /dev/ttyACM0 --follower /dev/ttyACM1] [--hz 60] | |
| 10 | bun install && bun so101 setup | |
| 11 | 11 | |
| 12 | 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`. | |
| 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 | |
| 18 | 19 | |
| 19 | 20 | ## Open / Closed |
| 20 | 21 |
Software/so101/package.json +1
| @@ -8,6 +8,7 @@ | ||
| 8 | 8 | "so101": "bin/so101" |
| 9 | 9 | }, |
| 10 | 10 | "scripts": { |
| 11 | "so101": "node bin/so101", | |
| 11 | 12 | "typecheck": "tsc --noEmit", |
| 12 | 13 | "test": "bun test" |
| 13 | 14 | }, |
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; | |
| 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"; | ||
| 2 | 2 | import { mkdtempSync } from "node:fs"; |
| 3 | 3 | import { tmpdir } from "node:os"; |
| 4 | 4 | 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"; | |
| 6 | 6 | |
| 7 | 7 | test("adapters: USB serial only, one per board, macOS callout device", () => { |
| 8 | 8 | const list = [ |
| @@ -53,3 +53,9 @@ test("--save finds a typed macOS tty path among the cu adapters", () => { | ||
| 53 | 53 | Object.defineProperty(process, "platform", was); |
| 54 | 54 | } |
| 55 | 55 | }); |
| 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. | ||
| 33 | 33 | return [...seen.values()].sort((a, b) => a.path.localeCompare(b.path)); |
| 34 | 34 | } |
| 35 | 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 | ||
| 36 | 55 | export interface Roles { |
| 37 | 56 | leader: string; |
| 38 | 57 | 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", () => { | ||
| 39 | 39 | expect(seen.size).toBeGreaterThan(3); |
| 40 | 40 | expect(offenders).toEqual([]); |
| 41 | 41 | }); |
| 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] | ||
| 49 | 49 | HTTP API + WebSocket target stream for this arm (run where the follower is) |
| 50 | 50 | leader --to ws://host:8101 [--hz 60] |
| 51 | 51 | read this leader arm and drive a remote follower through its serve |
| 52 | setup [--dry-run] guided: find both arms, reuse calibration, then drive | |
| 52 | 53 | teleop [--leader P --follower P [--save]] [--hz 60] |
| 53 | 54 | the leader arm drives the follower here, space stops it, q quits |
| 54 | 55 | |
| @@ -75,6 +76,7 @@ const OPTIONS = { | ||
| 75 | 76 | hold: { type: "boolean" }, |
| 76 | 77 | listen: { type: "string" }, |
| 77 | 78 | save: { type: "boolean" }, |
| 79 | "dry-run": { type: "boolean" }, | |
| 78 | 80 | leader: { type: "string" }, |
| 79 | 81 | follower: { type: "string" }, |
| 80 | 82 | to: { type: "string" }, |
| @@ -99,6 +101,7 @@ interface Values { | ||
| 99 | 101 | hold?: boolean; |
| 100 | 102 | listen?: string; |
| 101 | 103 | save?: boolean; |
| 104 | "dry-run"?: boolean; | |
| 102 | 105 | leader?: string; |
| 103 | 106 | follower?: string; |
| 104 | 107 | model?: string; |
| @@ -535,6 +538,12 @@ const COMMANDS: Record<string, Command> = { | ||
| 535 | 538 | }); |
| 536 | 539 | }, |
| 537 | 540 | |
| 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 | ||
| 538 | 547 | teleop: async (values) => { |
| 539 | 548 | const { teleop } = await import("./local-teleop-run.ts"); |
| 540 | 549 | 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 | } |
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.