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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

PUT /servo-params writes one joint's P/D/I and dead zones within safe ranges, only while nobody drives, and can put the factory values back #461

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/450-put-servo-params-writes-one-joint-s-p-d → main 3 commits fork 1 thread · 1 unresolved

  • Merged. smith/450-put-servo-params-writes-one-joint-s-p-d was merged into main as abe3d6f.

Review

Sign in to review this change.
7 files changed +256 −7
Software/README.md +25 −1
@@ -913,9 +913,33 @@ The app reaches the Pi through the forge's egress list; 100.89.95.68:8082,
913913 :8083 and :8084 are in `forge.json`'s `egress`. Camera 2 would need
914914 another three ports, and those ports in the egress list too.
915915
916 ## Tuning the servos
917
918 The owner can read and change a joint's control registers on the arm server;
919 anyone else gets a 404 through the relay.
920
921 - `GET /servo-params` lists each servo's delay, P/D/I, startup force, dead
922 zones and velocity loop, and records the first complete reading of the
923 writable ones in `servo-factory.json` next to the settings.
924 - `PUT /servo-params {"id": n, "P"?, "D"?, "I"?, "cwDeadZone"?, "ccwDeadZone"?,
925 "minStartupForce"?}` writes one joint (EEPROM, with the Lock dance) and answers
926 with the read-back. Whole numbers only: P 1-64, D 0-64, I 0-32, dead zones
927 0-8, startup force 0-100. **These ranges are conservative guesses, not from
928 the Feetech datasheet**; widen them in `TUNABLE` (`src/servo-params.ts`) once
929 a tuning pull has shown what is safe. Torque_Limit is not reachable. A joint
930 that held torque gets it back where it rests.
931 - `POST /servo-params/factory {"id": n}` writes back the values recorded at
932 first read.
933 - Both writes are refused, before the bus is read, with 409 while a stream,
934 move, playback or recording drives and 423 during an E-stop or hold. Each
935 appears in the machine log.
936 - The terminal does not offer it: it speaks the room's chat and turns, not the
937 arm server's registers, and a register write should be a deliberate HTTP call
938 by the owner, not a command typed in a shared room.
939
916940 ## Servo factory baseline
917941
918 The first time the relay's link to the arm server is up with nobody holding a turn, it reads `GET /servo-params` once and, if every servo answered, keeps it as `servo-factory.json` (`{ recordedAt, armRun, servos }`) under `FORGE_APP_DATA`; it is never overwritten, and a partial read is retried on the next connect. The owner reads it at `GET /servo-params/factory` (404 for anyone else, or before it is recorded), and #450's factory restore and the tuning pulls read from it.
942 The first time the relay's link to the arm server is up with nobody holding a turn, it reads `GET /servo-params` once and, if every servo answered, keeps it as `servo-factory.json` (`{ recordedAt, armRun, servos }`) under `FORGE_APP_DATA`; it is never overwritten (the arm server keeps its own `servo-factory.json` of the writable registers, which `POST /servo-params/factory` writes back), and a partial read is retried on the next connect. The owner reads it at `GET /servo-params/factory` (404 for anyone else, or before it is recorded), and #450's factory restore and the tuning pulls read from it.
919943
920944 ## Metrics
921945
Software/so101/src/admin-relay.test.ts +9
@@ -159,3 +159,12 @@ test("/servo-params is the owner's; anyone else, signed in or not, gets 404", as
159159 // the owner's request is passed on; nothing listens at the simulation address, so it is not a 404
160160 expect((await fetch(`${base}/servo-params`, { headers: as("trevato", "admin") })).status).not.toBe(404);
161161 });
162
163 test("writing servo params and the factory restore are the owner's; anyone else gets 404", async () => {
164 const base = await boot();
165 for (const [method, path] of [["PUT", "/servo-params"], ["POST", "/servo-params/factory"]] as const) {
166 expect((await fetch(`${base}${path}`, { method, body: "{}" })).status).toBe(404);
167 expect((await fetch(`${base}${path}`, { method, body: "{}", headers: as("bob", "user") })).status).toBe(404);
168 expect((await fetch(`${base}${path}`, { method, body: "{}", headers: as("trevato", "admin") })).status).not.toBe(404);
169 }
170 });
Software/so101/src/fake-bus.ts +3
@@ -27,6 +27,8 @@ export class FakeBus implements Transport {
2727 readonly baudRate = 1_000_000;
2828 readonly servos = new Map<number, Uint8Array>();
2929 readonly writes: Array<{ id: number; address: number; bytes: number[] }> = [];
30 /** Read and sync-read instructions heard, one per packet. */
31 reads = 0;
3032 #listener: ((chunk: Uint8Array) => void) | undefined;
3133 readonly #parser = new FrameParser();
3234 readonly #options: FakeOptions;
@@ -78,6 +80,7 @@ export class FakeBus implements Transport {
7880 // Instruction packets have the status-packet shape, so the parser reads them too.
7981 for (const packet of this.#parser.push(data)) {
8082 const instruction = packet.error;
83 if (instruction === Instruction.Read || instruction === Instruction.SyncRead) this.reads++;
8184 const targets = packet.id === BROADCAST_ID ? [...this.servos.keys()].sort((a, b) => a - b) : [packet.id];
8285 for (const id of targets) {
8386 const memory = this.servos.get(id);
Software/so101/src/relay.ts +3 −3
@@ -163,9 +163,9 @@ const PAGE = /^\/(?:index\.html)?$/;
163163 const STATIC = /^\/(?:index\.html)?$|^\/(?:model|vendor|ui)\//;
164164 const LEADER = "/leader/calibration";
165165 /** Admins only: clearing an emergency stop and changing the settings (the safety limits among them). */
166 const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /settings/table-here"]);
167 /** Reads only the owner may make; anyone else is told there is no such page. */
168 const OWNER_READS = new Set(["GET /servo-params"]);
166 const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /settings/table-here", "PUT /servo-params", "POST /servo-params/factory"]);
167 /** Routes only the owner may reach; anyone else is told there is no such page. */
168 const OWNER_READS = new Set(["GET /servo-params", "PUT /servo-params", "POST /servo-params/factory"]);
169169 /** The driver's, and any admin's at any time. */
170170 const ADMIN_TOO = new Set(["POST /stop", "POST /resume", "POST /estop"]);
171171 /** What still goes to the arm server while the link is reconnecting: stopping it. */
Software/so101/src/server.ts +37 −3
@@ -6,6 +6,8 @@
66 * GET /status every joint: position, load, temperature, torque; what drives the arm; zone state and the table (tableZ)
77 * GET /limits each servo's calibrated Min/Max_Position_Limit
88 * GET /servo-params each servo's control registers (delay, P/D/I, startup force, dead zones, velocity loop), read-only; 409 while something drives
9 * PUT /servo-params {"id": n, "P"?, "D"?, "I"?, "cwDeadZone"?, "ccwDeadZone"?, "minStartupForce"?} write one joint's gains within safe ranges; 409 while anything drives, 423 on E-stop or hold
10 * POST /servo-params/factory {"id": n} write back the values recorded at first read
911 * GET /poses the saved poses
1012 * POST /pose/save {"name": "he1", "ids"?: [1,2,3,4,5]} remember where the joints are now
1113 * POST /pose/delete {"name": "he1"}
@@ -62,7 +64,7 @@ import { dirname, extname, join, resolve } from "node:path";
6264 import type { Duplex } from "node:stream";
6365 import { createHash, randomUUID } from "node:crypto";
6466 import { fileURLToPath } from "node:url";
65 import { readServoParams } from "./servo-params.ts";
67 import { ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts";
6668 import { WebSocketServer, type WebSocket } from "ws";
6769 import { browserModule } from "./browser-modules.ts";
6870 import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts";
@@ -176,6 +178,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
176178 const recordingsDir = join(dataDir, "recordings");
177179 const calibrationDir = join(dataDir, "calibration");
178180 const settingsFile = join(dataDir, "settings.json");
181 const factoryFile = join(dataDir, "servo-factory.json");
179182 const streamTorque = options.streamTorque ?? 70;
180183 const maxStep = options.maxStep ?? 400;
181184 const easeSpeed = options.easeSpeed ?? 300;
@@ -741,9 +744,40 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
741744 return json(res, 200, await status());
742745 case "GET /limits":
743746 return json(res, 200, Object.fromEntries(limits));
744 case "GET /servo-params":
747 case "GET /servo-params": {
745748 if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`);
746 return json(res, 200, await readServoParams(bus, ids));
749 const rows = await readServoParams(bus, ids);
750 recordFactory(factoryFile, rows);
751 return json(res, 200, rows);
752 }
753 case "PUT /servo-params":
754 case "POST /servo-params/factory": {
755 const body = await readBody(req);
756 const restoring = route === "POST /servo-params/factory";
757 // The body is checked and the arm claimed before the bus is touched: a refused request reads nothing.
758 let id: number;
759 let values: Record<string, number> | undefined;
760 try {
761 if (restoring) {
762 if (typeof body.id !== "number" || !ids.includes(body.id)) throw new ParamsError(400, `id must be one of ${ids.join(", ")}`);
763 id = body.id;
764 } else ({ id, values } = validateWrite(body, ids));
765 } catch (err) {
766 if (err instanceof ParamsError) throw new HttpError(err.status, err.message);
767 throw err;
768 }
769 claim("move"); // 423 on an E-stop or hold, 409 while anything drives; nothing else drives meanwhile
770 try {
771 recordFactory(factoryFile, await readServoParams(bus, ids)); // a baseline exists before the first change
772 values ??= factoryValues(factoryFile, id);
773 if (!values) throw new HttpError(409, `no factory values were recorded for servo ${id}`);
774 const row = await writeServoParams(bus, id, values);
775 machine.note("sent", restoring ? "servo params factory" : "servo params", { id, ...values });
776 return json(res, 200, row);
777 } finally {
778 release();
779 }
780 }
747781 case "GET /poses":
748782 return json(res, 200, loadPoses());
749783 case "GET /recordings":
Software/so101/src/servo-params-write.test.ts added +86
@@ -0,0 +1,86 @@
1 import { afterAll, beforeAll, expect, test } from "bun:test";
2 import { mkdtempSync, readFileSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import WebSocket from "ws";
6 import { FeetechBus } from "./bus.ts";
7 import { FakeBus } from "./fake-bus.ts";
8 import { startServer, type RunningServer } from "./server.ts";
9
10 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
11 let fake: FakeBus;
12 let server: RunningServer;
13 let base: string;
14 let dataDir: string;
15 const send = (method: string, path: string, body: unknown) => fetch(`${base}${path}`, { method, body: JSON.stringify(body) });
16 const reg = (id: number, address: number) => fake.servos.get(id)![address];
17
18 beforeAll(async () => {
19 dataDir = mkdtempSync(join(tmpdir(), "so101-params-"));
20 fake = new FakeBus([1, 2]);
21 Object.entries({ 21: 32, 22: 32, 23: 0, 24: 16, 26: 1, 27: 2 }).forEach(([a, v]) => (fake.servos.get(1)![Number(a)] = v));
22 server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1, 2], posesFile: join(dataDir, "poses.json"), watchdogMs: 60 });
23 base = `http://127.0.0.1:${server.port}`;
24 });
25 afterAll(() => Promise.race([server.close(), sleep(500)]));
26
27 test("a write lands, is read back, is logged, and the factory values put it back", async () => {
28 await fetch(`${base}/servo-params`); // first read records the factory values
29 const res = await send("PUT", "/servo-params", { id: 1, P: 20, D: 40, cwDeadZone: 0 });
30 expect(res.status).toBe(200);
31 const row = (await res.json()) as { id: number; registers: Record<string, number> };
32 expect(row.registers).toMatchObject({ P_Coefficient: 20, D_Coefficient: 40, CW_Dead_Zone: 0, CCW_Dead_Zone: 2 });
33 expect([reg(1, 21), reg(1, 22), reg(1, 26)]).toEqual([20, 40, 0]);
34 expect(reg(2, 21)).not.toBe(20);
35 const watcher = new WebSocket(`ws://127.0.0.1:${server.port}/watch`);
36 const texts: string[] = [];
37 watcher.on("message", (m) => texts.push(String(m)));
38 await new Promise((r) => watcher.once("open", r));
39 await sleep(300);
40 watcher.close();
41 expect(texts.find((t) => t.startsWith('{"machine":['))).toContain("servo params");
42 const back = await send("POST", "/servo-params/factory", { id: 1 });
43 expect(back.status).toBe(200);
44 expect([reg(1, 21), reg(1, 22), reg(1, 26)]).toEqual([32, 32, 1]);
45 expect(JSON.parse(readFileSync(join(dataDir, "servo-factory.json"), "utf8"))["1"].P_Coefficient).toBe(32);
46 });
47
48 test("out-of-range, unknown and torque-limit keys are refused with nothing written", async () => {
49 const before = fake.writes.length;
50 const reads = fake.reads;
51 for (const body of [{ id: 1, P: 500 }, { id: 1, P: 1.5 }, { id: 1, D: "9" }, { id: 9, P: 5 }, { id: 1 }, { id: 1, Torque_Limit: 5 }, { id: 1, P: 5, torqueLimit: 5 }]) {
52 expect((await send("PUT", "/servo-params", body)).status).toBe(400);
53 }
54 expect(fake.writes.length).toBe(before);
55 expect(fake.reads).toBe(reads);
56 });
57
58 test("a joint that held torque holds it again, goal at where it rests", async () => {
59 fake.servos.get(2)![40] = 1;
60 fake.servos.get(2)![56] = 0x34;
61 fake.servos.get(2)![57] = 0x08;
62 expect((await send("PUT", "/servo-params", { id: 2, D: 30 })).status).toBe(200);
63 const memory = fake.servos.get(2)!;
64 expect(memory[40]).toBe(1);
65 expect([memory[42], memory[43]]).toEqual([0x34, 0x08]);
66 memory[40] = 0;
67 });
68
69 test("a stream refuses it with 409, an E-stop with 423", async () => {
70 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
71 await new Promise((r) => ws.once("open", r));
72 const reads = fake.reads;
73 expect((await send("PUT", "/servo-params", { id: 1, P: 20 })).status).toBe(409);
74 expect((await send("POST", "/servo-params/factory", { id: 1 })).status).toBe(409);
75 expect(fake.reads).toBe(reads); // refused before the bus is read, mid-drive
76 const closed = new Promise((r) => ws.once("close", r));
77 ws.close();
78 await closed;
79 await sleep(40);
80 expect((await send("POST", "/estop", {})).status).toBe(200);
81 const readsBefore = fake.reads;
82 expect((await send("PUT", "/servo-params", { id: 1, P: 20 })).status).toBe(423);
83 expect((await send("POST", "/servo-params/factory", { id: 1 })).status).toBe(423);
84 expect(fake.reads).toBe(readsBefore);
85 expect(reg(1, 21)).toBe(32);
86 });
Software/so101/src/servo-params.ts +93
@@ -1,3 +1,4 @@
1 import { existsSync, readFileSync, renameSync, writeFileSync } from "node:fs";
12 import type { FeetechBus } from "./bus.ts";
23 import { JOINTS, STS3215 } from "./sts3215.ts";
34
@@ -44,3 +45,95 @@ export async function readServoParams(bus: FeetechBus, ids: readonly number[]):
4445 }
4546 return rows;
4647 }
48
49 /**
50 * What a write may set, by the body's key: the register and its inclusive range. Narrower than the byte: P and D
51 * above these make an SO-101 joint buzz or hunt, a dead zone past a few steps leaves it short of its goal.
52 * Torque_Limit and Max_Torque_Limit are not here, so no body can reach them.
53 */
54 export const TUNABLE = {
55 P: { register: "P_Coefficient", min: 1, max: 64 },
56 D: { register: "D_Coefficient", min: 0, max: 64 },
57 I: { register: "I_Coefficient", min: 0, max: 32 },
58 cwDeadZone: { register: "CW_Dead_Zone", min: 0, max: 8 },
59 ccwDeadZone: { register: "CCW_Dead_Zone", min: 0, max: 8 },
60 minStartupForce: { register: "Minimum_Startup_Force", min: 0, max: 100 },
61 } as const;
62
63 /** A request this module refuses: the server answers it with `status`. */
64 export class ParamsError extends Error {
65 readonly status: number;
66 constructor(status: number, message: string) {
67 super(message);
68 this.status = status;
69 }
70 }
71
72 type Tunable = keyof typeof TUNABLE;
73 type Registers = Record<string, number>;
74
75 /** The writable registers' values as first read, per servo id, in a file next to the settings. */
76 function loadFactory(file: string): Record<string, Registers> {
77 if (!existsSync(file)) return {};
78 try {

loadFactory throws a 500 on a corrupt file. That is right, since it never overwrites the baseline. The message could name the file path so the owner can find it.

79 return JSON.parse(readFileSync(file, "utf8")) as Record<string, Registers>;
80 } catch {
81 throw new ParamsError(500, "the factory values file is unreadable; not overwriting it");
82 }
83 }
84
85 /** Keep the first reading of each servo's tunable registers; later readings never replace it. */
86 export function recordFactory(file: string, rows: readonly ServoParams[]): void {
87 const factory = loadFactory(file);
88 let changed = false;
89 for (const row of rows) {
90 if (factory[row.id]) continue;
91 const kept: Registers = {};
92 for (const { register } of Object.values(TUNABLE)) if (register in row.registers) kept[register] = row.registers[register]!;
93 if (Object.keys(kept).length !== Object.keys(TUNABLE).length) continue; // only a whole reading is a baseline
94 factory[row.id] = kept;
95 changed = true;
96 }
97 if (!changed) return;
98 writeFileSync(`${file}.tmp`, JSON.stringify(factory, null, 1));
99 renameSync(`${file}.tmp`, file);
100 }
101
102 /** Check a PUT body: the servo id (one of `ids`) and at least one tunable value inside its range. */
103 export function validateWrite(body: Record<string, unknown>, ids: readonly number[]): { id: number; values: Registers } {
104 const id = body.id;
105 if (typeof id !== "number" || !ids.includes(id)) throw new ParamsError(400, `id must be one of ${ids.join(", ")}`);
106 const values: Registers = {};
107 for (const key of Object.keys(body)) {
108 if (key !== "id" && !Object.hasOwn(TUNABLE, key)) throw new ParamsError(400, `${key} cannot be written; allowed: ${Object.keys(TUNABLE).join(", ")}`);
109 }
110 for (const [key, { register, min, max }] of Object.entries(TUNABLE)) {
111 const value = body[key];
112 if (value === undefined) continue;
113 if (typeof value !== "number" || !Number.isInteger(value) || value < min || value > max) throw new ParamsError(400, `${key} must be a whole number from ${min} to ${max}`);
114 values[register] = value;
115 }
116 if (!Object.keys(values).length) throw new ParamsError(400, `give at least one of ${Object.keys(TUNABLE).join(", ")}`);
117 return { id, values };
118 }
119
120 /**
121 * Write the values to one servo with the Lock dance (the bus does it for EEPROM), then read the servo back.
122 * The dance switches torque off; a joint that held torque gets it back at the position it rests in.
123 */
124 export async function writeServoParams(bus: FeetechBus, id: number, values: Registers): Promise<ServoParams> {
125 const torque = (await bus.syncRead([id], STS3215.Torque_Enable)).get(id);
126 for (const [name, value] of Object.entries(values)) await bus.write(id, STS3215[name as keyof typeof STS3215] as never, value);
127 if (torque === 1) {
128 const position = (await bus.syncRead([id], STS3215.Present_Position)).get(id);
129 if (position !== undefined) await bus.syncWrite(STS3215.Goal_Position, [[id, position]]);
130 await bus.syncWrite(STS3215.Torque_Enable, [[id, 1]]);
131 }
132 const [row] = await readServoParams(bus, [id]);
133 return row!;
134 }
135
136 /** The values recorded at first read for one servo, or undefined when none were. */
137 export function factoryValues(file: string, id: number): Registers | undefined {
138 return loadFactory(file)[id];
139 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs