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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

Split the app into small modules so parallel pulls stop colliding (one feature, one file) #61

Parked opened this in answer to
Closed without merging

smith/arm:smith/58-split-the-app-into-small-modules-so-para → main 6 commits fork 4 threads · 4 unresolved

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
20 files changed +1632 −1265
Software/so101/src/lobby.test.ts +3 −2
@@ -40,7 +40,7 @@ async function client(port: number, headers: Record<string, string> = {}) {
4040 for (const w of [...waiters]) if (w.match(m)) waiters.splice(waiters.indexOf(w), 1), w.resolve(m);
4141 });
4242 await new Promise((r, j) => (ws.once("open", r), ws.once("error", j)));
43 const next = (match: (m: Msg) => boolean, ms = 2000) =>
43 const next = (match: (m: Msg) => boolean, ms = 4000) =>
4444 new Promise<Msg>((resolve, reject) => {
4545 waiters.push({ match, resolve });
4646 setTimeout(() => reject(new Error(`nothing matched in ${ms} ms; heard ${JSON.stringify(heard.slice(-5))}`)), ms);
@@ -555,10 +555,11 @@ describe("lobby", () => {
555555 await bob.next(drivingIs("alice"));
556556 bob.send({ type: "join" });
557557 await alice.next((m) => m.type === "state" && m.queue.length === 1);
558 const handed = alice.next(drivingIs("bob")); // listen before the form: the hand-over can land before its response does
558559 const ended = await form(port, { action: "end" }, OWNER);
559560 expect(ended.status).toBe(303);
560561 expect(ended.headers.get("location")).toBe("/admin?done=ended");
561 await alice.next(drivingIs("bob"));
562 await handed;
562563
563564 const saved = await form(port, { action: "save", turnSeconds: "90", idleSeconds: "20", maxSpeedPercent: "40", maxQueue: "3", announcement: '<script>alert("x")</script>' }, OWNER);
564565 expect(saved.status).toBe(303);
Software/so101/src/server-entry.test.ts added +22
@@ -0,0 +1,22 @@
1 import { expect, test } from "bun:test";
2 import { readFileSync } from "node:fs";
3 import { join } from "node:path";
4
5 const source = readFileSync(join(import.meta.dir, "server.ts"), "utf8");
6 const code = source.replace(/\/\*[\s\S]*?\*\//g, "").split("\n");
7
8 test("the entry only wires: it imports the feature modules and holds no route or control flow", () => {
9 const imports = code.filter((l) => /^import /.test(l));
10 expect(imports.filter((l) => l.includes('"./server/')).length).toBeGreaterThanOrEqual(8);
11 const body = code.filter((l) => !/^(import|export \{|export type)/.test(l)).join("\n");
12 expect(body).not.toMatch(/\b(if|for|while|switch)\s*\(\s*(?!const wss|const client)/);

The negative lookahead (?!const wss|const client) exempts the for (const client of …) loop in close(), so the test is bent around the code it checks. Move the shutdown into a helper (for example closeAll(http, wss, watch) in upgrade.ts or its own module), drop the exemption, and the "no control flow in the entry" claim holds without exceptions.

This lookahead lets in the for (const client of ...) loop in close(), which is control flow in the entry. Move the teardown into a server/lifecycle.ts, or let attachStream and attachWatch return a close(). Then this rule can be a plain "no if/for/while/switch" with no exceptions.

The negative lookahead (?!const wss|const client) is an exemption fitted to today's close(). Any for (const client …) would pass it. If you need it, assert the shape you want instead, e.g. that startServer's body is calls and returns only. Or drop the branching check and keep the import/route/length checks, which already carry the point. This is minor.

13 expect(body).not.toMatch(/["'`](GET|POST|PUT) \//);
14 expect(code.length).toBeLessThan(60);
15 });
16
17 test("every module under src/server stays small enough for one concern", () => {
18 const dir = join(import.meta.dir, "server");
19 for (const f of new Bun.Glob("*.ts").scanSync(dir)) {
20 expect(readFileSync(join(dir, f), "utf8").split("\n").length).toBeLessThan(320);
21 }
22 });
Software/so101/src/server.ts +26 −1263
@@ -49,1268 +49,33 @@
4949 * its goal is parked where it is, and it follows again once the leader backs off.
5050 */
5151
52 import { existsSync, mkdirSync, readdirSync, readFileSync, renameSync, statSync, unlinkSync, writeFileSync } from "node:fs";
53 import { createServer, type IncomingMessage, type ServerResponse } from "node:http";
54 import { dirname, extname, join, resolve } from "node:path";
55 import type { Duplex } from "node:stream";
56 import { createHash, randomUUID } from "node:crypto";
57 import { fileURLToPath } from "node:url";
58 import { WebSocketServer, type WebSocket } from "ws";
59 import { browserModule } from "./browser-modules.ts";
60 import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts";
52 import { createServer } from "node:http";
6153 import type { FeetechBus } from "./bus.ts";
62 import { ArmModel, gripperTicksToRadians, ticksToRadians, type Violation } from "./kinematics.ts";
63 import { clamp, type Frame } from "./mapping.ts";
64 import { MachineLog, perServo } from "./machine-log.ts";
65 import { Metrics } from "./metrics.ts";
66 import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts";
67 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts";
68 import { JOINTS, registerName, STS3215, ticksToDegrees } from "./sts3215.ts";
69 import { largestStep, parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts";
70 import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, SAFETY_KEYS, SafetyError, TICKS_PER_DEGREE, validateSafety, type SafetySettings } from "./safety.ts";
54 import { createContext } from "./server/context.ts";
55 import { moveRoutes } from "./server/move.ts";
56 import { recordRoutes } from "./server/record.ts";
57 import { createRouter } from "./server/router.ts";
58 import { actor, safetyRoutes } from "./server/safety.ts";
59 import { settingsRoutes } from "./server/settings.ts";
60 import { staticPrefixes, staticRoutes } from "./server/static.ts";
61 import { statusRoutes } from "./server/status.ts";
62 import { calibrationPut, storageRoutes } from "./server/storage.ts";
63 import { attachStream } from "./server/stream.ts";
64 import { attachUpgrades } from "./server/upgrade.ts";
65 import { attachWatch } from "./server/watch.ts";
outdated

This is a good thin entry. But it is the arm-local server, which only cli.ts and bench/fake-serve.ts start. The file where feature pulls collide is src/relay.ts, which serves queue, chat, admin, cameras and arm-link to every visitor, together with src/viewer.html. #58's module list (arm-link, camera, queue, chat, moderation, admin) describes relay.ts, not this file.

66 import type { RunningServer, ServerOptions } from "./server/types.ts";
7167
72 export interface ServerOptions {
73 host?: string;
74 port?: number;
75 ids?: readonly number[];
76 /** The poses file; recordings, calibration, settings and metrics live beside it. */
77 posesFile?: string;
78 /** Folder holding so101_new_calib.urdf and assets/; without it there is no safe zone and no 3D model. */
79 modelDir?: string;
80 /** Folder with the printable PNG sheets of the vision kit; found beside the package or in the repo layout. */
81 printablesDir?: string;
82 /** Torque cap while a stream drives the arm, percent. */
83 streamTorque?: number;
84 /** Largest per-frame change accepted on the stream, ticks. */
85 maxStep?: number;
86 /** With no frame for this long, the arm is told to hold where it is; overrides the holdAfterMs setting. */
87 watchdogMs?: number;
88 /** How often a stream's client is pinged; one that misses a ping is a lost connection, and the arm parks. */
89 heartbeatMs?: number;
90 /** Starting values for the safety settings, before what is stored (src/safety.ts). */
91 safety?: Partial<SafetySettings>;
92 /** Speed used to catch up with the first frame of a stream, ticks/s. */
93 easeSpeed?: number;
94 /** How often the stream's jam watch samples position and load, ms. */
95 jamSampleMs?: number;
96 /** How often each /watch connection is sent the status, ms. */
97 watchEveryMs?: number;
98 }
99
100 export interface RunningServer {
101 readonly port: number;
102 close(): Promise<void>;
103 }
104
105 export type Driver = "stream" | "move" | "play" | "record" | "park";
106
107 export interface Settings extends SafetySettings {
108 zone: "enforce" | "warn" | "off";
109 floorMarginMm: number;
110 baseMarginMm: number;
111 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
112 jointOffsetsDeg: Record<string, number>;
113 }
114
115 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY };
116
117 /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */
118 export interface EStop {
119 by: string;
120 at: string;
121 }
122
123 /**
124 * Who an E-stop or its clearing is credited to: the body's `by`, one line of at most 64 characters, kept as text
125 * (pages show it with textContent). It is only a label: on this server it is unauthenticated, and the relay sets it.
126 */
127 export function actor(body: Record<string, unknown>): string {
128 const by = typeof body.by === "string" ? body.by.replace(/[\p{Cc}\p{Cf}\s]+/gu, " ").trim().slice(0, 64).trim() : "";
129 return by || "someone at the arm";
130 }
131
132 /** Close codes a stream ends with on purpose; any other end is a lost connection. */
133 const CLEAN_CLOSE = new Set([1000, 1005]);
134 /** The close code for a stream ended by an emergency stop. */
135 export const ESTOP_CLOSE = 4423;
136
137 class HttpError extends Error {
138 readonly status: number;
139 constructor(status: number, message: string) {
140 super(message);
141 this.status = status;
142 }
143 }
144
145 const MAX_BODY = 1_000_000;
146 const URDF = "so101_new_calib.urdf";
147 /** Load, in percent of the servo's maximum, that counts as pushing against something. */
148 const JAM_LOAD = 45;
149 const JAM_WINDOW = 6;
150 /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */
151 const RESUME_AFTER_DROPS = 20;
152 const VENDORED = new Set(["three", "urdf-loader"]);
153 /** A module the page loads from src/ui: plain JavaScript, never its tests. */
154 const isUiModule = (name: string) => /^[a-z][a-z0-9-]*\.js$/.test(name);
155 const MIME: Record<string, string> = { ".js": "text/javascript", ".mjs": "text/javascript", ".json": "application/json", ".urdf": "application/xml", ".stl": "model/stl", ".dae": "model/vnd.collada+xml", ".png": "image/png", ".jpg": "image/jpeg" };
68 export { DEFAULT_SETTINGS, ESTOP_CLOSE } from "./server/types.ts";
69 export type { Driver, EStop, RunningServer, ServerOptions, Settings } from "./server/types.ts";
70 export { actor };
15671
15772 export async function startServer(bus: FeetechBus, options: ServerOptions = {}): Promise<RunningServer> {
158 const ids = options.ids ?? [1, 2, 3, 4, 5, 6];
159 const posesFile = resolve(options.posesFile ?? "poses.json");
160 const dataDir = dirname(posesFile);
161 const recordingsDir = join(dataDir, "recordings");
162 const calibrationDir = join(dataDir, "calibration");
163 const settingsFile = join(dataDir, "settings.json");
164 const streamTorque = options.streamTorque ?? 70;
165 const maxStep = options.maxStep ?? 400;
166 const easeSpeed = options.easeSpeed ?? 300;
167 const jamSampleMs = options.jamSampleMs ?? 100;
168 const heartbeatMs = options.heartbeatMs ?? 1000;
169 const estopFile = join(dataDir, "estop.json");
170 let safetyBase: SafetySettings;
171 try {
172 safetyBase = validateSafety(options.safety ?? {});
173 } catch (err) {
174 throw new Error(`safety options: ${(err as Error).message}`);
175 }
176 const metrics = new Metrics(join(dataDir, "metrics.jsonl"));
177 // The machine, raw: every sync write as it goes on the wire, and each joint's last goal, for the Machine panel.
178 const machine = new MachineLog();
179 const run = randomUUID();
180 const goals = new Map<number, number>();
181 bus.onSyncWrite = (register, values) => {
182 if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v);
183 machine.note("sent", registerName(register), { values: perServo(values) });
184 };
185 /** The stream's dead-man hold is in force: no frame came for holdAfterMs, and the arm stays where it was. */
186 let held = false;
187
188 const limits = new Map<number, { min: number; max: number }>();
189 for (const id of ids) {
190 limits.set(id, {
191 min: await bus.read(id, STS3215.Min_Position_Limit),
192 max: await bus.read(id, STS3215.Max_Position_Limit),
193 });
194 }
195
196 // ------------------------------------------------------------------ the model and the zone
197 const here = dirname(fileURLToPath(import.meta.url));
198 const modelDir = options.modelDir ?? [join(here, "../../../Simulation/SO101"), join(here, "../model")].find((d) => existsSync(join(d, URDF)));
199 const printablesDir = options.printablesDir ?? [join(here, "../../vision/printables"), join(here, "../printables")].find((d) => existsSync(d));
200 let model: ArmModel | null = null;
201 if (modelDir && existsSync(join(modelDir, URDF))) {
202 try {
203 model = ArmModel.load(join(modelDir, URDF));
204 } catch (err) {
205 console.error(`model: ${err instanceof Error ? err.message : String(err)}; no safe zone`);
206 }
207 } else console.error(`model: no ${URDF} in ${modelDir ?? "(no model dir)"}; no safe zone and no 3D view`);
208
209 let settings: Settings = { ...DEFAULT_SETTINGS, ...safetyBase };
210 if (existsSync(settingsFile)) {
211 // Field by field: one bad value must not throw away the zone mode or the other trims.
212 try {
213 const stored = JSON.parse(readFileSync(settingsFile, "utf8")) as Record<string, unknown>;
214 for (const [key, value] of Object.entries(stored)) {
215 try {
216 settings = validateSettings({ ...settings, [key]: value });
217 } catch (err) {
218 console.error(`settings: ignoring ${key} in ${settingsFile}: ${err instanceof Error ? err.message : String(err)}`);
219 }
220 }
221 } catch (err) {
222 console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`);
223 }
224 }
225 const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 });
226 /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */
227 const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745;
228 const toQ = (positions: ReadonlyMap<number, number>) => {
229 const q = new Map<string, number>();
230 for (const [id, ticks] of positions) {
231 const name = JOINTS[id];
232 const lim = limits.get(id);
233 if (!name || !lim) continue;
234 const rad = name === "gripper" ? gripperTicksToRadians(ticks, lim.min, lim.max, gripperOpen) : ticksToRadians(ticks, lim.min, lim.max);
235 q.set(name, rad + ((settings.jointOffsetsDeg[name] ?? 0) * Math.PI) / 180);
236 }
237 return q;
238 };
239 const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null);
240 /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */
241 const approach = (r: ReturnType<typeof evaluate>) => {
242 if (r && model) metrics.approach(r.lowest.z - model.floorZ, r.lowest.link);
243 };
244 const describeViolation = (v: Violation) => {
245 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
246 const marginMm = v.what === "table" ? settings.floorMarginMm : settings.baseMarginMm;
247 const beyond = v.depth * 1000 - marginMm; // the depth includes the margin
248 return beyond > 0
249 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`
250 : `${part} would come within ${(-beyond).toFixed(0)} mm of the ${v.what}, inside the ${marginMm} mm margin`;
251 };
252 /** The worst violation on the straight joint-space path from one pose to another, or null. */
253 const checkPath = (from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => {
254 if (!model || settings.zone === "off") return null;
255 let worst: Violation | null = null;
256 for (let s = 1; s <= steps; s++) {
257 const mix = new Map<number, number>();
258 for (const [id, a] of from) mix.set(id, Math.round(a + ((to.get(id) ?? a) - a) * (s / steps)));
259 const v = model.check(toQ(mix), zoneSettings());
260 if (v && (!worst || v.depth > worst.depth)) worst = v;
261 }
262 return worst;
263 };
264
265 // ------------------------------------------------------------------ who drives the arm
266 let driver: Driver | null = null;
267 let current: AbortController | null = null;
268 const claim = (kind: Driver): AbortController => {
269 if (estop) throw new HttpError(423, stopped(estop));
270 if (gate.held) throw new HttpError(423, holding(gate.held));
271 if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`);
272 driver = kind;
273 current = new AbortController();
274 machine.note("state", "driver", { driving: kind });
275 return current;
276 };
277 const release = () => {
278 if (driver) machine.note("state", "driver", { driving: null });
279 driver = null;
280 current = null;
281 };
282 const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d];
283
284 // ------------------------------------------------------------------ the safety envelope (src/safety.ts)
285 /** Where each joint may be sent, ticks: the calibrated range, less the margin, narrowed by the admin's limits. */
286 const bounds = () => jointBounds(limits, settings);
287 const speedLimits = () => motionLimits(settings);
288 /** The gripper's force limit, for every supervised move. */
289 const torqueCaps = () => new Map([[6, settings.gripperTorquePercent]]);
290 /** The servo registers that enforce the same speed and acceleration in hardware, whatever the server sends. */
291 const servoLimits = async (targetIds: readonly number[]) => {
292 const { speed, accel } = speedLimits();
293 await bus.syncWrite(STS3215.Goal_Velocity, targetIds.map((id) => [id, speed] as const));
294 await bus.syncWrite(STS3215.Acceleration, targetIds.map((id) => [id, accelRegister(accel)] as const));
295 };
296 let estop: EStop | null = null;
297 try {
298 const stored = JSON.parse(readFileSync(estopFile, "utf8")) as Partial<EStop>;
299 if (typeof stored.by === "string" && typeof stored.at === "string") estop = { by: stored.by, at: stored.at };
300 else estop = { by: "an unreadable record", at: new Date().toISOString() }; // a stop on disk that cannot be read still stops
301 } catch (err) {
302 if ((err as NodeJS.ErrnoException).code !== "ENOENT") estop = { by: "an unreadable record", at: new Date().toISOString() };
303 }
304 /** STOP's hold: every frame is dropped and every move refused until /resume (src/command-gate.ts). */
305 const gate = new CommandGate();
306 const holding = (h: { by: string; at: string }) => `stopped (${h.by}, ${h.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until someone resumes`;
307 const stopped = (e: EStop) => `emergency stop (${e.by}, ${e.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until an admin clears it`;
308 /** Torque on, goal where each joint is: the arm stays put and resists. */
309 const holdHere = async () => {
310 const positions = await bus.syncRead(ids, STS3215.Present_Position);
311 const here = ids.filter((id) => positions.has(id));
312 await bus.syncWrite(STS3215.Goal_Position, here.map((id) => [id, positions.get(id)!] as const));
313 await bus.syncWrite(STS3215.Torque_Enable, here.map((id) => [id, 1] as const));
314 };
315 /** Halt everything and hold with torque; nothing moves the arm until `clearEStop`. Hitting it again only holds again. */
316 const engageEStop = async (by: string): Promise<EStop> => {
317 if (!estop) {
318 estop = { by, at: new Date().toISOString() };
319 writeAtomic(estopFile, JSON.stringify(estop));
320 metrics.event("estop", { by });
321 machine.note("state", "emergency stop", { by });
322 }
323 const was = driver;
324 if (was === "stream") for (const client of wss.clients) end(client, ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks
325 else if (was === "record" && recording) {
326 const { done } = recording;
327 recording = null; // nothing is saved: the recording ends where it was cut
328 current?.abort();
329 void done.then(release);
330 } else current?.abort(); // a move, a playback or a park stops where it is
331 await holdHere();
332 return estop;
333 };
334 const clearEStop = (by: string) => {
335 if (!estop) return false;
336 estop = null;
337 try {
338 unlinkSync(estopFile);
339 } catch {
340 /* already gone */
341 }
342 metrics.event("estop-clear", { by });
343 machine.note("state", "emergency stop cleared", { by });
344 return true;
345 };
346
347 /**
348 * The driver's connection is gone: go slowly to the park pose (the saved pose named in the settings, or the middle
349 * of every arm joint's range), under supervision, unless the arm is stopped, busy, or the way there is in the zone.
350 */
351 const park = async (why: string) => {
352 if (estop || gate.held || driver) return;
353 const controller = claim("park");
354 try {
355 const pose = loadPoses()[settings.parkPose];
356 const b = bounds();
357 const targets: Array<[number, number]> = pose
358 ? pose.ids.flatMap((id, i) => (b.has(id) ? [[id, bound(b, id, pose.positions[i]!)!] as [number, number]] : []))
359 : ids.filter((id) => id !== 6 && b.has(id)).map((id) => [id, bound(b, id, (limits.get(id)!.min + limits.get(id)!.max) / 2)!]);
360 if (!targets.length) return;
361 const from = await bus.syncRead(ids, STS3215.Present_Position);
362 const violation = checkPath(from, new Map([...from, ...targets]));
363 if (violation && settings.zone === "enforce") {
364 metrics.event("park", { why, parked: false, error: `refused: ${describeViolation(violation)}` });
365 machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: true, source: "park" });
366 return;
367 }
368 await moveToPose(bus, targets.map(([id]) => id), targets.map(([, t]) => t), {
369 speed: Math.round(settings.parkSpeedDegS * TICKS_PER_DEGREE),
370 torquePercent: 50,
371 torqueCaps: torqueCaps(),
372 signal: controller.signal,
373 });
374 metrics.event("park", { why, parked: true, pose: pose ? settings.parkPose : "middle" });
375 machine.note("state", "parked", { why, pose: pose ? settings.parkPose : "middle" });
376 } catch (err) {
377 metrics.event("park", { why, parked: false, error: err instanceof Error ? err.message : String(err) });
378 } finally {
379 release();
380 }
381 };
382 /** What the stream is up against right now, for /status. */
383 let blocked: Violation | null = null;
384 const jams = new Map<number, { position: number; direction: number }>();
385 let streamStats: { frames: number; dropped: number; refused: number } | null = null;
386
387 // ------------------------------------------------------------------ files
388 const writeAtomic = (path: string, text: string) => {
389 mkdirSync(dirname(path), { recursive: true });
390 writeFileSync(`${path}.tmp`, text);
391 renameSync(`${path}.tmp`, path);
392 };
393 const loadPoses = (): PoseFile => {
394 try {
395 return parsePoseFile(readFileSync(posesFile, "utf8"));
396 } catch (err) {
397 if ((err as NodeJS.ErrnoException).code === "ENOENT") return {};
398 throw new HttpError(500, `${posesFile}: ${err instanceof Error ? err.message : String(err)}`);
399 }
400 };
401 const poseNamed = (name: unknown): string => {
402 if (typeof name !== "string") throw new HttpError(400, "name must be a string");
403 try {
404 assertPoseName(name);
405 } catch (err) {
406 throw new HttpError(400, (err as Error).message);
407 }
408 return name;
409 };
410 const recordingPath = (name: string) => join(recordingsDir, `${name}.json`);
411 const loadRecording = (name: string): Trajectory => {
412 try {
413 return parseTrajectory(readFileSync(recordingPath(name), "utf8"));
414 } catch (err) {
415 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
416 throw new HttpError(500, `recording "${name}": ${err instanceof Error ? err.message : String(err)}`);
417 }
418 };
419 const listRecordings = () => {
420 const out: Record<string, { hz: number; frames: number; seconds: number; recorded?: string }> = {};
421 let names: string[] = [];
422 try {
423 names = readdirSync(recordingsDir).filter((f) => f.endsWith(".json"));
424 } catch {
425 return out;
426 }
427 for (const file of names) {
428 try {
429 const t = parseTrajectory(readFileSync(join(recordingsDir, file), "utf8"));
430 out[file.slice(0, -5)] = { hz: t.hz, frames: t.frames.length, seconds: Number((t.frames.length / t.hz).toFixed(1)), ...(t.recorded ? { recorded: t.recorded } : {}) };
431 } catch {
432 /* a file that is not a recording is not listed */
433 }
434 }
435 return out;
436 };
437 const loadCalibration = (): Record<string, unknown> => {
438 const out: Record<string, unknown> = { camera: null, handeye: null };
439 try {
440 for (const file of readdirSync(calibrationDir).filter((f) => f.endsWith(".json"))) {
441 try {
442 out[file.slice(0, -5)] = JSON.parse(readFileSync(join(calibrationDir, file), "utf8"));
443 } catch {
444 /* skip a broken file */
445 }
446 }
447 } catch {
448 /* none yet */
449 }
450 return out;
451 };
452 /** A file under a root, or null for anything that tries to leave it. */
453 const safeFile = (root: string, rel: string): string | null => {
454 if (rel.includes("\0") || rel.split("/").some((part) => part === "..")) return null;
455 const path = resolve(root, rel);
456 if (path !== root && !path.startsWith(root + "/")) return null;
457 try {
458 return statSync(path).isFile() ? path : null;
459 } catch {
460 return null;
461 }
462 };
463
464 // ------------------------------------------------------------------ helpers
465 const json = (res: ServerResponse, status: number, body: unknown) => {
466 res.writeHead(status, { "content-type": "application/json" });
467 res.end(JSON.stringify(body));
468 };
469 const file = (res: ServerResponse, path: string, cache: string) => {
470 res.writeHead(200, { "content-type": MIME[extname(path).toLowerCase()] ?? "application/octet-stream", "cache-control": cache });
471 res.end(readFileSync(path));
472 };
473 const readBody = (req: IncomingMessage) =>
474 new Promise<Record<string, unknown>>((resolve, reject) => {
475 let data = "";
476 req.on("data", (c) => {
477 data += c;
478 if (data.length > MAX_BODY) {
479 req.destroy();
480 reject(new HttpError(413, "body too large"));
481 }
482 });
483 req.on("end", () => {
484 if (!data.trim()) return resolve({});
485 try {
486 const parsed: unknown = JSON.parse(data);
487 if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) throw new Error();
488 resolve(parsed as Record<string, unknown>);
489 } catch {
490 reject(new HttpError(400, "body must be a JSON object"));
491 }
492 });
493 req.on("error", reject);
494 });
495 const number = (value: unknown, fallback: number, min: number, max: number, what: string): number => {
496 if (value === undefined) return fallback;
497 if (typeof value !== "number" || !Number.isFinite(value)) throw new HttpError(400, `${what} must be a number`);
498 return clamp(value, min, max);
499 };
500 const idList = (value: unknown): number[] => {
501 if (value === undefined) return [...ids];
502 if (!Array.isArray(value) || !value.length || !value.every((id) => Number.isInteger(id) && ids.includes(id as number))) {
503 throw new HttpError(400, `ids must be a non-empty list drawn from ${ids.join(", ")}`);
504 }
505 return [...new Set(value as number[])];
506 };
507 function validateSettings(s: Partial<Settings>): Settings {
508 let safety: SafetySettings;
509 try {
510 safety = validateSafety(Object.fromEntries(SAFETY_KEYS.map((key) => [key, s[key as keyof SafetySettings]])), safetyBase);
511 } catch (err) {
512 if (err instanceof SafetyError) throw new HttpError(400, err.message);
513 throw err;
514 }
515 const out: Settings = { ...DEFAULT_SETTINGS, ...safety, jointOffsetsDeg: {} };
516 if (s.zone !== undefined) {
517 if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"');
518 out.zone = s.zone;
519 }
520 for (const key of ["floorMarginMm", "baseMarginMm"] as const) {
521 if (s[key] === undefined) continue;
522 if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`);
523 out[key] = s[key]!;
524 }
525 if (s.jointOffsetsDeg !== undefined) {
526 if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object");
527 for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) {
528 if (!Object.values(JOINTS).includes(name)) throw new HttpError(400, `no joint "${name}"`);
529 if (typeof deg !== "number" || !Number.isFinite(deg) || Math.abs(deg) > 30) throw new HttpError(400, `offset for ${name} must be within ±30 degrees`);
530 if (deg !== 0) out.jointOffsetsDeg[name] = deg;
531 }
532 }
533 return out;
534 }
535
536 const status = async () => {
537 const positions = await bus.syncRead(ids, STS3215.Present_Position);
538 const loads = await bus.syncRead(ids, STS3215.Present_Load);
539 const temps = await bus.syncRead(ids, STS3215.Present_Temperature);
540 const torque = await bus.syncRead(ids, STS3215.Torque_Enable);
541 const b = bounds();
542 const joints = ids.map((id) => ({
543 id,
544 joint: JOINTS[id] ?? null,
545 position: positions.get(id) ?? null,
546 degrees: positions.has(id) ? Number(ticksToDegrees(positions.get(id)!).toFixed(1)) : null,
547 load: loads.has(id) ? loads.get(id)! / 10 : null,
548 temperature: temps.get(id) ?? null,
549 torque: torque.has(id) ? torque.get(id) === 1 : null,
550 /** The last Goal_Position this server wrote, ticks; null before the first. */
551 target: goals.get(id) ?? null,
552 /** The calibrated range (the servo's own limits), and the narrower bounds every command is held inside. */
553 min: limits.get(id)?.min ?? null,
554 max: limits.get(id)?.max ?? null,
555 lo: b.get(id)?.min ?? null,
556 hi: b.get(id)?.max ?? null,
557 }));
558 metrics.sample(joints);
559 const zone = evaluate(positions);
560 approach(zone);
561 return {
562 joints,
563 busy: driver !== null,
564 driving: driver,
565 recording: recording?.name ?? null,
566 zone: model ? settings.zone : "off",
567 violation: zone?.violation ?? null,
568 lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000).toFixed(0)) : null,
569 blocked,
570 jammed: [...jams.keys()],
571 stream: streamStats,
572 /** The stream's dead-man hold (no frame lately); `held` is the STOP's, which only /resume ends. */
573 deadman: held,
574 estop,
575 held: gate.held,
576 machine: { run, seq: machine.seq },
577 };
578 };
579
580 /** A supervised move to explicit targets; the one way joints move over HTTP. */
581 const move = async (targetIds: number[], requested: number[], body: Record<string, unknown>, source: string) => {
582 const controller = claim("move");
583 const b = bounds();
584 const positions = targetIds.map((id, i) => bound(b, id, requested[i]!) ?? requested[i]!);
585 targetIds.forEach((id, i) => positions[i] !== requested[i] && machine.note("clamp", "joint limit", { id, asked: requested[i]!, sent: positions[i]!, source }));
586 try {
587 const from = await bus.syncRead(ids, STS3215.Present_Position);
588 const to = new Map(from);
589 targetIds.forEach((id, i) => to.set(id, positions[i]!));
590 const violation = checkPath(from, to);
591 if (violation) {
592 metrics.event("block", { source, what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), mode: settings.zone });
593 machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source });
594 if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(violation)}`, violation } };
595 }
596 const speed = number(body.speed, 300, 20, speedLimits().speed, "speed");
597 if (typeof body.speed === "number" && body.speed > speed) machine.note("clamp", "speed limit", { asked: body.speed, sent: speed, source });
598 const result = await moveToPose(bus, targetIds, positions, {
599 speed,
600 torquePercent: number(body.torque, 70, 10, 100, "torque"),
601 torqueCaps: torqueCaps(),
602 signal: controller.signal,
603 });
604 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, targetIds.map((id) => [id, 0] as const));
605 metrics.counters.move++;
606 const adjusted = new Map(result.adjusted);
607 targetIds.forEach((id, i) => positions[i] !== requested[i] && adjusted.set(id, positions[i]!));
608 return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(adjusted), ...(violation ? { warning: describeViolation(violation) } : {}) } };
609 } catch (err) {
610 if (err instanceof MotionError) {
611 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "move", { source, error: err.message, stalled: err.stalled, missing: err.missing });
612 return { status: 409, body: { reached: false, error: err.message, stalled: err.stalled, missing: err.missing } };
613 }
614 throw err;
615 } finally {
616 release();
617 }
618 };
619
620 // ------------------------------------------------------------------ recording
621 type Recorded = { trajectory: Trajectory; filled: number } | { error: string };
622 let recording: { name: string; hz: number; done: Promise<Recorded> } | null = null;
623 const finishRecording = async () => {
624 if (!recording || !current) throw new HttpError(409, "nothing is being recorded");
625 const { name, done } = recording;
626 const controller = current;
627 recording = null; // taken: a second caller, racing this one, finds nothing to stop
628 controller.abort();
629 const result = await done;
630 release();
631 if ("error" in result) throw new HttpError(500, `recording "${name}" failed: ${result.error}; nothing saved`);
632 const { trajectory, filled } = result;
633 if (!trajectory.frames.length) throw new HttpError(409, "nothing recorded; did the servos answer?");
634 writeAtomic(recordingPath(name), serializeTrajectory(trajectory));
635 metrics.event("record", { name, frames: trajectory.frames.length, seconds: trajectory.frames.length / trajectory.hz });
636 return { saved: name, frames: trajectory.frames.length, seconds: Number((trajectory.frames.length / trajectory.hz).toFixed(1)), filled };
637 };
638
639 // ------------------------------------------------------------------ routes
640 const viewerPath = join(here, "viewer.html");
641 const uiDir = join(here, "ui");
642 /** The page's own modules, under a name that changes with their content: cached for good, and a deploy is seen at once. */
643 const uiVersion = () => {
644 const hash = createHash("sha256");
645 for (const f of readdirSync(uiDir).filter(isUiModule).sort()) hash.update(f).update(readFileSync(join(uiDir, f)));
646 return hash.digest("hex").slice(0, 12);
647 };
648 const nodeModules = resolve(here, "../node_modules");
649 const http = createServer(async (req, res) => {
650 try {
651 const url = new URL(req.url ?? "/", "http://x");
652 const route = `${req.method} ${url.pathname}`;
653 const decode = (text: string) => {
654 try {
655 return decodeURIComponent(text);
656 } catch {
657 return "\0"; // never a file
658 }
659 };
660 if (req.method === "GET" && url.pathname.startsWith("/model/") && modelDir) {
661 const path = safeFile(resolve(modelDir), decode(url.pathname.slice("/model/".length)));
662 if (!path || ![".urdf", ".stl", ".dae", ".png", ".jpg"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such model file" });
663 return file(res, path, "max-age=86400");
664 }
665 if (req.method === "GET" && url.pathname === "/printables") {
666 let files: string[] = [];
667 try {
668 files = printablesDir ? readdirSync(printablesDir).filter((f) => f.endsWith(".png")).sort() : [];
669 } catch {
670 /* none */
671 }
672 return json(res, 200, files.map((f) => ({ file: f, url: `/printables/${f}`, bytes: statSync(join(printablesDir!, f)).size })));
673 }
674 if (req.method === "GET" && url.pathname.startsWith("/printables/") && printablesDir) {
675 const path = safeFile(resolve(printablesDir), decode(url.pathname.slice("/printables/".length)));
676 if (!path || extname(path).toLowerCase() !== ".png") return json(res, 404, { error: "no such printable" });
677 return file(res, path, "max-age=3600");
678 }
679 if (req.method === "GET" && url.pathname.startsWith("/vendor/")) {
680 const [pkg, ...rest] = decode(url.pathname.slice("/vendor/".length)).split("/");
681 const path = pkg && VENDORED.has(pkg) && rest.length ? safeFile(join(nodeModules, pkg), rest.join("/")) : null;
682 if (!path || ![".js", ".mjs", ".json"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such vendored file" });
683 return file(res, path, "max-age=86400");
684 }
685 const ui = /^\/ui\/[0-9a-f]{12}\/([a-z][a-z0-9-]*\.js)$/.exec(url.pathname);
686 if (req.method === "GET" && ui) {
687 if (!isUiModule(ui[1]!) || !existsSync(join(uiDir, ui[1]!))) return json(res, 404, { error: "no such module" });
688 return file(res, join(uiDir, ui[1]!), "public, max-age=31536000, immutable");
689 }
690 if (req.method === "GET" && url.pathname.startsWith("/app/")) {
691 const served = await browserModule(decode(url.pathname.slice("/app/".length)));
692 if (!served) return json(res, 404, { error: "no such module" });
693 if (req.headers["if-none-match"] === served.etag) {
694 res.writeHead(304, { etag: served.etag, "cache-control": "no-cache" });
695 return res.end();
696 }
697 res.writeHead(200, { "content-type": "text/javascript; charset=utf-8", "cache-control": "no-cache", etag: served.etag });
698 return res.end(served.js);
699 }
700 if (req.method === "PUT" && url.pathname.startsWith("/calibration/")) {
701 const kind = url.pathname.slice("/calibration/".length);
702 if (!/^[a-z][a-z0-9_-]{0,31}$/.test(kind)) throw new HttpError(400, "calibration kind: lowercase letters, digits, - and _");
703 const body = await readBody(req);
704 writeAtomic(join(calibrationDir, `${kind}.json`), JSON.stringify({ ...body, saved: new Date().toISOString() }, null, 1));
705 return json(res, 200, { stored: kind });
706 }
707 switch (route) {
708 case "GET /":
709 case "GET /index.html":
710 res.writeHead(200, { "content-type": "text/html; charset=utf-8", "cache-control": "no-store" });
711 return res.end(readFileSync(viewerPath, "utf8").replaceAll("/ui/__VERSION__/", `/ui/${uiVersion()}/`)); // read per request: a viewer change deploys with rsync alone
712 case "GET /leader-map.js":
713 return file(res, join(here, "leader-map.js"), "no-cache");
714 case "GET /status":
715 return json(res, 200, await status());
716 case "GET /limits":
717 return json(res, 200, Object.fromEntries(limits));
718 case "GET /poses":
719 return json(res, 200, loadPoses());
720 case "GET /recordings":
721 return json(res, 200, listRecordings());
722 case "GET /calibration":
723 return json(res, 200, loadCalibration());
724 case "GET /metrics":
725 return json(res, 200, metrics.summary());
726 case "GET /settings":
727 return json(res, 200, { ...settings, model: model !== null });
728 case "PUT /settings": {
729 const body = await readBody(req);
730 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => {
731 const patch = body[key];
732 return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]);
733 };
734 settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg") } as Partial<Settings>);
735 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
736 return json(res, 200, { ...settings, model: model !== null });
737 }
738 case "GET /model.json":
739 if (!model) return json(res, 404, { error: "no model loaded on this server" });
740 return json(res, 200, {
741 urdf: `/model/${URDF}`,
742 floorZ: model.floorZ,
743 baseTop: model.baseTop,
744 joints: model.movable.map((j) => ({ name: j.name, id: Number(Object.entries(JOINTS).find(([, n]) => n === j.name)?.[0] ?? 0), lower: j.lower, upper: j.upper })),
745 });
746 case "POST /pose/save": {
747 const body = await readBody(req);
748 const name = poseNamed(body.name);
749 const wanted = idList(body.ids);
750 const positions = await bus.syncRead(wanted, STS3215.Present_Position);
751 const missing = wanted.filter((id) => !positions.has(id));
752 if (missing.length) throw new HttpError(409, `no response from servo ${missing.join(", ")}`);
753 const pose = { ids: wanted, positions: wanted.map((id) => positions.get(id)!), saved: new Date().toISOString() };
754 writeAtomic(posesFile, serializePoseFile({ ...loadPoses(), [name]: pose }));
755 const onStops = wanted.filter((id) => {
756 const lim = limits.get(id)!;
757 return positions.get(id)! - lim.min < 40 || lim.max - positions.get(id)! < 40;
758 });
759 return json(res, 200, { name, ...pose, onStops });
760 }
761 case "POST /pose/delete": {
762 const name = poseNamed((await readBody(req)).name);
763 const poses = loadPoses();
764 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
765 const { [name]: _, ...rest } = poses;
766 writeAtomic(posesFile, serializePoseFile(rest));
767 return json(res, 200, { deleted: name });
768 }
769 case "POST /pose/go": {
770 const body = await readBody(req);
771 const name = poseNamed(body.name);
772 const poses = loadPoses();
773 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
774 const pose = poses[name]!;
775 const result = await move([...pose.ids], [...pose.positions], body, `pose ${name}`);
776 return json(res, result.status, result.body);
777 }
778 case "POST /move": {
779 const body = await readBody(req);
780 const targets = body.targets;
781 if (typeof targets !== "object" || targets === null || Array.isArray(targets)) throw new HttpError(400, "targets must be an object of id: ticks");
782 const targetIds: number[] = [];
783 const positions: number[] = [];
784 for (const [key, value] of Object.entries(targets as Record<string, unknown>)) {
785 const id = Number(key);
786 if (!/^\d+$/.test(key) || !ids.includes(id)) throw new HttpError(400, `no servo ${key} on this arm`);
787 if (!Number.isInteger(value)) throw new HttpError(400, `target for servo ${key} must be an integer`);
788 targetIds.push(id);
789 positions.push(value as number);
790 }
791 if (!targetIds.length) throw new HttpError(400, "targets is empty");
792 const result = await move(targetIds, positions, body, "move");
793 return json(res, result.status, result.body);
794 }
795 case "POST /grip": {
796 // Not supervised on purpose: a supervised close stops at the first contact and holds with no force.
797 // The servo's own torque cap does the squeezing, and its overload protection holds a steady grip.
798 const body = await readBody(req);
799 const id = 6;
800 const lim = limits.get(id);
801 if (!lim) throw new HttpError(400, "this arm has no gripper (servo 6)");
802 if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer");
803 const position = bound(bounds(), id, body.position as number)!;
804 const torque = Math.min(number(body.torque, 30, 10, 60, "torque"), settings.gripperTorquePercent); // the force limit, whatever is asked
805 const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), speedLimits().speed); // and the speed limit
806 if (position !== body.position) machine.note("clamp", "joint limit", { id, asked: body.position as number, sent: position, source: "grip" });
807 if (typeof body.torque === "number" && body.torque > torque) machine.note("clamp", "gripper force", { asked: body.torque, sent: torque, source: "grip" });
808 claim("move");
809 try {
810 const present = await bus.syncRead([id], STS3215.Present_Position);
811 if (!present.has(id)) throw new HttpError(409, "no response from the gripper");
812 await bus.syncWrite(STS3215.Goal_Position, [[id, present.get(id)!]]);
813 await bus.syncWrite(STS3215.Torque_Limit, [[id, Math.round(torque * 10)]]); // stays low while it holds; the next supervised gripper move puts the ceiling back
814 await bus.syncWrite(STS3215.Goal_Velocity, [[id, speed]]);
815 await bus.syncWrite(STS3215.Torque_Enable, [[id, 1]]);
816 await bus.syncWrite(STS3215.Goal_Position, [[id, position]]);
817 } finally {
818 release();
819 }
820 return json(res, 200, { gripping: position, torque });
821 }
822 case "POST /torque": {
823 const { on } = await readBody(req);
824 if (typeof on !== "boolean") throw new HttpError(400, "on must be true or false");
825 if (estop) throw new HttpError(423, stopped(estop));
826 if (gate.held && !on) throw new HttpError(423, holding(gate.held));
827 if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`);
828 if (on) {
829 const positions = await bus.syncRead(ids, STS3215.Present_Position);
830 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
831 }
832 await bus.syncWrite(STS3215.Torque_Enable, ids.map((id) => [id, on ? 1 : 0] as const));
833 return json(res, 200, { torque: on });
834 }
835 case "POST /stop": {
836 const body = await readBody(req);
837 // Held first, before anything awaits: a frame already on its way finds the gate shut.
838 if (body.hold !== false && gate.hold(actor(body))) metrics.event("hold", { by: actor(body) });
839 const was = driver;
840 if (was === "stream") for (const client of wss.clients) end(client, 1000, "stopped from the viewer"); // its close handler holds the arm
841 else if (was === "record") {
842 const summary = await finishRecording();
843 return json(res, 200, { stopped: was, ...summary });
844 } else current?.abort(); // the supervisor stops the move where it is
845 return json(res, 200, { stopped: was });
846 }
847 case "POST /resume": {
848 const by = actor(await readBody(req));
849 const resumed = gate.resume();
850 if (resumed) metrics.event("resume", { by });
851 return json(res, 200, { resumed, held: null });
852 }
853 case "POST /estop": {
854 const by = actor(await readBody(req));
855 const was = driver;
856 return json(res, 200, { estop: await engageEStop(by), stopped: was });
857 }
858 case "POST /estop/clear": {
859 return json(res, 200, { cleared: clearEStop(actor(await readBody(req))) });
860 }
861 case "POST /record/start": {
862 const body = await readBody(req);
863 const name = poseNamed(body.name);
864 const hz = number(body.hz, 30, 1, 100, "hz");
865 const wanted = idList(body.ids);
866 if (Object.hasOwn(listRecordings(), name)) throw new HttpError(409, `recording "${name}" already exists; pick another name`);
867 const controller = claim("record");
868 const done: Promise<Recorded> = recordTrajectory(bus, wanted, { hz, signal: controller.signal }).then(
869 (r) => r,
870 (err: unknown) => ({ error: err instanceof Error ? err.message : String(err) }), // reported by /record/stop, never unhandled
871 );
872 recording = { name, hz, done };
873 return json(res, 200, { recording: name, hz, ids: wanted });
874 }
875 case "POST /record/stop":
876 return json(res, 200, await finishRecording());
877 case "POST /recording/delete": {
878 const name = poseNamed((await readBody(req)).name);
879 if (recording?.name === name) throw new HttpError(409, `"${name}" is being recorded right now`);
880 try {
881 unlinkSync(recordingPath(name));
882 } catch (err) {
883 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
884 throw err;
885 }
886 return json(res, 200, { deleted: name });
887 }
888 case "POST /play": {
889 const body = await readBody(req);
890 const name = poseNamed(body.name);
891 const recorded = loadRecording(name);
892 // Every frame inside the joints' bounds, and the whole playback no faster than the speed limit allows.
893 const b = bounds();
894 const trajectory: Trajectory = { ...recorded, frames: recorded.frames.map((f) => f.map((p, i) => bound(b, recorded.ids[i]!, p) ?? p)) };
895 const step = largestStep(trajectory);
896 const asked = number(body.rate, 1, 0.1, 2, "rate");
897 const rate = step > 0 ? Math.min(asked, speedLimits().speed / (step * trajectory.hz)) : asked;
898 if (rate < asked) machine.note("clamp", "speed limit", { asked, sent: Number(rate.toFixed(3)), source: `play ${name}` });
899 const controller = claim("play");
900 try {
901 if (model && settings.zone !== "off") {
902 // The way to the first frame, then every frame (sampled when the recording is long), each judged as the whole
903 // pose with the joints the recording does not drive left where they are. Nothing moves before this passes.
904 const present = await bus.syncRead(ids, STS3215.Present_Position);
905 const poseAt = (frame: readonly number[]) => {
906 const pose = new Map(present);
907 trajectory.ids.forEach((id, i) => pose.set(id, frame[i]!));
908 return pose;
909 };
910 const refuse = (v: Violation, where: string): boolean => {
911 metrics.event("block", { source: `play ${name}`, where, what: v.what, link: v.link, depthMm: Math.round(v.depth * 1000), mode: settings.zone });
912 machine.note("clamp", "zone", { what: v.what, link: v.link, depthMm: Math.round(v.depth * 1000), refused: settings.zone === "enforce", source: `play ${name}` });
913 if (settings.zone !== "enforce") return false;
914 json(res, 409, { played: 0, error: `refused: ${where}: ${describeViolation(v)}`, violation: v });
915 return true;
916 };
917 const onTheWay = checkPath(present, poseAt(trajectory.frames[0]!));
918 if (onTheWay && refuse(onTheWay, "on the way to the first frame")) return;
919 const stride = Math.max(1, Math.ceil(trajectory.frames.length / 3000));
920 for (let index = 0; index < trajectory.frames.length; index += stride) {
921 const v = model.check(toQ(poseAt(trajectory.frames[index]!)), zoneSettings());
922 if (v) {
923 if (refuse(v, `frame ${index} of ${trajectory.frames.length}`)) return;
924 break;
925 }
926 }
927 }
928 const played = await playTrajectory(bus, trajectory, {
929 rate,
930 torquePercent: number(body.torque, 70, 10, 100, "torque"),
931 torqueCaps: torqueCaps(),
932 signal: controller.signal,
933 });
934 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, trajectory.ids.map((id) => [id, 0] as const));
935 metrics.event("play", { name, played, frames: trajectory.frames.length, rate });
936 return json(res, 200, { played, frames: trajectory.frames.length, rate: Number(rate.toFixed(3)) });
937 } catch (err) {
938 if (err instanceof MotionError) {
939 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "play", { source: `play ${name}`, error: err.message, stalled: err.stalled, missing: err.missing });
940 return json(res, 409, { played: 0, error: err.message, stalled: err.stalled, missing: err.missing });
941 }
942 throw err;
943 } finally {
944 release();
945 }
946 }
947 }
948 json(res, 404, { error: "no such route" });
949 } catch (err) {
950 if (err instanceof HttpError) return json(res, err.status, { error: err.message });
951 json(res, 500, { error: err instanceof Error ? err.message : String(err) });
952 }
953 });
954
955 // ------------------------------------------------------------------ the stream
956 // Frames are merged down to the command rate; each joint's target is pulled inside its bounds, judged by the zone,
957 // and handed to the governor, which moves the goals toward it within the speed and acceleration limits.
958 // Silence makes the arm hold (the dead-man); a connection that is lost rather than closed makes it park.
959 const wss = new WebSocketServer({ noServer: true, maxPayload: 4096 });
960 /** Each stream's halt: what it has queued or under way stops at once, not when the close handshake comes back. */
961 const halts = new Map<WebSocket, () => void>();
962 const end = (ws: WebSocket, code: number, why: string) => {
963 halts.get(ws)?.();
964 ws.close(code, why);
965 };
966 /** Each sender's clock, by address: one device keeps one clock, so a burst it flushes on a new stream is still known as old. */
967 const clocks = new Map<string, Clock>();
968 const clock = (req: IncomingMessage | undefined) => {
969 const key = req?.socket.remoteAddress ?? "";
970 if (!clocks.has(key) && clocks.size >= 64) clocks.delete(clocks.keys().next().value!);
971 const c = clocks.get(key) ?? new Clock();
972 clocks.set(key, c);
973 return c;
974 };
975 wss.on("connection", (ws: WebSocket, req?: IncomingMessage) => {
976 if (estop) {
977 ws.close(ESTOP_CLOSE, stopped(estop));
978 return;
979 }
980 if (gate.held) {
981 ws.close(HELD_CLOSE, holding(gate.held));
982 return;
983 }
984 if (driver) {
985 ws.close(1013, `${describe(driver)} is driving the arm`);
986 return;
987 }
988 claim("stream");
989 const startedAt = Date.now();
990 const stats = { frames: 0, dropped: 0, refused: 0, merged: 0, clamped: 0, gated: 0 };
991 /** Seq, age and the hold, frame by frame; stamps are optional here (the CLI leader sends none). */
992 const admission = gate.open(clock(req), false);
993 /** Set the moment the stream ends: whatever is queued or under way gives up, and nothing more reaches the bus. */
994 let closed = false;
995 streamStats = stats;
996 const holdAfterMs = options.watchdogMs ?? settings.holdAfterMs;
997 const period = 1000 / settings.rateHz;
998 const governor = new Governor(bounds, speedLimits);
999 let last: Map<number, number> | null = null;
1000 /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */
1001 const drops = new Map<number, number>();
1002 /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */
1003 let heldDepth = 0;
1004 /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */
1005 let present = new Map<number, number>();
1006 let armed = false;
1007 let arming = false;
1008 let timer: ReturnType<typeof setTimeout> | undefined;
1009 let chain: Promise<void> = Promise.resolve();
1010 const run = (fn: () => Promise<void>) => (chain = chain.then(fn).catch((err: unknown) => console.error(`stream: ${err instanceof Error ? err.message : String(err)}`)));
1011
1012 const hold = () =>
1013 run(async () => {
1014 if (closed) return;
1015 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1016 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
1017 governor.reset(positions);
1018 last = null;
1019 held = true;
1020 machine.note("state", "dead-man hold", { afterMs: holdAfterMs });
1021 });
1022 const arm = () =>
1023 run(async () => {
1024 if (closed) return;
1025 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1026 present = new Map(positions);
1027 const ready = ids.filter((id) => positions.has(id));
1028 await bus.syncWrite(STS3215.Goal_Position, ready.map((id) => [id, positions.get(id)!] as const));
1029 await bus.syncWrite(STS3215.Torque_Limit, ready.map((id) => [id, Math.round((id === 6 ? Math.min(streamTorque, settings.gripperTorquePercent) : streamTorque) * 10)] as const));
1030 await servoLimits(ready);
1031 await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const));
1032 governor.reset(positions);
1033 last = null; // the first frame eases the arm over, however far away the leader is
1034 heldDepth = evaluate(positions)?.violation?.depth ?? 0;
1035 armed = true;
1036 });
1037 let easing = false;
1038
1039 // The jam watch: a joint that pushes hard without moving toward its target is parked where it is.
1040 const windows = new Map<number, Array<{ p: number; l: number }>>();
1041 const observe = async (id: number, p: number, l: number, target: number | undefined): Promise<boolean> => {
1042 present.set(id, p);
1043 const w = windows.get(id) ?? [];
1044 w.push({ p, l });
1045 if (w.length > JAM_WINDOW) w.shift();
1046 windows.set(id, w);
1047 if (jams.has(id) || w.length < JAM_WINDOW || target === undefined) return jams.has(id);
1048 const pushing = w.every((s) => Math.abs(s.l) >= JAM_LOAD * 10);
1049 const travel = Math.max(...w.map((s) => s.p)) - Math.min(...w.map((s) => s.p));
1050 if (!(pushing && travel < STALL_SPAN_TICKS && Math.abs(target - p) > 30)) return false;
1051 await bus.syncWrite(STS3215.Goal_Position, [[id, p]]);
1052 governor.pin(id, p);
1053 jams.set(id, { position: p, direction: Math.sign(target - p) });
1054 last?.set(id, p);
1055 metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10 });
1056 machine.note("clamp", "jam", { id, position: p, target, load: l / 10 });
1057 return true;
1058 };
1059 const watch = setInterval(
1060 () =>
1061 void run(async () => {
1062 if (closed || !armed || last === null) return;
1063 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1064 const loads = await bus.syncRead(ids, STS3215.Present_Load);
1065 for (const id of ids) {
1066 const p = positions.get(id), l = loads.get(id);
1067 if (p !== undefined && l !== undefined) await observe(id, p, l, last.get(id));
1068 }
1069 }),
1070 jamSampleMs,
1071 );
1072
1073 // The governor's clock: at the command rate, the goals move one step toward the targets.
1074 let ticking = false;
1075 let lastTick = performance.now();
1076 const ticker = setInterval(() => {
1077 const now = performance.now();
1078 const dt = Math.min(now - lastTick, 2 * period) / 1000; // a late tick never turns into a jump
1079 lastTick = now;
1080 if (!armed || easing || ticking || !governor.moving) return;
1081 ticking = true;
1082 void run(async () => {
1083 try {
1084 if (closed) return;
1085 const step = governor.step(dt);
1086 if (step.length) await bus.syncWrite(STS3215.Goal_Position, step);
1087 } finally {
1088 ticking = false;
1089 }
1090 });
1091 }, period);
1092
1093 // The heartbeat: a client that does not answer a ping before the next one is gone, however quiet its socket.
1094 let answered = true;
1095 ws.on("pong", () => (answered = true));
1096 const heartbeat = setInterval(() => {
1097 if (!answered) return ws.terminate();
1098 answered = false;
1099 ws.ping();
1100 }, heartbeatMs);
1101
1102 const follow = (frame: ReadonlyMap<number, number>) =>
1103 run(async () => {
1104 if (closed) return; // queued before a stop: dropped, never replayed
1105 const b = bounds();
1106 const targets: Array<readonly [number, number]> = [];
1107 let ease = last === null; // after a hold, everything eases
1108 for (const [id, value] of frame) {
1109 const target = bound(b, id, value);
1110 if (target === null) continue;
1111 if (target !== Math.round(value)) {
1112 stats.clamped++;
1113 machine.note("clamp", "joint limit", { id, asked: Math.round(value), sent: target, source: "stream" });
1114 }
1115 const jam = jams.get(id);
1116 if (jam) {
1117 if ((target - jam.position) * jam.direction > -10) continue; // still pushing into the obstacle: wait for the leader to back off
1118 jams.delete(id);
1119 windows.delete(id);
1120 }
1121 const previous = last?.get(id);
1122 if (previous === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped
1123 else if (Math.abs(target - previous) > maxStep) {
1124 stats.dropped++;
1125 machine.note("clamp", "too far in one frame", { id, asked: target, from: previous, maxStep });
1126 const n = (drops.get(id) ?? 0) + 1;
1127 drops.set(id, n);
1128 if (n < RESUME_AFTER_DROPS) continue; // no hand moves that far in one frame
1129 // The leader has stayed far away (after a block or a jam): ease over to it rather than leave the joint stranded.
1130 drops.delete(id);
1131 ease = true;
1132 } else drops.delete(id);
1133 targets.push([id, target]);
1134 }
1135 if (!targets.length) return;
1136 stats.frames++;
1137 if (held) {
1138 held = false;
1139 machine.note("state", "following again", {});
1140 }
1141 // The zone: the frame is judged as the whole pose it would make, with uncommanded joints where they are.
1142 const pose = new Map(present);
1143 for (const [id, t] of last ?? []) pose.set(id, t);
1144 for (const [id, t] of targets) pose.set(id, t);
1145 const zone = evaluate(pose);
1146 if (zone?.violation) {
1147 if (!blocked) metrics.event("block", { source: "stream", what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), mode: settings.zone });
1148 blocked = zone.violation;
1149 if (settings.zone === "enforce" && zone.violation.depth > heldDepth + 0.001) {
1150 stats.refused++;
1151 machine.note("clamp", "zone", { what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), refused: true, source: "stream" });
1152 return; // deeper in than the arm already is: it stays at the last good targets; a frame that backs out is let through
1153 }
1154 } else blocked = null;
1155 heldDepth = zone?.violation?.depth ?? 0;
1156 approach(zone);
1157 if (ease) {
1158 // Catch up slowly with every joint in this frame, the servos' own speed held low, then hand over to the governor.
1159 // A joint that jams on the way is parked and no longer waited for.
1160 easing = true;
1161 try {
1162 await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, speedLimits().speed)] as const));
1163 await bus.syncWrite(STS3215.Goal_Position, targets);
1164 const deadline = Date.now() + 4000;
1165 const wanted = new Map(targets);
1166 while (Date.now() < deadline && !closed) {
1167 const positions = await bus.syncRead([...wanted.keys()], STS3215.Present_Position);
1168 const loads = await bus.syncRead([...wanted.keys()], STS3215.Present_Load);
1169 let worst = 0;
1170 for (const [id, t] of wanted) {
1171 const p = positions.get(id), l = loads.get(id);
1172 if (p === undefined || l === undefined) continue;
1173 if (await observe(id, p, l, t)) continue; // jammed: parked, not waited for
1174 worst = Math.max(worst, Math.abs(p - t));
1175 }
1176 if (worst <= 25) break;
1177 await new Promise((r) => setTimeout(r, 50));
1178 }
1179 if (!closed) await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, speedLimits().speed] as const));
1180 } finally {
1181 easing = false;
1182 }
1183 last = new Map([...(last ?? []), ...targets]);
1184 for (const [id, t] of targets) governor.pin(id, jams.get(id)?.position ?? t);
1185 for (const [id, jam] of jams) last.set(id, jam.position);
1186 return;
1187 }
1188 for (const [id, t] of targets) {
1189 governor.aim(id, t);
1190 last!.set(id, t);
1191 }
1192 });
1193
1194 // Frames faster than the command rate are merged, the latest value per joint winning, and taken at the next slot.
1195 let pending: Map<number, number> | null = null;
1196 let pacer: ReturnType<typeof setTimeout> | undefined;
1197 let lastTaken = 0;
1198 const take = () => {
1199 pacer = undefined;
1200 if (!pending || closed) return;
1201 // While the arm catches up, the frame waits (later ones merge into it) and is taken once it has: never dropped,
1202 // or a leader that sends one last frame and stops would leave the arm short of it.
1203 if (easing) return void (pacer = setTimeout(take, period));
1204 const frame = pending;
1205 pending = null;
1206 lastTaken = Date.now();
1207 follow(frame);
1208 };
1209 ws.on("message", (data) => {
1210 if (closed) return;
1211 const verdict = admission.admit(String(data));
1212 if (!verdict.ok) {
1213 if (verdict.drop !== "malformed") stats.gated++;
1214 return;
1215 }
1216 const frame = verdict.joints;
1217 clearTimeout(timer);
1218 timer = setTimeout(() => void hold(), holdAfterMs);
1219 if (!arming) {
1220 arming = true;
1221 void arm();
1222 }
1223 if (pending) stats.merged++;
1224 pending = new Map([...(pending ?? []), ...frame]);
1225 pacer ??= setTimeout(take, Math.max(0, lastTaken + period - Date.now()));
1226 });
1227 const halt = () => {
1228 closed = true;
1229 pending = null;
1230 clearTimeout(timer);
1231 clearTimeout(pacer);
1232 clearInterval(watch);
1233 clearInterval(ticker);
1234 };
1235 halts.set(ws, halt);
1236 ws.on("close", (code) => {
1237 halt();
1238 halts.delete(ws);
1239 admission.close();
1240 clearInterval(heartbeat);
1241 const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE;
1242 void run(async () => {
1243 try {
1244 if (armed) {
1245 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1246 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
1247 await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, id === 6 ? settings.gripperTorquePercent * 10 : 1000] as const));
1248 }
1249 } finally {
1250 blocked = null;
1251 held = false;
1252 jams.clear();
1253 streamStats = null;
1254 metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats, ...(lost ? { lost: code } : {}) });
1255 release(); // whatever the bus did, the arm is free for the next driver
1256 }
1257 if (lost && armed) void park(`the stream was lost (${code})`);
1258 });
1259 });
1260 });
1261
1262 // ------------------------------------------------------------------ the watch
1263 // Read-only: the status, pushed. One timer reads the bus while anyone watches; a watcher's messages are ignored.
1264 const watchers = new WebSocketServer({ noServer: true, maxPayload: 1024 });
1265 const watchEveryMs = options.watchEveryMs ?? 250;
1266 let watching: ReturnType<typeof setInterval> | undefined;
1267 let reading = false;
1268 const machineFrame = (entries: readonly unknown[]) => JSON.stringify({ machine: entries, run });
1269 const broadcast = async () => {
1270 if (reading) return; // a slow bus read is not stacked behind another: the next tick asks again
1271 reading = true;
1272 try {
1273 const text = JSON.stringify(await status());
1274 for (const w of watchers.clients) {
1275 if (w.readyState !== w.OPEN) continue;
1276 w.send(text);
1277 // A new watcher's first message is a status; the machine log so far comes right after it.
1278 if (fresh.delete(w)) w.send(machineFrame(machine.recent(200)));
1279 }
1280 } catch (err) {
1281 console.error(`watch: ${err instanceof Error ? err.message : String(err)}`);
1282 } finally {
1283 reading = false;
1284 }
1285 };
1286 /** The machine log goes out as it is written, while anyone watches; a watcher yet to get its first status gets it all then. */
1287 let unsubscribe: (() => void) | undefined;
1288 const fresh = new WeakSet<WebSocket>();
1289 watchers.on("connection", (ws: WebSocket) => {
1290 watching ??= setInterval(() => void broadcast(), watchEveryMs);
1291 unsubscribe ??= machine.subscribe((batch) => {
1292 const text = machineFrame(batch);
1293 for (const w of watchers.clients) if (w.readyState === w.OPEN && !fresh.has(w)) w.send(text);
1294 });
1295 fresh.add(ws);
1296 void broadcast();
1297 ws.on("error", () => ws.terminate());
1298 ws.on("close", () => {
1299 if (watchers.clients.size > 0 || !watching) return;
1300 clearInterval(watching);
1301 watching = undefined;
1302 unsubscribe?.();
1303 unsubscribe = undefined;
1304 });
1305 });
1306
1307 http.on("upgrade", (req: IncomingMessage, socket: Duplex, head: Buffer) => {
1308 const path = new URL(req.url ?? "/", "http://x").pathname;
1309 const to = path === "/follow" ? wss : path === "/watch" ? watchers : null;
1310 if (!to) return void socket.end("HTTP/1.1 404 Not Found\r\nconnection: close\r\ncontent-length: 0\r\n\r\n");
1311 socket.on("error", () => socket.destroy());
1312 to.handleUpgrade(req, socket, head, (ws) => to.emit("connection", ws, req));
1313 });
73 const ctx = await createContext(bus, options);
74 const routes = { ...staticRoutes, ...statusRoutes, ...settingsRoutes, ...storageRoutes, ...moveRoutes, ...recordRoutes, ...safetyRoutes };
75 const http = createServer(createRouter(ctx, routes, [calibrationPut, ...staticPrefixes]));
76 const wss = attachStream(ctx);
77 const watch = attachWatch(ctx);
78 attachUpgrades(http, { "/follow": wss, "/watch": watch.watchers });
131479
131580 await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve));
131681 const address = http.address();
@@ -1319,12 +84,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
131984 port,
132085 close: () =>
132186 new Promise<void>((resolve) => {
1322 clearInterval(watching);
1323 machine.close();
1324 bus.onSyncWrite = null;
1325 for (const client of [...wss.clients, ...watchers.clients]) client.terminate();
87 watch.stop();
88 for (const client of [...wss.clients, ...watch.watchers.clients]) client.terminate();
132689 wss.close();
1327 watchers.close();
90 watch.watchers.close();
132891 http.close(() => resolve());
132992 http.closeAllConnections(); // the viewer keeps idle keep-alive connections open; close() alone would wait for them
133093 }),
Software/so101/src/server/clocks.ts added +15
@@ -0,0 +1,15 @@
1 /** Each sender's clock, by address: one device keeps one clock, so a burst it flushes on a new stream is still known as old. */
2
3 import type { IncomingMessage } from "node:http";
4 import { Clock } from "../command-gate.ts";
5
6 export function senderClocks(limit = 64): (req: IncomingMessage | undefined) => Clock {
7 const clocks = new Map<string, Clock>();
8 return (req) => {
9 const key = req?.socket.remoteAddress ?? "";
10 if (!clocks.has(key) && clocks.size >= limit) clocks.delete(clocks.keys().next().value!);
11 const c = clocks.get(key) ?? new Clock();
12 clocks.set(key, c);
13 return c;
14 };
15 }
Software/so101/src/server/context.ts added +76
@@ -0,0 +1,76 @@
1 /** Builds the context one arm server runs on: its paths, its limits read off the servos, its model, its settings and its stored stop. */
2
3 import { randomUUID } from "node:crypto";
4 import { dirname, join, resolve } from "node:path";
5 import type { FeetechBus } from "../bus.ts";
6 import { CommandGate } from "../command-gate.ts";
7 import { MachineLog, perServo } from "../machine-log.ts";
8 import { Metrics } from "../metrics.ts";
9 import { validateSafety, type SafetySettings } from "../safety.ts";
10 import { registerName, STS3215 } from "../sts3215.ts";
11 import { loadEStop } from "./safety.ts";
12 import { loadSettings } from "./settings.ts";
13 import type { Ctx, ServerOptions } from "./types.ts";
14 import { loadModel } from "./zone.ts";
15
16 export async function createContext(bus: FeetechBus, options: ServerOptions): Promise<Ctx> {
17 const posesFile = resolve(options.posesFile ?? "poses.json");
18 const dataDir = dirname(posesFile);
19 const estopFile = join(dataDir, "estop.json");
20 const settingsFile = join(dataDir, "settings.json");
21 const ids = options.ids ?? [1, 2, 3, 4, 5, 6];
22 let safetyBase: SafetySettings;
23 try {
24 safetyBase = validateSafety(options.safety ?? {});
25 } catch (err) {
26 throw new Error(`safety options: ${(err as Error).message}`);
27 }
28 const limits = new Map<number, { min: number; max: number }>();
29 for (const id of ids) {
30 limits.set(id, {
31 min: await bus.read(id, STS3215.Min_Position_Limit),
32 max: await bus.read(id, STS3215.Max_Position_Limit),
33 });
34 }
35 const machine = new MachineLog();
36 const goals = new Map<number, number>();
37 bus.onSyncWrite = (register, values) => {
38 if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v);
39 machine.note("sent", registerName(register), { values: perServo(values) });
40 };
41 return {
42 bus,
43 options,
44 ids,
45 posesFile,
46 recordingsDir: join(dataDir, "recordings"),
47 calibrationDir: join(dataDir, "calibration"),
48 settingsFile,
49 estopFile,
50 streamTorque: options.streamTorque ?? 70,
51 maxStep: options.maxStep ?? 400,
52 easeSpeed: options.easeSpeed ?? 300,
53 jamSampleMs: options.jamSampleMs ?? 100,
54 watchEveryMs: options.watchEveryMs ?? 250,
55 heartbeatMs: options.heartbeatMs ?? 1000,
56 safetyBase,
57 metrics: new Metrics(join(dataDir, "metrics.jsonl")),
58 machine,
59 gate: new CommandGate(),
60 halts: new Map(),
61 runId: randomUUID(),
62 goals,
63 limits,
64 ...loadModel(options),
65 settings: loadSettings(settingsFile, safetyBase),
66 driver: null,
67 current: null,
68 estop: loadEStop(estopFile),
69 recording: null,
70 blocked: null,
71 held: false,
72 jams: new Map(),
73 streamStats: null,
74 wss: null,
75 };
76 }
Software/so101/src/server/driver.ts added +34
@@ -0,0 +1,34 @@
1 /** Who drives the arm: one thing at a time, and nothing at all while an emergency stop is in force. */
2
3 import type { WebSocket } from "ws";
4 import { HttpError } from "./http.ts";
5 import type { Ctx, Driver, EStop } from "./types.ts";
6
7 export const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d];
8
9 export const holding = (h: { by: string; at: string }) => `stopped (${h.by}, ${h.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until someone resumes`;
10
11 export const stopped = (e: EStop) => `emergency stop (${e.by}, ${e.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until an admin clears it`;
12
13 export const claim = (ctx: Ctx, kind: Driver): AbortController => {
14 if (ctx.estop) throw new HttpError(423, stopped(ctx.estop));
15 if (ctx.gate.held) throw new HttpError(423, holding(ctx.gate.held));
16 if (ctx.driver) throw new HttpError(409, `${describe(ctx.driver)} is driving the arm`);
17 ctx.driver = kind;
18 ctx.current = new AbortController();
19 ctx.machine.note("state", "driver", { driving: kind });
20 return ctx.current;
21 };
22
23 export const release = (ctx: Ctx) => {
24 if (ctx.driver) ctx.machine.note("state", "driver", { driving: null });
25 ctx.driver = null;
26 ctx.current = null;
27 };
28
29 /** Close a stream's socket after halting it: what it has queued or under way stops now, not when the close handshake comes back. */
30 export const end = (ctx: Ctx, ws: WebSocket, code: number, why: string) => {
31 ctx.halts.get(ws)?.();
32 ws.close(code, why);
33 };
34
Software/so101/src/server/http.ts added +90
@@ -0,0 +1,90 @@
1 /** The plumbing every route shares: answering in JSON, serving a file, reading a body, validating numbers and paths. */
2
3 import { mkdirSync, readFileSync, renameSync, statSync, writeFileSync } from "node:fs";
4 import type { IncomingMessage, ServerResponse } from "node:http";
5 import { dirname, extname, resolve } from "node:path";
6 import { clamp } from "../mapping.ts";
7
8 export class HttpError extends Error {
9 readonly status: number;
10 constructor(status: number, message: string) {
11 super(message);
12 this.status = status;
13 }
14 }
15
16 const MAX_BODY = 1_000_000;
17 const MIME: Record<string, string> = { ".js": "text/javascript", ".mjs": "text/javascript", ".json": "application/json", ".urdf": "application/xml", ".stl": "model/stl", ".dae": "model/vnd.collada+xml", ".png": "image/png", ".jpg": "image/jpeg" };
18
19 export const json = (res: ServerResponse, status: number, body: unknown) => {
20 res.writeHead(status, { "content-type": "application/json" });
21 res.end(JSON.stringify(body));
22 };
23
24 export const file = (res: ServerResponse, path: string, cache: string) => {
25 res.writeHead(200, { "content-type": MIME[extname(path).toLowerCase()] ?? "application/octet-stream", "cache-control": cache });
26 res.end(readFileSync(path));
27 };
28
29 export const readBody = (req: IncomingMessage) =>
30 new Promise<Record<string, unknown>>((resolve, reject) => {
31 let data = "";
32 req.on("data", (c) => {
33 data += c;
34 if (data.length > MAX_BODY) {
35 req.destroy();
36 reject(new HttpError(413, "body too large"));
37 }
38 });
39 req.on("end", () => {
40 if (!data.trim()) return resolve({});
41 try {
42 const parsed: unknown = JSON.parse(data);
43 if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) throw new Error();
44 resolve(parsed as Record<string, unknown>);
45 } catch {
46 reject(new HttpError(400, "body must be a JSON object"));
47 }
48 });
49 req.on("error", reject);
50 });
51
52 export const number = (value: unknown, fallback: number, min: number, max: number, what: string): number => {
53 if (value === undefined) return fallback;
54 if (typeof value !== "number" || !Number.isFinite(value)) throw new HttpError(400, `${what} must be a number`);
55 return clamp(value, min, max);
56 };
57
58 export const idList = (ids: readonly number[], value: unknown): number[] => {
59 if (value === undefined) return [...ids];
60 if (!Array.isArray(value) || !value.length || !value.every((id) => Number.isInteger(id) && ids.includes(id as number))) {
61 throw new HttpError(400, `ids must be a non-empty list drawn from ${ids.join(", ")}`);
62 }
63 return [...new Set(value as number[])];
64 };
65
66 export const writeAtomic = (path: string, text: string) => {
67 mkdirSync(dirname(path), { recursive: true });
68 writeFileSync(`${path}.tmp`, text);
69 renameSync(`${path}.tmp`, path);
70 };
71
72 /** A file under a root, or null for anything that tries to leave it. */
73 export const safeFile = (root: string, rel: string): string | null => {
74 if (rel.includes("\0") || rel.split("/").some((part) => part === "..")) return null;
75 const path = resolve(root, rel);
76 if (path !== root && !path.startsWith(root + "/")) return null;
77 try {
78 return statSync(path).isFile() ? path : null;
79 } catch {
80 return null;
81 }
82 };
83
84 export const decode = (text: string) => {
85 try {
86 return decodeURIComponent(text);
87 } catch {
88 return "\0"; // never a file
89 }
90 };
Software/so101/src/server/move.ts added +135
@@ -0,0 +1,135 @@
1 /** Supervised moves over HTTP: any joints, a saved pose, the gripper, torque, and stopping whatever drives the arm. */
2
3 import { MotionError, moveToPose } from "../motion.ts";
4 import { bound } from "../safety.ts";
5 import { STS3215 } from "../sts3215.ts";
6 import { claim, describe, end, holding, release, stopped } from "./driver.ts";
7 import { HttpError, json, number, readBody } from "./http.ts";
8 import { finishRecording } from "./record.ts";
9 import { actor, bounds, speedLimits, torqueCaps } from "./safety.ts";
10 import { loadPoses, poseNamed } from "./storage.ts";
11 import type { Ctx, Routes } from "./types.ts";
12 import { checkPath, describeViolation } from "./zone.ts";
13
14 /** A supervised move to explicit targets; the one way joints move over HTTP. */
15 const move = async (ctx: Ctx, targetIds: number[], requested: number[], body: Record<string, unknown>, source: string) => {
16 const { bus, metrics, settings } = ctx;

Small behaviour drift. This copies settings into a local when the request starts. The old code read the live settings after each await, so a PUT /settings that changes zone while syncRead is in flight used to apply to this move and now does not. The pull says no behaviour change, so read ctx.settings where it is used, as stream.ts already does.

17 const controller = claim(ctx, "move");
18 const b = bounds(ctx);
19 const positions = targetIds.map((id, i) => bound(b, id, requested[i]!) ?? requested[i]!);
20 targetIds.forEach((id, i) => positions[i] !== requested[i] && ctx.machine.note("clamp", "joint limit", { id, asked: requested[i]!, sent: positions[i]!, source }));
21 try {
22 const from = await bus.syncRead(ctx.ids, STS3215.Present_Position);
23 const to = new Map(from);
24 targetIds.forEach((id, i) => to.set(id, positions[i]!));
25 const violation = checkPath(ctx, from, to);
26 if (violation) {
27 metrics.event("block", { source, what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), mode: settings.zone });
28 ctx.machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source });
29 if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(ctx, violation)}`, violation } };
30 }
31 const speed = number(body.speed, 300, 20, speedLimits(ctx).speed, "speed");
32 if (typeof body.speed === "number" && body.speed > speed) ctx.machine.note("clamp", "speed limit", { asked: body.speed, sent: speed, source });
33 const result = await moveToPose(bus, targetIds, positions, {
34 speed,
35 torquePercent: number(body.torque, 70, 10, 100, "torque"),
36 torqueCaps: torqueCaps(ctx),
37 signal: controller.signal,
38 });
39 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, targetIds.map((id) => [id, 0] as const));
40 metrics.counters.move++;
41 const adjusted = new Map(result.adjusted);
42 targetIds.forEach((id, i) => positions[i] !== requested[i] && adjusted.set(id, positions[i]!));
43 return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(adjusted), ...(violation ? { warning: describeViolation(ctx, violation) } : {}) } };
44 } catch (err) {
45 if (err instanceof MotionError) {
46 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "move", { source, error: err.message, stalled: err.stalled, missing: err.missing });
47 return { status: 409, body: { reached: false, error: err.message, stalled: err.stalled, missing: err.missing } };
48 }
49 throw err;
50 } finally {
51 release(ctx);
52 }
53 };
54
55 export const moveRoutes: Routes = {
56 "POST /pose/go": async ({ ctx, req, res }) => {
57 const body = await readBody(req);
58 const name = poseNamed(body.name);
59 const poses = loadPoses(ctx);
60 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
61 const pose = poses[name]!;
62 const result = await move(ctx, [...pose.ids], [...pose.positions], body, `pose ${name}`);
63 return json(res, result.status, result.body);
64 },
65 "POST /move": async ({ ctx, req, res }) => {
66 const body = await readBody(req);
67 const targets = body.targets;
68 if (typeof targets !== "object" || targets === null || Array.isArray(targets)) throw new HttpError(400, "targets must be an object of id: ticks");
69 const targetIds: number[] = [];
70 const positions: number[] = [];
71 for (const [key, value] of Object.entries(targets as Record<string, unknown>)) {
72 const id = Number(key);
73 if (!/^\d+$/.test(key) || !ctx.ids.includes(id)) throw new HttpError(400, `no servo ${key} on this arm`);
74 if (!Number.isInteger(value)) throw new HttpError(400, `target for servo ${key} must be an integer`);
75 targetIds.push(id);
76 positions.push(value as number);
77 }
78 if (!targetIds.length) throw new HttpError(400, "targets is empty");
79 const result = await move(ctx, targetIds, positions, body, "move");
80 return json(res, result.status, result.body);
81 },
82 "POST /grip": async ({ ctx, req, res }) => {
83 // Not supervised on purpose: a supervised close stops at the first contact and holds with no force.
84 // The servo's own torque cap does the squeezing, and its overload protection holds a steady grip.
85 const { bus } = ctx;
86 const body = await readBody(req);
87 const id = 6;
88 const lim = ctx.limits.get(id);
89 if (!lim) throw new HttpError(400, "this arm has no gripper (servo 6)");
90 if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer");
91 const position = bound(bounds(ctx), id, body.position as number)!;
92 const torque = Math.min(number(body.torque, 30, 10, 60, "torque"), ctx.settings.gripperTorquePercent); // the force limit, whatever is asked
93 const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), speedLimits(ctx).speed); // and the speed limit
94 if (position !== body.position) ctx.machine.note("clamp", "joint limit", { id, asked: body.position as number, sent: position, source: "grip" });
95 if (typeof body.torque === "number" && body.torque > torque) ctx.machine.note("clamp", "gripper force", { asked: body.torque, sent: torque, source: "grip" });
96 claim(ctx, "move");
97 try {
98 const present = await bus.syncRead([id], STS3215.Present_Position);
99 if (!present.has(id)) throw new HttpError(409, "no response from the gripper");
100 await bus.syncWrite(STS3215.Goal_Position, [[id, present.get(id)!]]);
101 await bus.syncWrite(STS3215.Torque_Limit, [[id, Math.round(torque * 10)]]); // stays low while it holds; the next supervised gripper move puts the ceiling back
102 await bus.syncWrite(STS3215.Goal_Velocity, [[id, speed]]);
103 await bus.syncWrite(STS3215.Torque_Enable, [[id, 1]]);
104 await bus.syncWrite(STS3215.Goal_Position, [[id, position]]);
105 } finally {
106 release(ctx);
107 }
108 return json(res, 200, { gripping: position, torque });
109 },
110 "POST /torque": async ({ ctx, req, res }) => {
111 const { on } = await readBody(req);
112 if (typeof on !== "boolean") throw new HttpError(400, "on must be true or false");
113 if (ctx.estop) throw new HttpError(423, stopped(ctx.estop));
114 if (ctx.gate.held && !on) throw new HttpError(423, holding(ctx.gate.held));
115 if (ctx.driver) throw new HttpError(409, `${describe(ctx.driver)} is driving the arm`);
116 if (on) {
117 const positions = await ctx.bus.syncRead(ctx.ids, STS3215.Present_Position);
118 await ctx.bus.syncWrite(STS3215.Goal_Position, ctx.ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
119 }
120 await ctx.bus.syncWrite(STS3215.Torque_Enable, ctx.ids.map((id) => [id, on ? 1 : 0] as const));
121 return json(res, 200, { torque: on });
122 },
123 "POST /stop": async ({ ctx, req, res }) => {
124 const body = await readBody(req);
125 // Held first, before anything awaits: a frame already on its way finds the gate shut.
126 if (body.hold !== false && ctx.gate.hold(actor(body))) ctx.metrics.event("hold", { by: actor(body) });
127 const was = ctx.driver;
128 if (was === "stream") for (const client of ctx.wss!.clients) end(ctx, client, 1000, "stopped from the viewer"); // its close handler holds the arm
129 else if (was === "record") {
130 const summary = await finishRecording(ctx);
131 return json(res, 200, { stopped: was, ...summary });
132 } else ctx.current?.abort(); // the supervisor stops the move where it is
133 return json(res, 200, { stopped: was });
134 },
135 };
Software/so101/src/server/record.ts added +107
@@ -0,0 +1,107 @@
1 /** Recording the joints while a person moves the arm, and playing a recording back inside the zone and the speed limit. */
2
3 import { MotionError, playTrajectory, recordTrajectory } from "../motion.ts";
4 import type { Violation } from "../kinematics.ts";
5 import { bound } from "../safety.ts";
6 import { STS3215 } from "../sts3215.ts";
7 import { largestStep, serializeTrajectory, type Trajectory } from "../trajectory.ts";
8 import { claim, release } from "./driver.ts";
9 import { HttpError, idList, json, number, readBody, writeAtomic } from "./http.ts";
10 import { bounds, speedLimits, torqueCaps } from "./safety.ts";
11 import { listRecordings, loadRecording, poseNamed, recordingPath } from "./storage.ts";
12 import type { Ctx, Recorded, Routes } from "./types.ts";
13 import { checkPath, describeViolation, toQ, zoneSettings } from "./zone.ts";
14
15 export const finishRecording = async (ctx: Ctx) => {
16 if (!ctx.recording || !ctx.current) throw new HttpError(409, "nothing is being recorded");
17 const { name, done } = ctx.recording;
18 const controller = ctx.current;
19 ctx.recording = null; // taken: a second caller, racing this one, finds nothing to stop
20 controller.abort();
21 const result = await done;
22 release(ctx);
23 if ("error" in result) throw new HttpError(500, `recording "${name}" failed: ${result.error}; nothing saved`);
24 const { trajectory, filled } = result;
25 if (!trajectory.frames.length) throw new HttpError(409, "nothing recorded; did the servos answer?");
26 writeAtomic(recordingPath(ctx, name), serializeTrajectory(trajectory));
27 ctx.metrics.event("record", { name, frames: trajectory.frames.length, seconds: trajectory.frames.length / trajectory.hz });
28 return { saved: name, frames: trajectory.frames.length, seconds: Number((trajectory.frames.length / trajectory.hz).toFixed(1)), filled };
29 };
30
31 export const recordRoutes: Routes = {
32 "POST /record/start": async ({ ctx, req, res }) => {
33 const body = await readBody(req);
34 const name = poseNamed(body.name);
35 const hz = number(body.hz, 30, 1, 100, "hz");
36 const wanted = idList(ctx.ids, body.ids);
37 if (Object.hasOwn(listRecordings(ctx), name)) throw new HttpError(409, `recording "${name}" already exists; pick another name`);
38 const controller = claim(ctx, "record");
39 const done: Promise<Recorded> = recordTrajectory(ctx.bus, wanted, { hz, signal: controller.signal }).then(
40 (r) => r,
41 (err: unknown) => ({ error: err instanceof Error ? err.message : String(err) }), // reported by /record/stop, never unhandled
42 );
43 ctx.recording = { name, hz, done };
44 return json(res, 200, { recording: name, hz, ids: wanted });
45 },
46 "POST /record/stop": async ({ ctx, res }) => json(res, 200, await finishRecording(ctx)),
47 "POST /play": async ({ ctx, req, res }) => {
48 const { bus, metrics, model, settings } = ctx;

Same issue as in move.ts: settings is taken once here, and refuse() and the zone check read it after several awaits. Use ctx.settings there.

49 const body = await readBody(req);
50 const name = poseNamed(body.name);
51 const recorded = loadRecording(ctx, name);
52 // Every frame inside the joints' bounds, and the whole playback no faster than the speed limit allows.
53 const b = bounds(ctx);
54 const trajectory: Trajectory = { ...recorded, frames: recorded.frames.map((f) => f.map((p, i) => bound(b, recorded.ids[i]!, p) ?? p)) };
55 const step = largestStep(trajectory);
56 const asked = number(body.rate, 1, 0.1, 2, "rate");
57 const rate = step > 0 ? Math.min(asked, speedLimits(ctx).speed / (step * trajectory.hz)) : asked;
58 if (rate < asked) ctx.machine.note("clamp", "speed limit", { asked, sent: Number(rate.toFixed(3)), source: `play ${name}` });
59 const controller = claim(ctx, "play");
60 try {
61 if (model && settings.zone !== "off") {
62 // The way to the first frame, then every frame (sampled when the recording is long), each judged as the whole
63 // pose with the joints the recording does not drive left where they are. Nothing moves before this passes.
64 const present = await bus.syncRead(ctx.ids, STS3215.Present_Position);
65 const poseAt = (frame: readonly number[]) => {
66 const pose = new Map(present);
67 trajectory.ids.forEach((id, i) => pose.set(id, frame[i]!));
68 return pose;
69 };
70 const refuse = (v: Violation, where: string): boolean => {
71 metrics.event("block", { source: `play ${name}`, where, what: v.what, link: v.link, depthMm: Math.round(v.depth * 1000), mode: settings.zone });
72 ctx.machine.note("clamp", "zone", { what: v.what, link: v.link, depthMm: Math.round(v.depth * 1000), refused: settings.zone === "enforce", source: `play ${name}` });
73 if (settings.zone !== "enforce") return false;
74 json(res, 409, { played: 0, error: `refused: ${where}: ${describeViolation(ctx, v)}`, violation: v });
75 return true;
76 };
77 const onTheWay = checkPath(ctx, present, poseAt(trajectory.frames[0]!));
78 if (onTheWay && refuse(onTheWay, "on the way to the first frame")) return;
79 const stride = Math.max(1, Math.ceil(trajectory.frames.length / 3000));
80 for (let index = 0; index < trajectory.frames.length; index += stride) {
81 const v = model.check(toQ(ctx, poseAt(trajectory.frames[index]!)), zoneSettings(ctx));
82 if (v) {
83 if (refuse(v, `frame ${index} of ${trajectory.frames.length}`)) return;
84 break;
85 }
86 }
87 }
88 const played = await playTrajectory(bus, trajectory, {
89 rate,
90 torquePercent: number(body.torque, 70, 10, 100, "torque"),
91 torqueCaps: torqueCaps(ctx),
92 signal: controller.signal,
93 });
94 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, trajectory.ids.map((id) => [id, 0] as const));
95 metrics.event("play", { name, played, frames: trajectory.frames.length, rate });
96 return json(res, 200, { played, frames: trajectory.frames.length, rate: Number(rate.toFixed(3)) });
97 } catch (err) {
98 if (err instanceof MotionError) {
99 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "play", { source: `play ${name}`, error: err.message, stalled: err.stalled, missing: err.missing });
100 return json(res, 409, { played: 0, error: err.message, stalled: err.stalled, missing: err.missing });
101 }
102 throw err;
103 } finally {
104 release(ctx);
105 }
106 },
107 };
Software/so101/src/server/router.ts added +23
@@ -0,0 +1,23 @@
1 /** Sends each request to the route that answers it: exact "METHOD /path" first, then the prefix families; any error becomes JSON. */
2
3 import type { IncomingMessage, ServerResponse } from "node:http";
4 import { HttpError, json } from "./http.ts";
5 import type { Ctx, PrefixRoute, Routes } from "./types.ts";
6
7 export function createRouter(ctx: Ctx, routes: Routes, prefixes: readonly PrefixRoute[]) {
8 return async (req: IncomingMessage, res: ServerResponse) => {
9 try {
10 const url = new URL(req.url ?? "/", "http://x");
11 const call = { ctx, req, res, url };
12 for (const p of prefixes) {
13 if (req.method === p.method && url.pathname.startsWith(p.prefix) && (await p.handle(call)) !== false) return;
14 }
15 const handler = Object.hasOwn(routes, `${req.method} ${url.pathname}`) ? routes[`${req.method} ${url.pathname}`] : undefined;
16 if (handler) return void (await handler(call));
17 json(res, 404, { error: "no such route" });
18 } catch (err) {
19 if (err instanceof HttpError) return json(res, err.status, { error: err.message });
20 json(res, 500, { error: err instanceof Error ? err.message : String(err) });
21 }
22 };
23 }
Software/so101/src/server/safety.ts added +138
@@ -0,0 +1,138 @@
1 /** The safety envelope in force on the arm (src/safety.ts): where joints may go, how fast, the emergency stop, and parking. */
2
3 import { readFileSync, unlinkSync } from "node:fs";
4 import { accelRegister, bound, jointBounds, motionLimits, TICKS_PER_DEGREE } from "../safety.ts";
5 import { moveToPose } from "../motion.ts";
6 import { STS3215 } from "../sts3215.ts";
7 import { claim, end, release } from "./driver.ts";
8 import { json, readBody, writeAtomic } from "./http.ts";
9 import { loadPoses } from "./storage.ts";
10 import { ESTOP_CLOSE, type Ctx, type EStop, type Routes } from "./types.ts";
11 import { checkPath, describeViolation } from "./zone.ts";
12
13 /** Where each joint may be sent, ticks: the calibrated range, less the margin, narrowed by the admin's limits. */
14 export const bounds = (ctx: Ctx) => jointBounds(ctx.limits, ctx.settings);
15 export const speedLimits = (ctx: Ctx) => motionLimits(ctx.settings);
16 /** The gripper's force limit, for every supervised move. */
17 export const torqueCaps = (ctx: Ctx) => new Map([[6, ctx.settings.gripperTorquePercent]]);
18
19 /** The servo registers that enforce the same speed and acceleration in hardware, whatever the server sends. */
20 export const servoLimits = async (ctx: Ctx, targetIds: readonly number[]) => {
21 const { speed, accel } = speedLimits(ctx);
22 await ctx.bus.syncWrite(STS3215.Goal_Velocity, targetIds.map((id) => [id, speed] as const));
23 await ctx.bus.syncWrite(STS3215.Acceleration, targetIds.map((id) => [id, accelRegister(accel)] as const));
24 };
25
26 /** The stop on disk, if any; one that cannot be read still stops. */
27 export const loadEStop = (estopFile: string): EStop | null => {
28 try {
29 const stored = JSON.parse(readFileSync(estopFile, "utf8")) as Partial<EStop>;
30 if (typeof stored.by === "string" && typeof stored.at === "string") return { by: stored.by, at: stored.at };
31 return { by: "an unreadable record", at: new Date().toISOString() };
32 } catch (err) {
33 if ((err as NodeJS.ErrnoException).code !== "ENOENT") return { by: "an unreadable record", at: new Date().toISOString() };
34 return null;
35 }
36 };
37
38 /**
39 * Who an E-stop or its clearing is credited to: the body's `by`, one line of at most 64 characters, kept as text
40 * (pages show it with textContent). It is only a label: on this server it is unauthenticated, and the relay sets it.
41 */
42 export function actor(body: Record<string, unknown>): string {
43 const by = typeof body.by === "string" ? body.by.replace(/[\p{Cc}\p{Cf}\s]+/gu, " ").trim().slice(0, 64).trim() : "";
44 return by || "someone at the arm";
45 }
46
47 /** Torque on, goal where each joint is: the arm stays put and resists. */
48 export const holdHere = async (ctx: Ctx) => {
49 const positions = await ctx.bus.syncRead(ctx.ids, STS3215.Present_Position);
50 const here = ctx.ids.filter((id) => positions.has(id));
51 await ctx.bus.syncWrite(STS3215.Goal_Position, here.map((id) => [id, positions.get(id)!] as const));
52 await ctx.bus.syncWrite(STS3215.Torque_Enable, here.map((id) => [id, 1] as const));
53 };
54
55 /** Halt everything and hold with torque; nothing moves the arm until `clearEStop`. Hitting it again only holds again. */
56 const engageEStop = async (ctx: Ctx, by: string): Promise<EStop> => {
57 if (!ctx.estop) {
58 ctx.estop = { by, at: new Date().toISOString() };
59 writeAtomic(ctx.estopFile, JSON.stringify(ctx.estop));
60 ctx.metrics.event("estop", { by });
61 ctx.machine.note("state", "emergency stop", { by });
62 }
63 const was = ctx.driver;
64 if (was === "stream") for (const client of ctx.wss!.clients) end(ctx, client, ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks
65 else if (was === "record" && ctx.recording) {
66 const { done } = ctx.recording;
67 ctx.recording = null; // nothing is saved: the recording ends where it was cut
68 ctx.current?.abort();
69 void done.then(() => release(ctx));
70 } else ctx.current?.abort(); // a move, a playback or a park stops where it is
71 await holdHere(ctx);
72 return ctx.estop;
73 };
74
75 const clearEStop = (ctx: Ctx, by: string) => {
76 if (!ctx.estop) return false;
77 ctx.estop = null;
78 try {
79 unlinkSync(ctx.estopFile);
80 } catch {
81 /* already gone */
82 }
83 ctx.metrics.event("estop-clear", { by });
84 ctx.machine.note("state", "emergency stop cleared", { by });
85 return true;
86 };
87
88 /**
89 * The driver's connection is gone: go slowly to the park pose (the saved pose named in the settings, or the middle
90 * of every arm joint's range), under supervision, unless the arm is stopped, busy, or the way there is in the zone.
91 */
92 export const park = async (ctx: Ctx, why: string) => {
93 if (ctx.estop || ctx.gate.held || ctx.driver) return;
94 const controller = claim(ctx, "park");
95 try {
96 const pose = loadPoses(ctx)[ctx.settings.parkPose];
97 const b = bounds(ctx);
98 const targets: Array<[number, number]> = pose
99 ? pose.ids.flatMap((id, i) => (b.has(id) ? [[id, bound(b, id, pose.positions[i]!)!] as [number, number]] : []))
100 : ctx.ids.filter((id) => id !== 6 && b.has(id)).map((id) => [id, bound(b, id, (ctx.limits.get(id)!.min + ctx.limits.get(id)!.max) / 2)!]);
101 if (!targets.length) return;
102 const from = await ctx.bus.syncRead(ctx.ids, STS3215.Present_Position);
103 const violation = checkPath(ctx, from, new Map([...from, ...targets]));
104 if (violation && ctx.settings.zone === "enforce") {
105 ctx.metrics.event("park", { why, parked: false, error: `refused: ${describeViolation(ctx, violation)}` });
106 ctx.machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: true, source: "park" });
107 return;
108 }
109 await moveToPose(ctx.bus, targets.map(([id]) => id), targets.map(([, t]) => t), {
110 speed: Math.round(ctx.settings.parkSpeedDegS * TICKS_PER_DEGREE),
111 torquePercent: 50,
112 torqueCaps: torqueCaps(ctx),
113 signal: controller.signal,
114 });
115 ctx.metrics.event("park", { why, parked: true, pose: pose ? ctx.settings.parkPose : "middle" });
116 ctx.machine.note("state", "parked", { why, pose: pose ? ctx.settings.parkPose : "middle" });
117 } catch (err) {
118 ctx.metrics.event("park", { why, parked: false, error: err instanceof Error ? err.message : String(err) });
119 } finally {
120 release(ctx);
121 }
122 };
123
124 export const safetyRoutes: Routes = {
125 "POST /estop": async ({ ctx, req, res }) => {
126 const by = actor(await readBody(req));
127 const was = ctx.driver;
128 return json(res, 200, { estop: await engageEStop(ctx, by), stopped: was });
129 },
130 "POST /resume": async ({ ctx, req, res }) => {
131 const by = actor(await readBody(req));
132 const resumed = ctx.gate.resume();
133 if (resumed) ctx.metrics.event("resume", { by });
134 return json(res, 200, { resumed, held: null });
135 },
136 "POST /estop/clear": async ({ ctx, req, res }) => json(res, 200, { cleared: clearEStop(ctx, actor(await readBody(req))) }),
137 };
138
Software/so101/src/server/settings.ts added +81
@@ -0,0 +1,81 @@
1 /** The settings: the safe zone, the trims and the safety limits - validated, stored beside the poses, read and changed over HTTP. */
2
3 import { existsSync, readFileSync } from "node:fs";
4 import { SAFETY_KEYS, SafetyError, validateSafety, type SafetySettings } from "../safety.ts";
5 import { JOINTS } from "../sts3215.ts";
6 import { HttpError, json, readBody, writeAtomic } from "./http.ts";
7 import { DEFAULT_SETTINGS, type Routes, type Settings } from "./types.ts";
8 import { URDF } from "./zone.ts";
9
10 export function validateSettings(safetyBase: SafetySettings, s: Partial<Settings>): Settings {
11 let safety: SafetySettings;
12 try {
13 safety = validateSafety(Object.fromEntries(SAFETY_KEYS.map((key) => [key, s[key as keyof SafetySettings]])), safetyBase);
14 } catch (err) {
15 if (err instanceof SafetyError) throw new HttpError(400, err.message);
16 throw err;
17 }
18 const out: Settings = { ...DEFAULT_SETTINGS, ...safety, jointOffsetsDeg: {} };
19 if (s.zone !== undefined) {
20 if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"');
21 out.zone = s.zone;
22 }
23 for (const key of ["floorMarginMm", "baseMarginMm"] as const) {
24 if (s[key] === undefined) continue;
25 if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`);
26 out[key] = s[key]!;
27 }
28 if (s.jointOffsetsDeg !== undefined) {
29 if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object");
30 for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) {
31 if (!Object.values(JOINTS).includes(name)) throw new HttpError(400, `no joint "${name}"`);
32 if (typeof deg !== "number" || !Number.isFinite(deg) || Math.abs(deg) > 30) throw new HttpError(400, `offset for ${name} must be within ±30 degrees`);
33 if (deg !== 0) out.jointOffsetsDeg[name] = deg;
34 }
35 }
36 return out;
37 }
38
39 /** The starting values, then what is stored; field by field, so one bad value does not throw away the zone mode or the other trims. */
40 export function loadSettings(settingsFile: string, safetyBase: SafetySettings): Settings {
41 let settings: Settings = { ...DEFAULT_SETTINGS, ...safetyBase };
42 if (existsSync(settingsFile)) {
43 try {
44 const stored = JSON.parse(readFileSync(settingsFile, "utf8")) as Record<string, unknown>;
45 for (const [key, value] of Object.entries(stored)) {
46 try {
47 settings = validateSettings(safetyBase, { ...settings, [key]: value });
48 } catch (err) {
49 console.error(`settings: ignoring ${key} in ${settingsFile}: ${err instanceof Error ? err.message : String(err)}`);
50 }
51 }
52 } catch (err) {
53 console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`);
54 }
55 }
56 return settings;
57 }
58
59 export const settingsRoutes: Routes = {
60 "GET /settings": ({ ctx, res }) => json(res, 200, { ...ctx.settings, model: ctx.model !== null }),
61 "PUT /settings": async ({ ctx, req, res }) => {
62 const body = await readBody(req);
63 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => {
64 const patch = body[key];
65 return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...ctx.settings[key], ...patch } : (patch ?? ctx.settings[key]);
66 };
67 ctx.settings = validateSettings(ctx.safetyBase, { ...ctx.settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg") } as Partial<Settings>);
68 writeAtomic(ctx.settingsFile, JSON.stringify(ctx.settings, null, 1));
69 return json(res, 200, { ...ctx.settings, model: ctx.model !== null });
70 },
71 "GET /model.json": ({ ctx, res }) => {
72 const { model } = ctx;
73 if (!model) return json(res, 404, { error: "no model loaded on this server" });
74 return json(res, 200, {
75 urdf: `/model/${URDF}`,
76 floorZ: model.floorZ,
77 baseTop: model.baseTop,
78 joints: model.movable.map((j) => ({ name: j.name, id: Number(Object.entries(JOINTS).find(([, n]) => n === j.name)?.[0] ?? 0), lower: j.lower, upper: j.upper })),
79 });
80 },
81 };
Software/so101/src/server/static.ts added +102
@@ -0,0 +1,102 @@
1 /** What the browser loads: the page, its modules, the arm model and meshes, the vendored libraries and the printable sheets. */
2
3 import { createHash } from "node:crypto";
4 import { existsSync, readdirSync, readFileSync, statSync } from "node:fs";
5 import { dirname, extname, join, resolve } from "node:path";
6 import { fileURLToPath } from "node:url";
7 import { browserModule } from "../browser-modules.ts";
8 import { decode, file, json, safeFile } from "./http.ts";
9 import type { PrefixRoute, Routes } from "./types.ts";
10
11 const here = dirname(dirname(fileURLToPath(import.meta.url)));
12 const viewerPath = join(here, "viewer.html");
13 const uiDir = join(here, "ui");
14 const nodeModules = resolve(here, "../node_modules");
15 const VENDORED = new Set(["three", "urdf-loader"]);
16 /** A module the page loads from src/ui: plain JavaScript, never its tests. */
17 const isUiModule = (name: string) => /^[a-z][a-z0-9-]*\.js$/.test(name);
18
19 /** The page's own modules, under a name that changes with their content: cached for good, and a deploy is seen at once. */
20 const uiVersion = () => {
21 const hash = createHash("sha256");
22 for (const f of readdirSync(uiDir).filter(isUiModule).sort()) hash.update(f).update(readFileSync(join(uiDir, f)));
23 return hash.digest("hex").slice(0, 12);
24 };
25
26 const page: Routes[string] = ({ res }) => {
27 res.writeHead(200, { "content-type": "text/html; charset=utf-8", "cache-control": "no-store" });
28 return res.end(readFileSync(viewerPath, "utf8").replaceAll("/ui/__VERSION__/", `/ui/${uiVersion()}/`)); // read per request: a viewer change deploys with rsync alone
29 };
30
31 export const staticRoutes: Routes = {
32 "GET /": page,
33 "GET /index.html": page,
34 "GET /leader-map.js": ({ res }) => file(res, join(here, "leader-map.js"), "no-cache"),
35 "GET /printables": ({ ctx, res }) => {
36 const { printablesDir } = ctx;
37 let files: string[] = [];
38 try {
39 files = printablesDir ? readdirSync(printablesDir).filter((f) => f.endsWith(".png")).sort() : [];
40 } catch {
41 /* none */
42 }
43 return json(res, 200, files.map((f) => ({ file: f, url: `/printables/${f}`, bytes: statSync(join(printablesDir!, f)).size })));
44 },
45 };
46
47 export const staticPrefixes: PrefixRoute[] = [
48 {
49 method: "GET",
50 prefix: "/model/",
51 handle: ({ ctx, res, url }) => {
52 if (!ctx.modelDir) return false;
53 const path = safeFile(resolve(ctx.modelDir), decode(url.pathname.slice("/model/".length)));
54 if (!path || ![".urdf", ".stl", ".dae", ".png", ".jpg"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such model file" });
55 return file(res, path, "max-age=86400");
56 },
57 },
58 {
59 method: "GET",
60 prefix: "/printables/",
61 handle: ({ ctx, res, url }) => {
62 if (!ctx.printablesDir) return false;
63 const path = safeFile(resolve(ctx.printablesDir), decode(url.pathname.slice("/printables/".length)));
64 if (!path || extname(path).toLowerCase() !== ".png") return json(res, 404, { error: "no such printable" });
65 return file(res, path, "max-age=3600");
66 },
67 },
68 {
69 method: "GET",
70 prefix: "/vendor/",
71 handle: ({ res, url }) => {
72 const [pkg, ...rest] = decode(url.pathname.slice("/vendor/".length)).split("/");
73 const path = pkg && VENDORED.has(pkg) && rest.length ? safeFile(join(nodeModules, pkg), rest.join("/")) : null;
74 if (!path || ![".js", ".mjs", ".json"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such vendored file" });
75 return file(res, path, "max-age=86400");
76 },
77 },
78 {
79 method: "GET",
80 prefix: "/ui/",
81 handle: ({ res, url }) => {
82 const ui = /^\/ui\/[0-9a-f]{12}\/([a-z][a-z0-9-]*\.js)$/.exec(url.pathname);
83 if (!ui) return false;
84 if (!isUiModule(ui[1]!) || !existsSync(join(uiDir, ui[1]!))) return json(res, 404, { error: "no such module" });
85 return file(res, join(uiDir, ui[1]!), "public, max-age=31536000, immutable");
86 },
87 },
88 {
89 method: "GET",
90 prefix: "/app/",
91 handle: async ({ req, res, url }) => {
92 const served = await browserModule(decode(url.pathname.slice("/app/".length)));
93 if (!served) return json(res, 404, { error: "no such module" });
94 if (req.headers["if-none-match"] === served.etag) {
95 res.writeHead(304, { etag: served.etag, "cache-control": "no-cache" });
96 return res.end();
97 }
98 res.writeHead(200, { "content-type": "text/javascript; charset=utf-8", "cache-control": "no-cache", etag: served.etag });
99 return res.end(served.js);
100 },
101 },
102 ];
Software/so101/src/server/status.ts added +58
@@ -0,0 +1,58 @@
1 /** What the arm reports about itself: every joint, what drives it, the zone, and the metrics. */
2
3 import { STS3215, JOINTS, ticksToDegrees } from "../sts3215.ts";
4 import { json } from "./http.ts";
5 import { bounds } from "./safety.ts";
6 import type { Ctx, Routes } from "./types.ts";
7 import { approach, evaluate } from "./zone.ts";
8
9 export const status = async (ctx: Ctx) => {
10 const { bus, ids, model } = ctx;
11 const positions = await bus.syncRead(ids, STS3215.Present_Position);
12 const loads = await bus.syncRead(ids, STS3215.Present_Load);
13 const temps = await bus.syncRead(ids, STS3215.Present_Temperature);
14 const torque = await bus.syncRead(ids, STS3215.Torque_Enable);
15 const b = bounds(ctx);
16 const joints = ids.map((id) => ({
17 id,
18 joint: JOINTS[id] ?? null,
19 position: positions.get(id) ?? null,
20 degrees: positions.has(id) ? Number(ticksToDegrees(positions.get(id)!).toFixed(1)) : null,
21 load: loads.has(id) ? loads.get(id)! / 10 : null,
22 temperature: temps.get(id) ?? null,
23 torque: torque.has(id) ? torque.get(id) === 1 : null,
24 /** The last Goal_Position this server wrote, ticks; null before the first. */
25 target: ctx.goals.get(id) ?? null,
26 /** The calibrated range (the servo's own limits), and the narrower bounds every command is held inside. */
27 min: ctx.limits.get(id)?.min ?? null,
28 max: ctx.limits.get(id)?.max ?? null,
29 lo: b.get(id)?.min ?? null,
30 hi: b.get(id)?.max ?? null,
31 }));
32 ctx.metrics.sample(joints);
33 const zone = evaluate(ctx, positions);
34 approach(ctx, zone);
35 return {
36 joints,
37 busy: ctx.driver !== null,
38 driving: ctx.driver,
39 recording: ctx.recording?.name ?? null,
40 zone: model ? ctx.settings.zone : "off",
41 violation: zone?.violation ?? null,
42 lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000).toFixed(0)) : null,
43 blocked: ctx.blocked,
44 jammed: [...ctx.jams.keys()],
45 stream: ctx.streamStats,
46 /** The stream's dead-man hold (no frame lately); `held` is the STOP's, which only /resume ends. */
47 deadman: ctx.held,
48 estop: ctx.estop,
49 held: ctx.gate.held,
50 machine: { run: ctx.runId, seq: ctx.machine.seq },
51 };
52 };
53
54 export const statusRoutes: Routes = {
55 "GET /status": async ({ ctx, res }) => json(res, 200, await status(ctx)),
56 "GET /limits": ({ ctx, res }) => json(res, 200, Object.fromEntries(ctx.limits)),
57 "GET /metrics": ({ ctx, res }) => json(res, 200, ctx.metrics.summary()),
58 };
Software/so101/src/server/storage.ts added +127
@@ -0,0 +1,127 @@
1 /** What the arm remembers on disk beside the poses: the poses, the recordings and the vision kit's calibration. */
2
3 import { readdirSync, readFileSync, unlinkSync } from "node:fs";
4 import { join } from "node:path";
5 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "../poses.ts";
6 import { parseTrajectory, type Trajectory } from "../trajectory.ts";
7 import { STS3215 } from "../sts3215.ts";
8 import { HttpError, idList, json, readBody, writeAtomic } from "./http.ts";
9 import type { Call, Ctx, PrefixRoute, Routes } from "./types.ts";
10
11 export const loadPoses = (ctx: Ctx): PoseFile => {
12 try {
13 return parsePoseFile(readFileSync(ctx.posesFile, "utf8"));
14 } catch (err) {
15 if ((err as NodeJS.ErrnoException).code === "ENOENT") return {};
16 throw new HttpError(500, `${ctx.posesFile}: ${err instanceof Error ? err.message : String(err)}`);
17 }
18 };
19
20 export const poseNamed = (name: unknown): string => {
21 if (typeof name !== "string") throw new HttpError(400, "name must be a string");
22 try {
23 assertPoseName(name);
24 } catch (err) {
25 throw new HttpError(400, (err as Error).message);
26 }
27 return name;
28 };
29
30 export const recordingPath = (ctx: Ctx, name: string) => join(ctx.recordingsDir, `${name}.json`);
31
32 export const loadRecording = (ctx: Ctx, name: string): Trajectory => {
33 try {
34 return parseTrajectory(readFileSync(recordingPath(ctx, name), "utf8"));
35 } catch (err) {
36 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
37 throw new HttpError(500, `recording "${name}": ${err instanceof Error ? err.message : String(err)}`);
38 }
39 };
40
41 export const listRecordings = (ctx: Ctx) => {
42 const out: Record<string, { hz: number; frames: number; seconds: number; recorded?: string }> = {};
43 let names: string[] = [];
44 try {
45 names = readdirSync(ctx.recordingsDir).filter((f) => f.endsWith(".json"));
46 } catch {
47 return out;
48 }
49 for (const file of names) {
50 try {
51 const t = parseTrajectory(readFileSync(join(ctx.recordingsDir, file), "utf8"));
52 out[file.slice(0, -5)] = { hz: t.hz, frames: t.frames.length, seconds: Number((t.frames.length / t.hz).toFixed(1)), ...(t.recorded ? { recorded: t.recorded } : {}) };
53 } catch {
54 /* a file that is not a recording is not listed */
55 }
56 }
57 return out;
58 };
59
60 const loadCalibration = (ctx: Ctx): Record<string, unknown> => {
61 const out: Record<string, unknown> = { camera: null, handeye: null };
62 try {
63 for (const file of readdirSync(ctx.calibrationDir).filter((f) => f.endsWith(".json"))) {
64 try {
65 out[file.slice(0, -5)] = JSON.parse(readFileSync(join(ctx.calibrationDir, file), "utf8"));
66 } catch {
67 /* skip a broken file */
68 }
69 }
70 } catch {
71 /* none yet */
72 }
73 return out;
74 };
75
76 export const storageRoutes: Routes = {
77 "GET /poses": ({ ctx, res }) => json(res, 200, loadPoses(ctx)),
78 "GET /recordings": ({ ctx, res }) => json(res, 200, listRecordings(ctx)),
79 "GET /calibration": ({ ctx, res }) => json(res, 200, loadCalibration(ctx)),
80 "POST /pose/save": async ({ ctx, req, res }) => {
81 const body = await readBody(req);
82 const name = poseNamed(body.name);
83 const wanted = idList(ctx.ids, body.ids);
84 const positions = await ctx.bus.syncRead(wanted, STS3215.Present_Position);
85 const missing = wanted.filter((id) => !positions.has(id));
86 if (missing.length) throw new HttpError(409, `no response from servo ${missing.join(", ")}`);
87 const pose = { ids: wanted, positions: wanted.map((id) => positions.get(id)!), saved: new Date().toISOString() };
88 writeAtomic(ctx.posesFile, serializePoseFile({ ...loadPoses(ctx), [name]: pose }));
89 const onStops = wanted.filter((id) => {
90 const lim = ctx.limits.get(id)!;
91 return positions.get(id)! - lim.min < 40 || lim.max - positions.get(id)! < 40;
92 });
93 return json(res, 200, { name, ...pose, onStops });
94 },
95 "POST /pose/delete": async ({ ctx, req, res }) => {
96 const name = poseNamed((await readBody(req)).name);
97 const poses = loadPoses(ctx);
98 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
99 const { [name]: _, ...rest } = poses;
100 writeAtomic(ctx.posesFile, serializePoseFile(rest));
101 return json(res, 200, { deleted: name });
102 },
103 "POST /recording/delete": async ({ ctx, req, res }) => {
104 const name = poseNamed((await readBody(req)).name);
105 if (ctx.recording?.name === name) throw new HttpError(409, `"${name}" is being recorded right now`);
106 try {
107 unlinkSync(recordingPath(ctx, name));
108 } catch (err) {
109 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
110 throw err;
111 }
112 return json(res, 200, { deleted: name });
113 },
114 };
115
116 /** PUT /calibration/<kind>: store the vision kit's JSON next to the poses. */
117 export const calibrationPut: PrefixRoute = {
118 method: "PUT",
119 prefix: "/calibration/",
120 handle: async ({ ctx, req, res, url }: Call) => {
121 const kind = url.pathname.slice("/calibration/".length);
122 if (!/^[a-z][a-z0-9_-]{0,31}$/.test(kind)) throw new HttpError(400, "calibration kind: lowercase letters, digits, - and _");
123 const body = await readBody(req);
124 writeAtomic(join(ctx.calibrationDir, `${kind}.json`), JSON.stringify({ ...body, saved: new Date().toISOString() }, null, 1));
125 return json(res, 200, { stored: kind });
126 },
127 };
Software/so101/src/server/stream.ts added +318
@@ -0,0 +1,318 @@
1 /** The /follow WebSocket: a leader's joint targets become goals, held to the bounds, the zone, the speed limits and the dead-man. */
2
3 import type { IncomingMessage } from "node:http";
4 import { WebSocketServer, type WebSocket } from "ws";
5 import { HELD_CLOSE } from "../command-gate.ts";
6 import { STALL_SPAN_TICKS } from "../motion.ts";
7 import { bound, Governor } from "../safety.ts";
8 import { JOINTS, STS3215 } from "../sts3215.ts";
9 import { senderClocks } from "./clocks.ts";
10 import { claim, describe, holding, release, stopped } from "./driver.ts";
11 import { bounds, park, servoLimits, speedLimits } from "./safety.ts";
12 import { ESTOP_CLOSE, type Ctx } from "./types.ts";
13 import { approach, evaluate } from "./zone.ts";
14
15 /** Close codes a stream ends with on purpose; any other end is a lost connection. */
16 const CLEAN_CLOSE = new Set([1000, 1005]);
17 /** Load, in percent of the servo's maximum, that counts as pushing against something. */
18 const JAM_LOAD = 45;
19 const JAM_WINDOW = 6;
20 /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */
21 const RESUME_AFTER_DROPS = 20;
22
23 // Frames are merged down to the command rate; each joint's target is pulled inside its bounds, judged by the zone,
24 // and handed to the governor, which moves the goals toward it within the speed and acceleration limits.
25 // Silence makes the arm hold (the dead-man); a connection that is lost rather than closed makes it park.
26 export function attachStream(ctx: Ctx): WebSocketServer {
27 const { bus, ids, metrics, jams, options, streamTorque, maxStep, easeSpeed, jamSampleMs, heartbeatMs } = ctx;
28 const wss = new WebSocketServer({ noServer: true, maxPayload: 4096 });
29 ctx.wss = wss;
30 const clock = senderClocks();
31 wss.on("connection", (ws: WebSocket, req?: IncomingMessage) => {
32 if (ctx.estop) {
33 ws.close(ESTOP_CLOSE, stopped(ctx.estop));
34 return;
35 }
36 if (ctx.gate.held) {
37 ws.close(HELD_CLOSE, holding(ctx.gate.held));
38 return;
39 }
40 if (ctx.driver) {
41 ws.close(1013, `${describe(ctx.driver)} is driving the arm`);
42 return;
43 }
44 claim(ctx, "stream");
45 const startedAt = Date.now();
46 const stats = { frames: 0, dropped: 0, refused: 0, merged: 0, clamped: 0, gated: 0 };
47 /** Seq, age and the hold, frame by frame; stamps are optional here (the CLI leader sends none). */
48 const admission = ctx.gate.open(clock(req), false);
49 /** Set the moment the stream ends: whatever is queued or under way gives up, and nothing more reaches the bus. */
50 let closed = false;
51 ctx.streamStats = stats;
52 const holdAfterMs = options.watchdogMs ?? ctx.settings.holdAfterMs;
53 const period = 1000 / ctx.settings.rateHz;
54 const governor = new Governor(() => bounds(ctx), () => speedLimits(ctx));
55 let last: Map<number, number> | null = null;
56 /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */
57 const drops = new Map<number, number>();
58 /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */
59 let heldDepth = 0;
60 /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */
61 let present = new Map<number, number>();
62 let armed = false;
63 let arming = false;
64 let timer: ReturnType<typeof setTimeout> | undefined;
65 let chain: Promise<void> = Promise.resolve();
66 const run = (fn: () => Promise<void>) => (chain = chain.then(fn).catch((err: unknown) => console.error(`stream: ${err instanceof Error ? err.message : String(err)}`)));
67
68 const hold = () =>
69 run(async () => {
70 if (closed) return;
71 const positions = await bus.syncRead(ids, STS3215.Present_Position);
72 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
73 governor.reset(positions);
74 last = null;
75 ctx.held = true;
76 ctx.machine.note("state", "dead-man hold", { afterMs: holdAfterMs });
77 });
78 const arm = () =>
79 run(async () => {
80 if (closed) return;
81 const positions = await bus.syncRead(ids, STS3215.Present_Position);
82 present = new Map(positions);
83 const ready = ids.filter((id) => positions.has(id));
84 await bus.syncWrite(STS3215.Goal_Position, ready.map((id) => [id, positions.get(id)!] as const));
85 await bus.syncWrite(STS3215.Torque_Limit, ready.map((id) => [id, Math.round((id === 6 ? Math.min(streamTorque, ctx.settings.gripperTorquePercent) : streamTorque) * 10)] as const));
86 await servoLimits(ctx, ready);
87 await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const));
88 governor.reset(positions);
89 last = null; // the first frame eases the arm over, however far away the leader is
90 heldDepth = evaluate(ctx, positions)?.violation?.depth ?? 0;
91 armed = true;
92 });
93 let easing = false;
94
95 // The jam watch: a joint that pushes hard without moving toward its target is parked where it is.
96 const windows = new Map<number, Array<{ p: number; l: number }>>();
97 const observe = async (id: number, p: number, l: number, target: number | undefined): Promise<boolean> => {
98 present.set(id, p);
99 const w = windows.get(id) ?? [];
100 w.push({ p, l });
101 if (w.length > JAM_WINDOW) w.shift();
102 windows.set(id, w);
103 if (jams.has(id) || w.length < JAM_WINDOW || target === undefined) return jams.has(id);
104 const pushing = w.every((s) => Math.abs(s.l) >= JAM_LOAD * 10);
105 const travel = Math.max(...w.map((s) => s.p)) - Math.min(...w.map((s) => s.p));
106 if (!(pushing && travel < STALL_SPAN_TICKS && Math.abs(target - p) > 30)) return false;
107 await bus.syncWrite(STS3215.Goal_Position, [[id, p]]);
108 governor.pin(id, p);
109 jams.set(id, { position: p, direction: Math.sign(target - p) });
110 last?.set(id, p);
111 metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10 });
112 ctx.machine.note("clamp", "jam", { id, position: p, target, load: l / 10 });
113 return true;
114 };
115 const watch = setInterval(
116 () =>
117 void run(async () => {
118 if (closed || !armed || last === null) return;
119 const positions = await bus.syncRead(ids, STS3215.Present_Position);
120 const loads = await bus.syncRead(ids, STS3215.Present_Load);
121 for (const id of ids) {
122 const p = positions.get(id), l = loads.get(id);
123 if (p !== undefined && l !== undefined) await observe(id, p, l, last.get(id));
124 }
125 }),
126 jamSampleMs,
127 );
128
129 // The governor's clock: at the command rate, the goals move one step toward the targets.
130 let ticking = false;
131 let lastTick = performance.now();
132 const ticker = setInterval(() => {
133 const now = performance.now();
134 const dt = Math.min(now - lastTick, 2 * period) / 1000; // a late tick never turns into a jump
135 lastTick = now;
136 if (!armed || easing || ticking || !governor.moving) return;
137 ticking = true;
138 void run(async () => {
139 try {
140 if (closed) return;
141 const step = governor.step(dt);
142 if (step.length) await bus.syncWrite(STS3215.Goal_Position, step);
143 } finally {
144 ticking = false;
145 }
146 });
147 }, period);
148
149 // The heartbeat: a client that does not answer a ping before the next one is gone, however quiet its socket.
150 let answered = true;
151 ws.on("pong", () => (answered = true));
152 const heartbeat = setInterval(() => {
153 if (!answered) return ws.terminate();
154 answered = false;
155 ws.ping();
156 }, heartbeatMs);
157
158 const follow = (frame: ReadonlyMap<number, number>) =>
159 run(async () => {
160 if (closed) return; // queued before a stop: dropped, never replayed
161 const b = bounds(ctx);
162 const targets: Array<readonly [number, number]> = [];
163 let ease = last === null; // after a hold, everything eases
164 for (const [id, value] of frame) {
165 const target = bound(b, id, value);
166 if (target === null) continue;
167 if (target !== Math.round(value)) {
168 stats.clamped++;
169 ctx.machine.note("clamp", "joint limit", { id, asked: Math.round(value), sent: target, source: "stream" });
170 }
171 const jam = jams.get(id);
172 if (jam) {
173 if ((target - jam.position) * jam.direction > -10) continue; // still pushing into the obstacle: wait for the leader to back off
174 jams.delete(id);
175 windows.delete(id);
176 }
177 const previous = last?.get(id);
178 if (previous === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped
179 else if (Math.abs(target - previous) > maxStep) {
180 stats.dropped++;
181 ctx.machine.note("clamp", "too far in one frame", { id, asked: target, from: previous, maxStep });
182 const n = (drops.get(id) ?? 0) + 1;
183 drops.set(id, n);
184 if (n < RESUME_AFTER_DROPS) continue; // no hand moves that far in one frame
185 // The leader has stayed far away (after a block or a jam): ease over to it rather than leave the joint stranded.
186 drops.delete(id);
187 ease = true;
188 } else drops.delete(id);
189 targets.push([id, target]);
190 }
191 if (!targets.length) return;
192 stats.frames++;
193 if (ctx.held) {
194 ctx.held = false;
195 ctx.machine.note("state", "following again", {});
196 }
197 // The zone: the frame is judged as the whole pose it would make, with uncommanded joints where they are.
198 const pose = new Map(present);
199 for (const [id, t] of last ?? []) pose.set(id, t);
200 for (const [id, t] of targets) pose.set(id, t);
201 const zone = evaluate(ctx, pose);
202 if (zone?.violation) {
203 if (!ctx.blocked) metrics.event("block", { source: "stream", what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), mode: ctx.settings.zone });
204 ctx.blocked = zone.violation;
205 if (ctx.settings.zone === "enforce" && zone.violation.depth > heldDepth + 0.001) {
206 stats.refused++;
207 ctx.machine.note("clamp", "zone", { what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), refused: true, source: "stream" });
208 return; // deeper in than the arm already is: it stays at the last good targets; a frame that backs out is let through
209 }
210 } else ctx.blocked = null;
211 heldDepth = zone?.violation?.depth ?? 0;
212 approach(ctx, zone);
213 if (ease) {
214 // Catch up slowly with every joint in this frame, the servos' own speed held low, then hand over to the governor.
215 // A joint that jams on the way is parked and no longer waited for.
216 easing = true;
217 try {
218 await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, speedLimits(ctx).speed)] as const));
219 await bus.syncWrite(STS3215.Goal_Position, targets);
220 const deadline = Date.now() + 4000;
221 const wanted = new Map(targets);
222 while (Date.now() < deadline && !closed) {
223 const positions = await bus.syncRead([...wanted.keys()], STS3215.Present_Position);
224 const loads = await bus.syncRead([...wanted.keys()], STS3215.Present_Load);
225 let worst = 0;
226 for (const [id, t] of wanted) {
227 const p = positions.get(id), l = loads.get(id);
228 if (p === undefined || l === undefined) continue;
229 if (await observe(id, p, l, t)) continue; // jammed: parked, not waited for
230 worst = Math.max(worst, Math.abs(p - t));
231 }
232 if (worst <= 25) break;
233 await new Promise((r) => setTimeout(r, 50));
234 }
235 if (!closed) await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, speedLimits(ctx).speed] as const));
236 } finally {
237 easing = false;
238 }
239 last = new Map([...(last ?? []), ...targets]);
240 for (const [id, t] of targets) governor.pin(id, jams.get(id)?.position ?? t);
241 for (const [id, jam] of jams) last.set(id, jam.position);
242 return;
243 }
244 for (const [id, t] of targets) {
245 governor.aim(id, t);
246 last!.set(id, t);
247 }
248 });
249
250 // Frames faster than the command rate are merged, the latest value per joint winning, and taken at the next slot.
251 let pending: Map<number, number> | null = null;
252 let pacer: ReturnType<typeof setTimeout> | undefined;
253 let lastTaken = 0;
254 const take = () => {
255 pacer = undefined;
256 if (!pending || closed) return;
257 // While the arm catches up, the frame waits (later ones merge into it) and is taken once it has: never dropped,
258 // or a leader that sends one last frame and stops would leave the arm short of it.
259 if (easing) return void (pacer = setTimeout(take, period));
260 const frame = pending;
261 pending = null;
262 lastTaken = Date.now();
263 follow(frame);
264 };
265 ws.on("message", (data) => {
266 if (closed) return;
267 const verdict = admission.admit(String(data));
268 if (!verdict.ok) {
269 if (verdict.drop !== "malformed") stats.gated++;
270 return;
271 }
272 const frame = verdict.joints;
273 clearTimeout(timer);
274 timer = setTimeout(() => void hold(), holdAfterMs);
275 if (!arming) {
276 arming = true;
277 void arm();
278 }
279 if (pending) stats.merged++;
280 pending = new Map([...(pending ?? []), ...frame]);
281 pacer ??= setTimeout(take, Math.max(0, lastTaken + period - Date.now()));
282 });
283 const halt = () => {
284 closed = true;
285 pending = null;
286 clearTimeout(timer);
287 clearTimeout(pacer);
288 clearInterval(watch);
289 clearInterval(ticker);
290 };
291 ctx.halts.set(ws, halt);
292 ws.on("close", (code) => {
293 halt();
294 ctx.halts.delete(ws);
295 admission.close();
296 clearInterval(heartbeat);
297 const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE;
298 void run(async () => {
299 try {
300 if (armed) {
301 const positions = await bus.syncRead(ids, STS3215.Present_Position);
302 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
303 await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, id === 6 ? ctx.settings.gripperTorquePercent * 10 : 1000] as const));
304 }
305 } finally {
306 ctx.blocked = null;
307 ctx.held = false;
308 jams.clear();
309 ctx.streamStats = null;
310 metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats, ...(lost ? { lost: code } : {}) });
311 release(ctx); // whatever the bus did, the arm is free for the next driver
312 }
313 if (lost && armed) void park(ctx, `the stream was lost (${code})`);
314 });
315 });
316 });
317 return wss;
318 }
Software/so101/src/server/types.ts added +132
@@ -0,0 +1,132 @@
1 /** What the arm server's modules share: its options, its settings, and the one state object they all read (the context). */
2
3 import type { IncomingMessage, ServerResponse } from "node:http";
4 import type { WebSocket, WebSocketServer } from "ws";
5 import type { FeetechBus } from "../bus.ts";
6 import type { CommandGate } from "../command-gate.ts";
7 import type { ArmModel, Violation } from "../kinematics.ts";
8 import type { MachineLog } from "../machine-log.ts";
9 import type { Metrics } from "../metrics.ts";
10 import { DEFAULT_SAFETY, type SafetySettings } from "../safety.ts";
11 import type { Trajectory } from "../trajectory.ts";
12
13 export interface ServerOptions {
14 host?: string;
15 port?: number;
16 ids?: readonly number[];
17 /** The poses file; recordings, calibration, settings and metrics live beside it. */
18 posesFile?: string;
19 /** Folder holding so101_new_calib.urdf and assets/; without it there is no safe zone and no 3D model. */
20 modelDir?: string;
21 /** Folder with the printable PNG sheets of the vision kit; found beside the package or in the repo layout. */
22 printablesDir?: string;
23 /** Torque cap while a stream drives the arm, percent. */
24 streamTorque?: number;
25 /** Largest per-frame change accepted on the stream, ticks. */
26 maxStep?: number;
27 /** With no frame for this long, the arm is told to hold where it is; overrides the holdAfterMs setting. */
28 watchdogMs?: number;
29 /** How often a stream's client is pinged; one that misses a ping is a lost connection, and the arm parks. */
30 heartbeatMs?: number;
31 /** Starting values for the safety settings, before what is stored (src/safety.ts). */
32 safety?: Partial<SafetySettings>;
33 /** Speed used to catch up with the first frame of a stream, ticks/s. */
34 easeSpeed?: number;
35 /** How often the stream's jam watch samples position and load, ms. */
36 jamSampleMs?: number;
37 /** How often each /watch connection is sent the status, ms. */
38 watchEveryMs?: number;
39 }
40
41 export interface RunningServer {
42 readonly port: number;
43 close(): Promise<void>;
44 }
45
46 export type Driver = "stream" | "move" | "play" | "record" | "park";
47
48 export interface Settings extends SafetySettings {
49 zone: "enforce" | "warn" | "off";
50 floorMarginMm: number;
51 baseMarginMm: number;
52 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
53 jointOffsetsDeg: Record<string, number>;
54 }
55
56 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY };
57
58 /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */
59 export interface EStop {
60 by: string;
61 at: string;
62 }
63
64 export type Recorded = { trajectory: Trajectory; filled: number } | { error: string };
65
66 /** Everything one running arm server is: fixed at start, plus the few things that change while it runs. */
67 export interface Ctx {
68 readonly bus: FeetechBus;
69 readonly options: ServerOptions;
70 readonly ids: readonly number[];
71 readonly posesFile: string;
72 readonly recordingsDir: string;
73 readonly calibrationDir: string;
74 readonly settingsFile: string;
75 readonly estopFile: string;
76 readonly streamTorque: number;
77 readonly maxStep: number;
78 readonly easeSpeed: number;
79 readonly jamSampleMs: number;
80 readonly watchEveryMs: number;
81 readonly heartbeatMs: number;
82 readonly safetyBase: SafetySettings;
83 readonly metrics: Metrics;
84 /** The machine, raw: every register written and every change of state or clamp, for the Machine panel. */
85 readonly machine: MachineLog;
86 /** STOP's hold: every frame is dropped and every move refused until /resume (src/command-gate.ts). */
87 readonly gate: CommandGate;
88 /** Each stream's halt: what it has queued or under way stops at once, not when the close handshake comes back. */
89 readonly halts: Map<WebSocket, () => void>;
90 /** Changes when the server restarts, and the machine log's numbering starts over. */
91 readonly runId: string;
92 /** Each joint's last Goal_Position this server wrote. */
93 readonly goals: Map<number, number>;
94 readonly limits: Map<number, { min: number; max: number }>;
95 readonly modelDir: string | undefined;
96 readonly printablesDir: string | undefined;
97 readonly model: ArmModel | null;
98 settings: Settings;
99 driver: Driver | null;
100 current: AbortController | null;
101 estop: EStop | null;
102 recording: { name: string; hz: number; done: Promise<Recorded> } | null;
103 /** What the stream is up against right now, for /status. */
104 blocked: Violation | null;
105 /** The stream's dead-man hold is in force: no frame came for holdAfterMs, and the arm stays where it was. */
106 held: boolean;
107 readonly jams: Map<number, { position: number; direction: number }>;
108 streamStats: { frames: number; dropped: number; refused: number } | null;
109 /** The stream's socket server, set once the stream is wired. */
110 wss: WebSocketServer | null;
111 }
112
113 /** One request, with the context it is served from. */
114 export interface Call {
115 ctx: Ctx;
116 req: IncomingMessage;
117 res: ServerResponse;
118 url: URL;
119 }
120
121 /** Routes by "METHOD /path"; a handler answers through `res`, or returns nothing after it has. */
122 export type Routes = Record<string, (call: Call) => unknown>;
123
124 /** A route family under a path prefix; it returns false when the request is not its to answer. */
125 export interface PrefixRoute {
126 method: string;
127 prefix: string;
128 handle: (call: Call) => unknown;
129 }
130
131 /** The close code for a stream ended by an emergency stop. */
132 export const ESTOP_CLOSE = 4423;
Software/so101/src/server/upgrade.ts added +15
@@ -0,0 +1,15 @@
1 /** Sends each WebSocket upgrade to the server for its path; any other path gets a 404. */
2
3 import type { IncomingMessage, Server } from "node:http";
4 import type { Duplex } from "node:stream";
5 import type { WebSocketServer } from "ws";
6
7 export function attachUpgrades(http: Server, byPath: Readonly<Record<string, WebSocketServer>>): void {
8 http.on("upgrade", (req: IncomingMessage, socket: Duplex, head: Buffer) => {
9 const path = new URL(req.url ?? "/", "http://x").pathname;
10 const to = Object.hasOwn(byPath, path) ? byPath[path] : undefined;
11 if (!to) return void socket.end("HTTP/1.1 404 Not Found\r\nconnection: close\r\ncontent-length: 0\r\n\r\n");
12 socket.on("error", () => socket.destroy());
13 to.handleUpgrade(req, socket, head, (ws) => to.emit("connection", ws, req));
14 });
15 }
Software/so101/src/server/watch.ts added +59
@@ -0,0 +1,59 @@
1 /** WS /watch: read-only. The status, pushed; one timer reads the bus while anyone watches, and a watcher's messages are ignored. */
2
3 import { WebSocketServer, type WebSocket } from "ws";
4 import { status } from "./status.ts";
5 import type { Ctx } from "./types.ts";
6
7 export interface Watch {
8 readonly watchers: WebSocketServer;
9 stop(): void;
10 }
11
12 export function attachWatch(ctx: Ctx): Watch {
13 const watchers = new WebSocketServer({ noServer: true, maxPayload: 1024 });
14 let watching: ReturnType<typeof setInterval> | undefined;
15 let reading = false;
16 const machineFrame = (entries: readonly unknown[]) => JSON.stringify({ machine: entries, run: ctx.runId });
17 /** The machine log goes out as it is written, while anyone watches; a watcher yet to get its first status gets it all then. */
18 let unsubscribe: (() => void) | undefined;
19 const fresh = new WeakSet<WebSocket>();
20 const broadcast = async () => {
21 if (reading) return; // a slow bus read is not stacked behind another: the next tick asks again
22 reading = true;
23 try {
24 const text = JSON.stringify(await status(ctx));
25 for (const w of watchers.clients) {
26 if (w.readyState !== w.OPEN) continue;
27 w.send(text);
28 // A new watcher's first message is a status; the machine log so far comes right after it.
29 if (fresh.delete(w)) w.send(machineFrame(ctx.machine.recent(200)));
30 }
31 } catch (err) {
32 console.error(`watch: ${err instanceof Error ? err.message : String(err)}`);
33 } finally {
34 reading = false;
35 }
36 };
37 watchers.on("connection", (ws: WebSocket) => {
38 watching ??= setInterval(() => void broadcast(), ctx.watchEveryMs);
39 unsubscribe ??= ctx.machine.subscribe((batch) => {
40 const text = machineFrame(batch);
41 for (const w of watchers.clients) if (w.readyState === w.OPEN && !fresh.has(w)) w.send(text);
42 });
43 fresh.add(ws);
44 void broadcast();
45 ws.on("error", () => ws.terminate());
46 ws.on("close", () => {
47 if (watchers.clients.size > 0 || !watching) return;
48 clearInterval(watching);
49 watching = undefined;
50 unsubscribe?.();
51 unsubscribe = undefined;
52 });
53 });
54 return { watchers, stop: () => {
55 clearInterval(watching);
56 ctx.machine.close();
57 ctx.bus.onSyncWrite = null;
58 }, };
59 }
Software/so101/src/server/zone.ts added +71
@@ -0,0 +1,71 @@
1 /** The arm's own geometry: the model, and judging a pose or a path against the table and the base. */
2
3 import { existsSync } from "node:fs";
4 import { dirname, join } from "node:path";
5 import { fileURLToPath } from "node:url";
6 import { ArmModel, gripperTicksToRadians, ticksToRadians, type Violation } from "../kinematics.ts";
7 import { JOINTS } from "../sts3215.ts";
8 import type { Ctx, ServerOptions } from "./types.ts";
9
10 export const URDF = "so101_new_calib.urdf";
11
12 /** The model (and where it and the printables are served from); without one there is no safe zone and no 3D view. */
13 export function loadModel(options: ServerOptions) {
14 const here = dirname(dirname(fileURLToPath(import.meta.url)));
15 const modelDir = options.modelDir ?? [join(here, "../../../Simulation/SO101"), join(here, "../model")].find((d) => existsSync(join(d, URDF)));
16 const printablesDir = options.printablesDir ?? [join(here, "../../vision/printables"), join(here, "../printables")].find((d) => existsSync(d));
17 let model: ArmModel | null = null;
18 if (modelDir && existsSync(join(modelDir, URDF))) {
19 try {
20 model = ArmModel.load(join(modelDir, URDF));
21 } catch (err) {
22 console.error(`model: ${err instanceof Error ? err.message : String(err)}; no safe zone`);
23 }
24 } else console.error(`model: no ${URDF} in ${modelDir ?? "(no model dir)"}; no safe zone and no 3D view`);
25 return { modelDir, printablesDir, model };
26 }
27
28 export const zoneSettings = (ctx: Ctx) => ({ floorMargin: ctx.settings.floorMarginMm / 1000, baseMargin: ctx.settings.baseMarginMm / 1000 });
29
30 /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */
31 export const toQ = (ctx: Ctx, positions: ReadonlyMap<number, number>) => {
32 const gripperOpen = ctx.model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745;
33 const q = new Map<string, number>();
34 for (const [id, ticks] of positions) {
35 const name = JOINTS[id];
36 const lim = ctx.limits.get(id);
37 if (!name || !lim) continue;
38 const rad = name === "gripper" ? gripperTicksToRadians(ticks, lim.min, lim.max, gripperOpen) : ticksToRadians(ticks, lim.min, lim.max);
39 q.set(name, rad + ((ctx.settings.jointOffsetsDeg[name] ?? 0) * Math.PI) / 180);
40 }
41 return q;
42 };
43
44 export const evaluate = (ctx: Ctx, positions: ReadonlyMap<number, number>) => (ctx.model && ctx.settings.zone !== "off" ? ctx.model.evaluate(toQ(ctx, positions), zoneSettings(ctx)) : null);
45
46 /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */
47 export const approach = (ctx: Ctx, r: ReturnType<typeof evaluate>) => {
48 if (r && ctx.model) ctx.metrics.approach(r.lowest.z - ctx.model.floorZ, r.lowest.link);
49 };
50
51 export const describeViolation = (ctx: Ctx, v: Violation) => {
52 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
53 const marginMm = v.what === "table" ? ctx.settings.floorMarginMm : ctx.settings.baseMarginMm;
54 const beyond = v.depth * 1000 - marginMm; // the depth includes the margin
55 return beyond > 0
56 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`
57 : `${part} would come within ${(-beyond).toFixed(0)} mm of the ${v.what}, inside the ${marginMm} mm margin`;
58 };
59
60 /** The worst violation on the straight joint-space path from one pose to another, or null. */
61 export const checkPath = (ctx: Ctx, from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => {
62 if (!ctx.model || ctx.settings.zone === "off") return null;
63 let worst: Violation | null = null;
64 for (let s = 1; s <= steps; s++) {
65 const mix = new Map<number, number>();
66 for (const [id, a] of from) mix.set(id, Math.round(a + ((to.get(id) ?? a) - a) * (s / steps)));
67 const v = ctx.model.check(toQ(ctx, mix), zoneSettings(ctx));
68 if (v && (!worst || v.depth > worst.depth)) worst = v;
69 }
70 return worst;
71 };

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs