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) #76

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

  • 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)/);
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";
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;
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;
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