Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Precision, forever: calibrate the model to the arm, tune the servos against the scorecard, report weekly #507
smith/arm:smith/439-precision-forever-calibrate-the-model-to → main 10 commits fork
9 files
changed +368
−9
Software/README.md +14
| @@ -1024,6 +1024,20 @@ anyone else gets a 404 through the relay. | ||
| 1024 | 1024 | that held torque gets it back where it rests. |
| 1025 | 1025 | - `POST /servo-params/factory {"id": n}` writes back the values recorded at |
| 1026 | 1026 | first read. |
| 1027 | - The arm server owns the gains. At start, and when a servo that stopped | |
| 1028 | answering answers again (a swap), it writes `settings.servoGains` to each | |
| 1029 | servo that lacks them, only while nobody drives. The defaults are what the | |
| 1030 | steward measured on the real arm, 2026-10-21: the shipped P16 I0 settled 24-27 | |
| 1031 | ticks (about 2.4°) short and never arrived; P64 arrives in 150-330 ms with at | |
| 1032 | most 3 ticks of overshoot (shoulder and elbow also I4). The gripper stays at | |
| 1033 | the factory P16, torque-capped on purpose. The factory file is written | |
| 1034 | before the first gain, so `POST /servo-params/factory` still restores P16; | |
| 1035 | the relay's `servo-factory.json` is read after the start-up write and may | |
| 1036 | already show P64. | |
| 1037 | - The page keeps a released slider's intent for 1 s (three times the slowest | |
| 1038 | settle), arrival within 12 ticks. `bun bench/real-probe.ts <arm server> | |
| 1039 | --joint 2 [--step 60]` times one small step on the real arm, refuses while a | |
| 1040 | turn is held, and always moves the joint back to where it started. | |
| 1027 | 1041 | - Both writes are refused, before the bus is read, with 409 while a stream, |
| 1028 | 1042 | move, playback or recording drives and 423 during an E-stop or hold. Each |
| 1029 | 1043 | appears in the machine log. |
Software/so101/bench/real-probe.test.ts added +26
| @@ -0,0 +1,26 @@ | ||
| 1 | import { expect, test } from "bun:test"; | |
| 2 | import { plan, summarize } from "./real-probe.ts"; | |
| 3 | ||
| 4 | const run = (positions: number[]) => positions.map((position, i) => ({ t: i * 50, position })); | |
| 5 | ||
| 6 | test("arrival is the start of the last stay within the tolerance, overshoot is past the target", () => { | |
| 7 | const r = summarize(run([2048, 2080, 2112, 2125, 2130, 2128]), 2048, 2120); | |
| 8 | expect(r).toEqual({ arriveMs: 100, finalError: -8, overshoot: 10 }); | |
| 9 | }); | |
| 10 | ||
| 11 | test("a joint that settles short never arrives", () => { | |
| 12 | const r = summarize(run([2048, 2060, 2070, 2080, 2084]), 2048, 2108); | |
| 13 | expect(r).toEqual({ arriveMs: null, finalError: 24, overshoot: 0 }); | |
| 14 | }); | |
| 15 | ||
| 16 | test("a downward step reads the same way", () => { | |
| 17 | expect(summarize(run([2048, 2000, 1990]), 2048, 1988)).toEqual({ arriveMs: 50, finalError: 2, overshoot: 0 }); | |
| 18 | }); | |
| 19 | ||
| 20 | test("a step is small and stays inside the range, turning round when there is no room", () => { | |
| 21 | expect(plan(2000, 60, 800, 3200)).toEqual({ to: 2060 }); | |
| 22 | expect(plan(3180, 60, 800, 3200)).toEqual({ to: 3120 }); | |
| 23 | expect(() => plan(2000, 0, 800, 3200)).toThrow(); | |
| 24 | expect(() => plan(2000, 151, 800, 3200)).toThrow(); | |
| 25 | expect(() => plan(2000, 60, 1990, 2010)).toThrow("no room"); | |
| 26 | }); |
Software/so101/bench/real-probe.ts added +143
| @@ -0,0 +1,143 @@ | ||
| 1 | /** | |
| 2 | * One joint of the real arm, one small step, timed: how long it takes to arrive, where it settles, how far it overshoots. | |
| 3 | * bun bench/real-probe.ts <http://arm-server:8101> --joint 2 [--step 60] [--settle 1500] | |
| 4 | * It refuses while anyone drives, a turn is held, or the arm is stopped. It puts the joint back where it started | |
| 5 | * after every run, however the run ended: repeated one-way steps walk a gravity joint into its limit (the elbow walked | |
| 6 | * 300 ticks to its clamp), so the start pose is restored and checked, and a run that cannot restore it says so loudly. | |
| 7 | */ | |
| 8 | ||
| 9 | /** Arrival tolerance in ticks: the page's intent hold uses the same (src/ui/intent.js). */ | |
| 10 | export const NEAR = 12; | |
| 11 | /** The largest step a probe may take, ticks (about 13 degrees). */ | |
| 12 | export const MAX_STEP = 150; | |
| 13 | ||
| 14 | export interface Sample { | |
| 15 | /** Milliseconds from the move's start. */ | |
| 16 | t: number; | |
| 17 | position: number; | |
| 18 | } | |
| 19 | ||
| 20 | export interface Result { | |
| 21 | /** First time within NEAR of the target and staying there, ms; null when it never did. */ | |
| 22 | arriveMs: number | null; | |
| 23 | /** Ticks short of (or past) the target at the last sample, signed in the direction of travel: positive is short. */ | |
| 24 | finalError: number; | |
| 25 | /** Most ticks past the target in the direction of travel; 0 when none. */ | |
| 26 | overshoot: number; | |
| 27 | } | |
| 28 | ||
| 29 | /** The numbers of one step from `from` to `to`. Pure: the samples are the only input. */ | |
| 30 | export function summarize(samples: readonly Sample[], from: number, to: number, near = NEAR): Result { | |
| 31 | const dir = Math.sign(to - from) || 1; | |
| 32 | const short = (p: number) => (to - p) * dir; | |
| 33 | let arriveMs: number | null = null; | |
| 34 | let overshoot = 0; | |
| 35 | for (const s of samples) { | |
| 36 | if (Math.abs(to - s.position) <= near) arriveMs ??= s.t; | |
| 37 | else arriveMs = null; // it left again: arrival is the start of the last stay | |
| 38 | overshoot = Math.max(overshoot, -short(s.position)); | |
| 39 | } | |
| 40 | const last = samples.at(-1); | |
| 41 | return { arriveMs, finalError: last ? short(last.position) : Math.abs(to - from), overshoot }; | |
| 42 | } | |
| 43 | ||
| 44 | /** The start and target for a step, kept inside the joint's calibrated range; a step that has no room either way is refused. */ | |
| 45 | export function plan(start: number, step: number, min: number, max: number): { to: number } { | |
| 46 | if (!Number.isInteger(step) || step === 0 || Math.abs(step) > MAX_STEP) throw new Error(`step must be a whole number of ticks, not 0, at most ${MAX_STEP} either way`); | |
| 47 | const to = start + step; | |
| 48 | if (to >= min && to <= max) return { to }; | |
| 49 | const back = start - step; | |
| 50 | if (back >= min && back <= max) return { to: back }; | |
| 51 | throw new Error(`no room for a ${Math.abs(step)}-tick step at ${start} (range ${min}-${max})`); | |
| 52 | } | |
| 53 | ||
| 54 | interface Joint { | |
| 55 | id: number; | |
| 56 | position: number | null; | |
| 57 | lo: number | null; | |
| 58 | hi: number | null; | |
| 59 | } | |
| 60 | interface Status { | |
| 61 | joints: Joint[]; | |
| 62 | busy: boolean; | |
| 63 | driving: string | null; | |
| 64 | estop: unknown; | |
| 65 | held: unknown; | |
| 66 | } | |
| 67 | ||
| 68 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 69 | ||
| 70 | /** Run one probe against the arm server at `base`; the joint is back at its start when this returns or throws (unless the arm stopped it). */ | |
| 71 | export async function probe(base: string, id: number, step: number, settleMs: number, now = () => performance.now()): Promise<Result & { id: number; start: number; to: number; returned: boolean }> { | |
| 72 | const get = async (): Promise<Status> => (await fetch(`${base}/status`)).json() as Promise<Status>; | |
| 73 | const move = async (to: number) => { | |
| 74 | const res = await fetch(`${base}/move`, { method: "POST", body: JSON.stringify({ targets: { [id]: to } }) }); | |
| 75 | if (!res.ok) throw new Error(`move refused (${res.status}): ${((await res.json().catch(() => ({}))) as { error?: string }).error ?? ""}`); | |
| 76 | }; | |
| 77 | const first = await get(); | |
| 78 | if (first.estop) throw new Error("refused: the arm is stopped"); | |
| 79 | if (first.held) throw new Error("refused: the arm is held; resume first"); | |
| 80 | if (first.busy) throw new Error(`refused: ${first.driving ?? "something"} is driving the arm`); | |
| 81 | const joint = first.joints.find((j) => j.id === id); | |
| 82 | if (!joint || joint.position === null || joint.lo === null || joint.hi === null) throw new Error(`servo ${id} is not answering`); | |
| 83 | const start = joint.position; | |
| 84 | const { to } = plan(start, step, joint.lo, joint.hi); | |
| 85 | ||
| 86 | const samples: Sample[] = []; | |
| 87 | const t0 = now(); | |
| 88 | let sampling = true; | |
| 89 | const sampler = (async () => { | |
| 90 | while (sampling) { | |
| 91 | const p = (await get().catch(() => null))?.joints.find((j) => j.id === id)?.position; | |
| 92 | if (typeof p === "number") samples.push({ t: Math.round(now() - t0), position: p }); | |
| 93 | await sleep(20); | |
| 94 | } | |
| 95 | })(); | |
| 96 | let returned = false; | |
| 97 | try { | |
| 98 | await move(to); | |
| 99 | await sleep(settleMs); | |
| 100 | } finally { | |
| 101 | sampling = false; | |
| 102 | await sampler; | |
| 103 | // Always back to the start, then check it got there; a failure here is the one to read first. | |
| 104 | try { | |
| 105 | await move(start); | |
| 106 | await sleep(settleMs); | |
| 107 | const back = (await get()).joints.find((j) => j.id === id)?.position; | |
| 108 | returned = typeof back === "number" && Math.abs(back - start) <= NEAR; | |
| 109 | } catch (err) { | |
| 110 | console.error(`COULD NOT RETURN servo ${id} to ${start}: ${err instanceof Error ? err.message : err}`); | |
| 111 | } | |
| 112 | } | |
| 113 | return { id, start, to, returned, ...summarize(samples, start, to) }; | |
| 114 | } | |
| 115 | ||
| 116 | if (import.meta.main) { | |
| 117 | const [base, ...rest] = process.argv.slice(2); | |
| 118 | const o = { joint: 0, step: 60, settle: 1500 }; | |
| 119 | try { | |
| 120 | if (!base || !/^https?:\/\/[^/\s]+$/.test(base)) throw new Error("first argument is the arm server, http://host:port"); | |
| 121 | while (rest.length) { | |
| 122 | const k = rest.shift(); | |
| 123 | const v = rest.shift() ?? ""; | |
| 124 | if (!/^-?\d{1,5}$/.test(v)) throw new Error(`bad value for ${k}`); | |
| 125 | if (k === "--joint") o.joint = Number(v); | |
| 126 | else if (k === "--step") o.step = Number(v); | |
| 127 | else if (k === "--settle") o.settle = Number(v); | |
| 128 | else throw new Error(`bad argument ${k}`); | |
| 129 | } | |
| 130 | if (o.joint < 1 || o.joint > 6 || o.settle < 300 || o.settle > 10000) throw new Error("--joint is 1-6, --settle 300-10000 ms"); | |
| 131 | } catch (err) { | |
| 132 | console.error(`${err instanceof Error ? err.message : err}\nusage: bun bench/real-probe.ts <http://arm-server:8101> --joint 1-6 [--step 60] [--settle 1500]`); | |
| 133 | process.exit(2); | |
| 134 | } | |
| 135 | try { | |
| 136 | const r = await probe(base, o.joint, o.step, o.settle); | |
| 137 | console.log(JSON.stringify(r)); | |
| 138 | if (!r.returned) process.exit(1); | |
| 139 | } catch (err) { | |
| 140 | console.error(err instanceof Error ? err.message : err); | |
| 141 | process.exit(1); | |
| 142 | } | |
| 143 | } |
Software/so101/src/machine-feed.test.ts +1 −1
| @@ -107,7 +107,7 @@ test("a clamp shows up in the log the moment it is applied, with the command tha | ||
| 107 | 107 | expect(goal.seq).toBeGreaterThan(clamp.seq); |
| 108 | 108 | expect(p.log.some((e) => e.what === "Goal_Position" && ((e.detail.values as Record<string, number>)["1"] ?? 0) > 3457)).toBe(false); |
| 109 | 109 | await p.until(() => p.messages.find((m) => m.type === "status" && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) > 2200 && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) <= 3457), "the goal in the joints, stepping toward 3457"); |
| 110 | expect(p.log.find((e) => e.kind === "state" && e.what === "driver")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) }); | |
| 110 | expect(p.log.find((e) => e.kind === "state" && e.what === "driver" && e.detail.driving === "stream")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) }); | |
| 111 | 111 | follow.close(); |
| 112 | 112 | p.ws.close(); |
| 113 | 113 | }); |
Software/so101/src/server.ts +45 −4
| @@ -70,6 +70,7 @@ import { dirname, extname, join, resolve } from "node:path"; | ||
| 70 | 70 | import type { Duplex } from "node:stream"; |
| 71 | 71 | import { createHash, randomUUID } from "node:crypto"; |
| 72 | 72 | import { fileURLToPath } from "node:url"; |
| 73 | import { DEFAULT_GAINS, applyGains, validateGains, type Gains } from "./servo-gains.ts"; | |
| 73 | 74 | import { ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts"; |
| 74 | 75 | import { WebSocketServer, type WebSocket } from "ws"; |
| 75 | 76 | import { browserModule } from "./browser-modules.ts"; |
| @@ -136,9 +137,11 @@ export interface Settings extends SafetySettings { | ||
| 136 | 137 | tableOffsetMm: number; |
| 137 | 138 | /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */ |
| 138 | 139 | jointOffsetsDeg: Record<string, number>; |
| 140 | /** Per joint, the P/I/D the arm server writes to the servo at start and when a servo comes back; the factory ones are kept beside the settings. */ | |
| 141 | servoGains: Record<string, Gains>; | |
| 139 | 142 | } |
| 140 | 143 | |
| 141 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 144 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, servoGains: { ...DEFAULT_GAINS }, ...DEFAULT_SAFETY }; | |
| 142 | 145 | |
| 143 | 146 | /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */ |
| 144 | 147 | export interface EStop { |
| @@ -210,11 +213,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 210 | 213 | const machine = new MachineLog(); |
| 211 | 214 | const run = randomUUID(); |
| 212 | 215 | const goals = new Map<number, number>(); |
| 216 | let tuning = false; | |
| 213 | 217 | /** Torque_Enable sits apart from the present block, so it is read at most every TORQUE_EVERY_MS, and at once after it is written. */ |
| 214 | 218 | let torque = new Map<number, number>(); |
| 215 | 219 | let torqueAt = -Infinity; |
| 216 | 220 | bus.onSyncWrite = (register, values) => { |
| 217 | if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v), tracking?.wrote(id, Date.now()); | |
| 221 | // Holding a joint where it rests while its gains are written is not a command: it is no target the page should show. | |
| 222 | if (register === STS3215.Goal_Position && !tuning) for (const [id, v] of values) goals.set(id, v), tracking?.wrote(id, Date.now()); | |
| 218 | 223 | if (register === STS3215.Torque_Enable) torqueAt = -Infinity; |
| 219 | 224 | machine.note("sent", registerName(register), { values: perServo(values) }); |
| 220 | 225 | }; |
| @@ -552,6 +557,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 552 | 557 | } |
| 553 | 558 | return [...new Set(value as number[])]; |
| 554 | 559 | }; |
| 560 | ||
| 555 | 561 | function validateSettings(s: Partial<Settings>): Settings { |
| 556 | 562 | let safety: SafetySettings; |
| 557 | 563 | try { |
| @@ -561,6 +567,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 561 | 567 | throw err; |
| 562 | 568 | } |
| 563 | 569 | const out: Settings = { ...DEFAULT_SETTINGS, ...safety, jointOffsetsDeg: {} }; |
| 570 | if (s.servoGains !== undefined) { | |
| 571 | try { | |
| 572 | out.servoGains = validateGains(s.servoGains); | |
| 573 | } catch (err) { | |
| 574 | throw new HttpError(400, err instanceof Error ? err.message : String(err)); | |
| 575 | } | |
| 576 | } | |
| 564 | 577 | if (s.zone !== undefined) { |
| 565 | 578 | if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"'); |
| 566 | 579 | out.zone = s.zone; |
| @@ -585,6 +598,31 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 585 | 598 | return out; |
| 586 | 599 | } |
| 587 | 600 | |
| 601 | /** Servos whose gains are not known to be settings.servoGains: all at start, and any that stopped answering. */ | |
| 602 | const untuned = new Set<number>(ids); | |
| 603 | /** Write the gains to the servos that lack them, while nobody drives; a busy or stopped arm is tried again at the next status. */ | |
| 604 | async function tuneServos(): Promise<void> { | |
| 605 | if (tuning || driver || estop || gate.held) return; | |
| 606 | try { | |
| 607 | claim("move"); | |
| 608 | } catch { | |
| 609 | return; | |
| 610 | } | |
| 611 | tuning = true; | |
| 612 | try { | |
| 613 | const present = [...(await bus.syncRead(ids, STS3215.Present_Position)).keys()].filter((id) => untuned.has(id)); | |
| 614 | if (!present.length) return; | |
| 615 | recordFactory(factoryFile, await readServoParams(bus, ids)); // the baseline is what the servos had before this server wrote to them | |
| 616 | for (const { id, values } of await applyGains(bus, present, settings.servoGains)) machine.note("sent", "servo gains", { id, ...values }); | |
| 617 | for (const id of present) untuned.delete(id); | |
| 618 | } catch (err) { | |
| 619 | console.error(`servo gains: ${err instanceof Error ? err.message : String(err)}`); | |
| 620 | } finally { | |
| 621 | release(); | |
| 622 | tuning = false; | |
| 623 | } | |
| 624 | } | |
| 625 | ||
| 588 | 626 | const status = async () => { |
| 589 | 627 | // Present_Position (56) through Present_Temperature (63) is one block: one round trip, not one per register. |
| 590 | 628 | const block = await bus.syncReadBlock(ids, PRESENT); |
| @@ -595,6 +633,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 595 | 633 | torque = await bus.syncRead(ids, STS3215.Torque_Enable); |
| 596 | 634 | torqueAt = performance.now(); |
| 597 | 635 | } |
| 636 | for (const id of ids) if (!positions.has(id)) untuned.add(id); // a servo that fell silent may be a new one | |
| 637 | if (positions.size && ids.some((id) => untuned.has(id) && positions.has(id))) void tuneServos(); | |
| 598 | 638 | const b = bounds(); |
| 599 | 639 | const q = model ? toQ(positions) : null; |
| 600 | 640 | const joints = ids.map((id) => ({ |
| @@ -842,11 +882,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 842 | 882 | return json(res, 200, { ...settings, model: model !== null }); |
| 843 | 883 | case "PUT /settings": { |
| 844 | 884 | const body = await readBody(req); |
| 845 | const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2") => { | |
| 885 | const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2" | "servoGains") => { | |
| 846 | 886 | const patch = body[key]; |
| 847 | 887 | return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]); |
| 848 | 888 | }; |
| 849 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg"), jointSpeedDegS: merged("jointSpeedDegS"), jointAccelDegS2: merged("jointAccelDegS2") } as Partial<Settings>); | |
| 889 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg"), jointSpeedDegS: merged("jointSpeedDegS"), jointAccelDegS2: merged("jointAccelDegS2"), servoGains: merged("servoGains") } as Partial<Settings>); | |
| 850 | 890 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 851 | 891 | return json(res, 200, { ...settings, model: model !== null }); |
| 852 | 892 | } |
| @@ -1577,6 +1617,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1577 | 1617 | to.handleUpgrade(req, socket, head, (ws) => to.emit("connection", ws, req)); |
| 1578 | 1618 | }); |
| 1579 | 1619 | |
| 1620 | await tuneServos(); | |
| 1580 | 1621 | await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve)); |
| 1581 | 1622 | const address = http.address(); |
| 1582 | 1623 | const port = typeof address === "object" && address ? address.port : options.port ?? 8101; |
Software/so101/src/servo-gains.test.ts added +61
| @@ -0,0 +1,61 @@ | ||
| 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 { FeetechBus } from "./bus.ts"; | |
| 6 | import { FakeBus } from "./fake-bus.ts"; | |
| 7 | import { startServer, type RunningServer } from "./server.ts"; | |
| 8 | import { DEFAULT_GAINS, validateGains } from "./servo-gains.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 | const dataDir = mkdtempSync(join(tmpdir(), "so101-gains-")); | |
| 15 | const reg = (id: number, address: number) => fake.servos.get(id)![address]; | |
| 16 | const P = 21, D = 22, I = 23; | |
| 17 | ||
| 18 | beforeAll(async () => { | |
| 19 | fake = new FakeBus([1, 2, 3]); | |
| 20 | for (const id of [1, 2, 3]) Object.entries({ 21: 16, 22: 32, 23: 0 }).forEach(([a, v]) => (fake.servos.get(id)![Number(a)] = v)); | |
| 21 | server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1, 2, 3], posesFile: join(dataDir, "poses.json"), watchdogMs: 60 }); | |
| 22 | base = `http://127.0.0.1:${server.port}`; | |
| 23 | }); | |
| 24 | afterAll(() => Promise.race([server.close(), sleep(500)])); | |
| 25 | ||
| 26 | test("the arm server writes the measured gains at start and keeps the factory values", async () => { | |
| 27 | expect([reg(1, P), reg(1, I), reg(1, D)]).toEqual([64, 0, 32]); | |
| 28 | expect([reg(2, P), reg(2, I)]).toEqual([64, 4]); | |
| 29 | expect([reg(3, P), reg(3, I)]).toEqual([64, 4]); | |
| 30 | const rows = (await (await fetch(`${base}/servo-params`)).json()) as Array<{ id: number; registers: Record<string, number> }>; | |
| 31 | expect(rows.find((r) => r.id === 2)!.registers.P_Coefficient).toBe(64); | |
| 32 | const factory = JSON.parse(readFileSync(join(dataDir, "servo-factory.json"), "utf8")) as Record<string, Record<string, number>>; | |
| 33 | expect(factory[1]!.P_Coefficient).toBe(16); // the baseline is what the servo had before this server wrote to it | |
| 34 | }); | |
| 35 | ||
| 36 | test("a servo that was replaced and answers again is tuned again", async () => { | |
| 37 | fake.servos.get(2)![P] = 16; // a new servo, factory P | |
| 38 | fake.servos.get(2)![I] = 0; | |
| 39 | const original = fake.servos.get(2)!; | |
| 40 | fake.servos.delete(2); | |
| 41 | await fetch(`${base}/status`); | |
| 42 | fake.servos.set(2, original); | |
| 43 | await fetch(`${base}/status`); | |
| 44 | await sleep(100); | |
| 45 | expect([reg(2, P), reg(2, I)]).toEqual([64, 4]); | |
| 46 | }); | |
| 47 | ||
| 48 | test("gains are checked: joint names and ranges", () => { | |
| 49 | expect(validateGains(DEFAULT_GAINS)).toEqual(DEFAULT_GAINS); | |
| 50 | expect(() => validateGains({ elbow: { P: 20 } })).toThrow("no joint"); | |
| 51 | expect(() => validateGains({ elbow_flex: { P: 65 } })).toThrow("P must be"); | |
| 52 | expect(() => validateGains({ elbow_flex: { Torque_Limit: 1000 } })).toThrow("not P, I or D"); | |
| 53 | expect(() => validateGains([])).toThrow(); | |
| 54 | }); | |
| 55 | ||
| 56 | test("settings carry the gains and refuse bad ones", async () => { | |
| 57 | const settings = (await (await fetch(`${base}/settings`)).json()) as { servoGains: unknown }; | |
| 58 | expect(settings.servoGains).toEqual(DEFAULT_GAINS); | |
| 59 | const bad = await fetch(`${base}/settings`, { method: "PUT", body: JSON.stringify({ servoGains: { gripper: { P: 99 } } }) }); | |
| 60 | expect(bad.status).toBe(400); | |
| 61 | }); |
Software/so101/src/servo-gains.ts added +74
| @@ -0,0 +1,74 @@ | ||
| 1 | import type { FeetechBus } from "./bus.ts"; | |
| 2 | import { TUNABLE, writeServoParams } from "./servo-params.ts"; | |
| 3 | import { JOINTS, STS3215 } from "./sts3215.ts"; | |
| 4 | ||
| 5 | /** One joint's loop gains; a gain left out stays as the servo has it. */ | |
| 6 | export interface Gains { | |
| 7 | P?: number; | |
| 8 | I?: number; | |
| 9 | D?: number; | |
| 10 | } | |
| 11 | ||
| 12 | /** | |
| 13 | * What the arm is tuned to, measured on the real arm 2026-10-21 (README, "Servo tuning"): the shipped P16 I0 settled | |
| 14 | * 24-27 ticks short and never arrived; P64 arrives in 150-330 ms with at most 3 ticks of overshoot. The gravity | |
| 15 | * joints take I4 for the last ticks. The gripper stays at the factory P16: its torque is capped on purpose. | |
| 16 | */ | |
| 17 | export const DEFAULT_GAINS: Readonly<Record<string, Gains>> = { | |
| 18 | shoulder_pan: { P: 64 }, | |
| 19 | shoulder_lift: { P: 64, I: 4 }, | |
| 20 | elbow_flex: { P: 64, I: 4 }, | |
| 21 | wrist_flex: { P: 64 }, | |
| 22 | wrist_roll: { P: 64 }, | |
| 23 | }; | |
| 24 | ||
| 25 | const KEYS = ["P", "I", "D"] as const; | |
| 26 | ||
| 27 | /** `servoGains` as stored or sent: joint name → gains inside the ranges a write may use; throws a message naming the first bad one. */ | |
| 28 | export function validateGains(input: unknown): Record<string, Gains> { | |
| 29 | if (typeof input !== "object" || input === null || Array.isArray(input)) throw new Error("servoGains must be an object of joint: {P, I, D}"); | |
| 30 | const out: Record<string, Gains> = {}; | |
| 31 | for (const [joint, gains] of Object.entries(input)) { | |
| 32 | if (!Object.values(JOINTS).includes(joint)) throw new Error(`no joint "${joint}"`); | |
| 33 | if (typeof gains !== "object" || gains === null || Array.isArray(gains)) throw new Error(`servoGains.${joint} must be an object of P, I, D`); | |
| 34 | const kept: Gains = {}; | |
| 35 | for (const key of Object.keys(gains)) if (!(KEYS as readonly string[]).includes(key)) throw new Error(`servoGains.${joint}.${key} is not P, I or D`); | |
| 36 | for (const key of KEYS) { | |
| 37 | const value = (gains as Gains)[key]; | |
| 38 | if (value === undefined) continue; | |
| 39 | const { min, max } = TUNABLE[key]; | |
| 40 | if (typeof value !== "number" || !Number.isInteger(value) || value < min || value > max) throw new Error(`servoGains.${joint}.${key} must be a whole number from ${min} to ${max}`); | |
| 41 | kept[key] = value; | |
| 42 | } | |
| 43 | out[joint] = kept; | |
| 44 | } | |
| 45 | return out; | |
| 46 | } | |
| 47 | ||
| 48 | /** | |
| 49 | * Write each listed servo the gains it lacks, and only those: a servo already at its gains is not touched, so a | |
| 50 | * restart costs the EEPROM nothing. Returns what was written, per servo id. The caller holds the arm. | |
| 51 | */ | |
| 52 | export async function applyGains(bus: FeetechBus, ids: readonly number[], gains: Readonly<Record<string, Gains>>): Promise<Array<{ id: number; values: Record<string, number> }>> { | |
| 53 | const written: Array<{ id: number; values: Record<string, number> }> = []; | |
| 54 | for (const id of ids) { | |
| 55 | const want = gains[JOINTS[id] ?? ""]; | |
| 56 | if (!want) continue; | |
| 57 | const values: Record<string, number> = {}; | |
| 58 | let silent = false; | |
| 59 | for (const key of KEYS) { | |
| 60 | const { register } = TUNABLE[key]; | |
| 61 | if (want[key] === undefined) continue; | |
| 62 | const now = (await bus.syncRead([id], STS3215[register])).get(id); | |
| 63 | if (now === undefined) { | |
| 64 | silent = true; // a servo that does not answer has nothing to write to; the next sighting tries again | |
| 65 | break; | |
| 66 | } | |
| 67 | if (now !== want[key]) values[register] = want[key]!; | |
| 68 | } | |
| 69 | if (silent || !Object.keys(values).length) continue; | |
| 70 | await writeServoParams(bus, id, values); | |
| 71 | written.push({ id, values }); | |
| 72 | } | |
| 73 | return written; | |
| 74 | } |
Software/so101/src/ui/intent.js +1 −1
| @@ -7,7 +7,7 @@ | ||
| 7 | 7 | export class Intent { |
| 8 | 8 | /** @param {{ hold?: number, near?: number }} [options] hold: ms an unreached target is kept after release; near: ticks that count as arrived */ |
| 9 | 9 | constructor(options = {}) { |
| 10 | this.hold = options.hold ?? 2000; | |
| 10 | this.hold = options.hold ?? 1000; | |
| 11 | 11 | this.near = options.near ?? 12; |
| 12 | 12 | /** @type {Map<number, { ticks: number, down: boolean, since: number, hold?: number }>} */ |
| 13 | 13 | this.targets = new Map(); |
Software/so101/src/ui/intent.test.ts +3 −3
| @@ -14,11 +14,11 @@ describe("Intent", () => { | ||
| 14 | 14 | expect(i.target(1, 2995, 600)).toBeNull(); |
| 15 | 15 | expect(i.target(1, 2048, 700)).toBeNull(); |
| 16 | 16 | }); |
| 17 | test("an arm that never arrives is followed again after 2 s", () => { | |
| 17 | test("an arm that never arrives is followed again after 1 s, three times the slowest settle measured (330 ms)", () => { | |
| 18 | 18 | const i = new Intent(); |
| 19 | 19 | i.set(2, 3000, 0); i.release(2, 0); |
| 20 | expect(i.target(2, 2048, 1999)).toBe(3000); | |
| 21 | expect(i.target(2, 2048, 2000)).toBeNull(); | |
| 20 | expect(i.target(2, 2048, 999)).toBe(3000); | |
| 21 | expect(i.target(2, 2048, 1000)).toBeNull(); | |
| 22 | 22 | }); |
| 23 | 23 | test("a held target is not dropped when the arm happens to be there", () => { |
| 24 | 24 | const i = new Intent(); |
new above
offline