Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
The arm server writes settings.servoGains to the servos at start and when a silent servo answers again, only where registers differ #554
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/516-the-arm-server-writes-settings-servogain → main 2 commits fork 3 threads · 3 unresolved
5 files
changed +190
−3
Software/README.md +1 −1
| @@ -819,7 +819,7 @@ conservative. The owner changes them in the settings panel, or with | ||
| 819 | 819 | | `parkPose` | `park` | where the arm goes, slowly, when its driver's connection is lost (a stream that drops or stops answering pings, not one that is closed); with no pose of that name, the middle of every arm joint's range | |
| 820 | 820 | | `parkSpeedDegS` | 20 | how fast the park move goes | |
| 821 | 821 | | `gripperTorquePercent` | 35 | the gripper's force limit on the stream, `/grip`, moves and playbacks, and while it holds | |
| 822 | | `servoGains` | joints 1, 4, 5: P64 I0 D32; joints 2, 3: P64 I4 D32 | the measured P/I/D per joint id (`"1"`-`"5"`), integers 0-254; a patch keeps the joints and gains it does not name; the gripper (6) is refused, it stays at P16 and torque-capped; held and validated only: nothing writes it to a servo yet, and the settings page and the terminal have no control for it on purpose; a `settings.json` without it loads the defaults | | |
| 822 | | `servoGains` | joints 1, 4, 5: P64 I0 D32; joints 2, 3: P64 I4 D32 | the measured P/I/D per joint id (`"1"`-`"5"`), integers 0-254; a patch keeps the joints and gains it does not name; the gripper (6) is refused, it stays at P16 and torque-capped; written to the servos at start and when a silent servo answers again, only the registers that differ and only while the arm is free; a `PUT` waits for the next start or swap; and the settings page and the terminal have no control for it on purpose; a `settings.json` without it loads the defaults | | |
| 823 | 823 | |
| 824 | 824 | **The table plane.** The zone's table starts at the bottom of the model's |
| 825 | 825 | base, which is rarely where the real table is. `tableOffsetMm` (-100 to 100, |
Software/so101/src/cli.ts +1 −1
| @@ -498,7 +498,7 @@ const COMMANDS: Record<string, Command> = { | ||
| 498 | 498 | const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":"); |
| 499 | 499 | const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port"); |
| 500 | 500 | return withBus(values, async (bus) => { |
| 501 | const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model }); | |
| 501 | const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model, gainSyncMs: 2000 }); | |
| 502 | 502 | console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`); |
| 503 | 503 | console.log("ctrl-c to stop"); |
| 504 | 504 | await new Promise<void>((resolve) => process.once("SIGINT", () => resolve())); |
Software/so101/src/server.ts +17 −1
| @@ -74,6 +74,7 @@ import { dirname, extname, join, resolve } from "node:path"; | ||
| 74 | 74 | import type { Duplex } from "node:stream"; |
| 75 | 75 | import { createHash, randomUUID } from "node:crypto"; |
| 76 | 76 | import { fileURLToPath } from "node:url"; |
| 77 | import { GainSync } from "./servo-gain-sync.ts"; | |
| 77 | 78 | import { TUNABLE, ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts"; |
| 78 | 79 | import { WebSocketServer, type WebSocket } from "ws"; |
| 79 | 80 | import { browserModule } from "./browser-modules.ts"; |
| @@ -119,6 +120,8 @@ export interface ServerOptions { | ||
| 119 | 120 | heartbeatMs?: number; |
| 120 | 121 | /** The commit the precision history tags its rows with; found from the environment or the checkout when not given. */ |
| 121 | 122 | commit?: string; |
| 123 | /** How often the arm is checked for servos whose gains need writing, ms; unset, the gains are not written (the CLI sets 2000). */ | |
| 124 | gainSyncMs?: number; | |
| 122 | 125 | /** How long each sweep target is held, ms (1500); tests shorten it. */ |
| 123 | 126 | tuningHoldMs?: number; |
| 124 | 127 | /** Replaces the stream sweep of the P experiment; tests give it the numbers to decide on. */ |
| @@ -150,7 +153,7 @@ export interface Settings extends SafetySettings { | ||
| 150 | 153 | tableOffsetMm: number; |
| 151 | 154 | /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */ |
| 152 | 155 | jointOffsetsDeg: Record<string, number>; |
| 153 | /** Measured P/I/D per joint id (1-5); held and validated here, not yet written to a servo. */ | |
| 156 | /** Measured P/I/D per joint id (1-5); written to the servos at start and when a silent servo answers again, not on PUT. */ | |
| 154 | 157 | servoGains: ServoGains; |
| 155 | 158 | } |
| 156 | 159 | |
| @@ -368,6 +371,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 368 | 371 | } |
| 369 | 372 | }, |
| 370 | 373 | }; |
| 374 | const gainSync = new GainSync({ | |
| 375 | bus, | |
| 376 | ids, | |
| 377 | gains: () => settings.servoGains, | |
| 378 | free: () => !driver && !estop && !gate.held, | |
| 379 | claim: () => void claim("move"), | |
| 380 | release, | |
| 381 | baseline: (rows) => recordFactory(factoryFile, rows), | |
| 382 | wrote: (id, values) => machine.note("sent", "servo gains", { id, ...values }), | |
| 383 | }); | |
| 371 | 384 | const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d]; |
| 372 | 385 | |
| 373 | 386 | // ------------------------------------------------------------------ the safety envelope (src/safety.ts) |
| @@ -1663,6 +1676,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1663 | 1676 | }); |
| 1664 | 1677 | |
| 1665 | 1678 | await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve)); |
| 1679 | const gainTimer = options.gainSyncMs ? setInterval(() => void gainSync.tick(), options.gainSyncMs) : undefined; | |
| 1680 | if (gainTimer) void gainSync.tick(); | |
| 1666 | 1681 | const address = http.address(); |
| 1667 | 1682 | const port = typeof address === "object" && address ? address.port : options.port ?? 8101; |
| 1668 | 1683 | return { |
| @@ -1670,6 +1685,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1670 | 1685 | close: () => |
| 1671 | 1686 | new Promise<void>((resolve) => { |
| 1672 | 1687 | clearInterval(watching); |
| 1688 | clearInterval(gainTimer); | |
| 1673 | 1689 | machine.close(); |
| 1674 | 1690 | precision.close(); |
| 1675 | 1691 | bus.onSyncWrite = null; |
Software/so101/src/servo-gain-sync.test.ts added +84
| @@ -0,0 +1,84 @@ | ||
| 1 | import { afterEach, 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 { FeetechBus } from "./bus.ts"; | |
| 6 | import { FakeBus } from "./fake-bus.ts"; | |
| 7 | import { startServer, type RunningServer } from "./server.ts"; | |
| 8 | ||
| 9 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 10 | const running: RunningServer[] = []; | |
| 11 | afterEach(async () => { | |
| 12 | for (const s of running.splice(0)) await Promise.race([s.close(), sleep(500)]); | |
| 13 | }); | |
| 14 | ||
| 15 | const gainsOf = (fake: FakeBus, id: number) => [fake.servos.get(id)![21], fake.servos.get(id)![23], fake.servos.get(id)![22]]; | |
| 16 | const gainWrites = (fake: FakeBus) => fake.writes.filter((w) => w.address >= 21 && w.address <= 23); | |
| 17 | const boot = async (fake: FakeBus, dir: string) => { | |
| 18 | const server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [...fake.servos.keys()], posesFile: join(dir, "poses.json"), gainSyncMs: 40, watchdogMs: 60 }); | |
| 19 | running.push(server); | |
| 20 | return server; | |
| 21 | }; | |
| 22 | const drop = new Set<number>(); | |
| 23 | const factory = (ids = [1, 2]) => { | |
| 24 | drop.clear(); | |
| 25 | const fake = new FakeBus(ids, { drop }); | |
| 26 | for (const id of ids) Object.entries({ 21: 16, 22: 32, 23: 0 }).forEach(([a, v]) => (fake.servos.get(id)![Number(a)] = v)); | |
| 27 | return fake; | |
| 28 | }; | |
| 29 | ||
| 30 | test("start writes the gains that differ, a restart writes nothing, and GET /servo-params shows the live values", async () => { | |
| 31 | const dir = mkdtempSync(join(tmpdir(), "so101-gains-")); | |
| 32 | const fake = factory(); | |
| 33 | const first = await boot(fake, dir); | |
| 34 | await sleep(300); | |
| 35 | expect(gainsOf(fake, 1)).toEqual([64, 0, 32]); // P, I, D of joint 1 | |
| 36 | expect(gainsOf(fake, 2)).toEqual([64, 4, 32]); | |
| 37 | expect(gainWrites(fake).some((w) => w.id === 1 && w.address === 23)).toBe(false); // I already 0: not written | |
| 38 | expect(JSON.parse(await Bun.file(join(dir, "servo-factory.json")).text())["1"].P_Coefficient).toBe(16); | |
| 39 | const rows = (await (await fetch(`http://127.0.0.1:${first.port}/servo-params`)).json()) as Array<{ id: number; registers: Record<string, number> }>; | |
| 40 | expect(rows.find((r) => r.id === 2)!.registers).toMatchObject({ P_Coefficient: 64, I_Coefficient: 4, D_Coefficient: 32 }); | |
| 41 | await first.close(); | |
| 42 | ||
| 43 | const before = gainWrites(fake).length; | |
| 44 | await boot(fake, dir); | |
| 45 | await sleep(300); | |
| 46 | expect(gainWrites(fake).length).toBe(before); | |
| 47 | }); | |
| 48 | ||
| 49 | test("a servo that goes silent and answers again at P16 is rewritten, but not while the arm is held", async () => { | |
| 50 | const dir = mkdtempSync(join(tmpdir(), "so101-gains-")); | |
| 51 | const fake = factory(); | |
| 52 | const server = await boot(fake, dir); | |
| 53 | const url = `http://127.0.0.1:${server.port}`; | |
| 54 | await sleep(300); | |
| 55 | drop.add(2); | |
| 56 | await sleep(150); // seen silent | |
| 57 | fake.servos.get(2)![21] = 16; // replaced | |
| 58 | await fetch(`${url}/stop`, { method: "POST", body: "{}" }); // hold | |
| 59 | drop.delete(2); | |
| 60 | const goal = fake.servos.get(2)!.slice(42, 44); | |
| 61 | await sleep(300); | |
| 62 | expect(fake.servos.get(2)![21]).toBe(16); // held: nothing written | |
| 63 | await fetch(`${url}/resume`, { method: "POST", body: "{}" }); | |
| 64 | await sleep(300); | |
| 65 | expect(fake.servos.get(2)![21]).toBe(64); | |
| 66 | expect(fake.servos.get(2)!.slice(42, 44)).toEqual(goal); // no joint moved | |
| 67 | }); | |
| 68 | ||
| 69 | test("one silent joint does not stop another from being written", async () => { | |
| 70 | const dir = mkdtempSync(join(tmpdir(), "so101-gains-")); | |
| 71 | const fake = factory([1, 2, 3]); | |
| 72 | await boot(fake, dir); | |
| 73 | await sleep(300); | |
| 74 | drop.add(2).add(3); | |
| 75 | await sleep(150); // both seen silent | |
| 76 | for (const id of [2, 3]) fake.servos.get(id)![21] = 16; // both replaced | |
| 77 | drop.delete(2); | |
| 78 | await sleep(300); | |
| 79 | expect(gainsOf(fake, 2)).toEqual([64, 4, 32]); | |
| 80 | expect(fake.servos.get(3)![21]).toBe(16); // still silent: untouched, still pending | |
| 81 | drop.delete(3); | |
| 82 | await sleep(300); | |
| 83 | expect(gainsOf(fake, 3)).toEqual([64, 4, 32]); | |
| 84 | }); |
Software/so101/src/servo-gain-sync.ts added +87
| @@ -0,0 +1,87 @@ | ||
| 1 | /** | |
| 2 | * Puts settings.servoGains on the servos: at start, and when a servo that stopped answering answers again (a replaced | |
| 3 | * servo ships at P16). Per joint it reads regs 21/22/23 (P/D/I) and writes only the ones that differ, so a plain | |
| 4 | * restart costs no EEPROM writes. It only acts while the arm is free, and tries again on the next tick otherwise. | |
| 5 | */ | |
| 6 | import type { FeetechBus } from "./bus.ts"; | |
| 7 | import { GAIN_JOINTS, type ServoGains } from "./servo-gains.ts"; | |
| 8 | import { readServoParams, writeServoParams } from "./servo-params.ts"; | |
| 9 | import { STS3215 } from "./sts3215.ts"; | |
| 10 | ||
| 11 | export interface GainSyncDeps { | |
| 12 | bus: FeetechBus; | |
| 13 | ids: readonly number[]; | |
| 14 | gains: () => ServoGains; | |
| 15 | /** True when nothing drives and the arm is neither stopped nor held. */ | |
| 16 | free: () => boolean; | |
| 17 | /** Take the arm for the writes (throws when it is not free) and give it back. */ | |
| 18 | claim: () => void; | |
| 19 | release: () => void; | |
| 20 | /** Called with a whole read before the first change, so the factory values are recorded. */ | |
| 21 | baseline: (rows: Awaited<ReturnType<typeof readServoParams>>) => void; | |
| 22 | /** Called once per servo written, with what changed. */ | |
| 23 | wrote: (id: number, values: Record<string, number>) => void; | |
| 24 | } | |
| 25 | ||
| 26 | const REGISTERS = [ | |
| 27 | ["p", "P_Coefficient"], | |
| 28 | ["d", "D_Coefficient"], | |
| 29 | ["i", "I_Coefficient"], | |
| 30 | ] as const; | |
| 31 | ||
| 32 | export class GainSync { | |
| 33 | /** Joints not yet known to hold the settings: all at start, then each that goes silent. */ | |
| 34 | readonly : Set<number>; | |
| 35 | = false; | |
| 36 | ||
| 37 | constructor(private readonly deps: GainSyncDeps) { | |
| 38 | this. = new Set(deps.ids.filter((id) => (GAIN_JOINTS as readonly string[]).includes(String(id)))); | |
| 39 | } | |
| 40 | ||
| 41 | /** One look at the bus: note who is silent, and write the gains of a pending joint that answers. */ | |
| 42 | async tick(): Promise<void> { | |
| 43 | const { bus, free } = this.deps; | |
| 44 | if (this. || !free()) return; | |
| 45 | this. = true; | |
| 46 | try { | |
| 47 | const joints = this.deps.ids.filter((id) => (GAIN_JOINTS as readonly string[]).includes(String(id))); | |
outdated
Redundant: pending is always a subset of ids, so this is just the gain joints in ids. |
||
| 48 | if (!joints.length) return; | |
| 49 | let answers: Map<number, number>; | |
| 50 | try { | |
| 51 | answers = await bus.syncRead(joints, STS3215.P_Coefficient); | |
| 52 | } catch { | |
| 53 | answers = new Map(); | |
| 54 | } | |
| 55 | for (const id of joints) if (!answers.has(id)) this..add(id); | |
| 56 | const ready = [...this.].filter((id) => answers.has(id)); | |
| 57 | if (!ready.length) return; | |
| 58 | try { | |
| 59 | this.deps.claim(); | |
| 60 | } catch { | |
| 61 | return; // the arm is not free; the next tick tries again | |
| 62 | } | |
| 63 | try { | |
| 64 | this.deps.baseline(await readServoParams(bus, ready)); // only servos that answered: a silent one blocks no other | |
outdated
readServoParams over all ids throws if any servo is silent, which aborts the writes for the joints that are ready. Use ready ids only. |
||
| 65 | const gains = this.deps.gains(); | |
| 66 | for (const id of ready) { | |
| 67 | const want = gains[String(id)]!; | |
| 68 | const now = await readServoParams(bus, [id]).then((r) => r[0]!.registers); | |
| 69 | const values: Record<string, number> = {}; | |
| 70 | for (const [key, register] of REGISTERS) if (now[register] !== undefined && now[register] !== want[key]) values[register] = want[key]; | |
| 71 | if (Object.keys(values).length) { | |
| 72 | const row = await writeServoParams(bus, id, values); | |
| 73 | if (Object.entries(values).some(([register, v]) => row.registers[register] !== v)) continue; // did not stick: try again | |
| 74 | this.deps.wrote(id, values); | |
| 75 | } | |
| 76 | this..delete(id); | |
| 77 | } | |
| 78 | } catch { | |
| 79 | // a servo went quiet mid-way: it stays pending | |
| 80 | } finally { | |
| 81 | this.deps.release(); | |
| 82 | } | |
| 83 | } finally { | |
| 84 | this. = false; | |
| 85 | } | |
| 86 | } | |
| 87 | } |
new above
offline
Minor: the sync claims the arm as "move". While it writes, a stream that connects is refused with "a move is driving the arm", and the machine log notes
driving: move, which is untrue. A dedicated driver kind (e.g. "gains" → "the gain write") would make that refusal and the log accurate. Fine to leave for later.