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

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/516-the-arm-server-writes-settings-servogain → main 2 commits fork 3 threads · 3 unresolved

  • Merged. smith/516-the-arm-server-writes-settings-servogain was merged into main as eec819f.

Review

Sign in to review this change.
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
819819 | `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 |
820820 | `parkSpeedDegS` | 20 | how fast the park move goes |
821821 | `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 |
823823
824824 **The table plane.** The zone's table starts at the bottom of the model's
825825 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> = {
498498 const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":");
499499 const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port");
500500 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 });
502502 console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`);
503503 console.log("ctrl-c to stop");
504504 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";
7474 import type { Duplex } from "node:stream";
7575 import { createHash, randomUUID } from "node:crypto";
7676 import { fileURLToPath } from "node:url";
77 import { GainSync } from "./servo-gain-sync.ts";
7778 import { TUNABLE, ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts";
7879 import { WebSocketServer, type WebSocket } from "ws";
7980 import { browserModule } from "./browser-modules.ts";
@@ -119,6 +120,8 @@ export interface ServerOptions {
119120 heartbeatMs?: number;
120121 /** The commit the precision history tags its rows with; found from the environment or the checkout when not given. */
121122 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;
122125 /** How long each sweep target is held, ms (1500); tests shorten it. */
123126 tuningHoldMs?: number;
124127 /** 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 {
150153 tableOffsetMm: number;
151154 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
152155 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. */
154157 servoGains: ServoGains;
155158 }
156159
@@ -368,6 +371,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
368371 }
369372 },
370373 };
374 const gainSync = new GainSync({
375 bus,
376 ids,
377 gains: () => settings.servoGains,
378 free: () => !driver && !estop && !gate.held,
379 claim: () => void claim("move"),

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.

380 release,
381 baseline: (rows) => recordFactory(factoryFile, rows),
382 wrote: (id, values) => machine.note("sent", "servo gains", { id, ...values }),
383 });
371384 const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d];
372385
373386 // ------------------------------------------------------------------ the safety envelope (src/safety.ts)
@@ -1663,6 +1676,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
16631676 });
16641677
16651678 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();
16661681 const address = http.address();
16671682 const port = typeof address === "object" && address ? address.port : options.port ?? 8101;
16681683 return {
@@ -1670,6 +1685,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
16701685 close: () =>
16711686 new Promise<void>((resolve) => {
16721687 clearInterval(watching);
1688 clearInterval(gainTimer);
16731689 machine.close();
16741690 precision.close();
16751691 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 #pending: Set<number>;
35 #busy = false;
36
37 constructor(private readonly deps: GainSyncDeps) {
38 this.#pending = 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.#busy || !free()) return;
45 this.#busy = 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.#pending.add(id);
56 const ready = [...this.#pending].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.#pending.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.#busy = false;
85 }
86 }
87 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs