Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
Split the app into small modules so parallel pulls stop colliding (one feature, one file) #61
smith/arm:smith/58-split-the-app-into-small-modules-so-para → main 6 commits fork 4 threads · 4 unresolved
Software/so101/src/lobby.test.ts +3 −2
| @@ -40,7 +40,7 @@ async function client(port: number, headers: Record<string, string> = {}) { | ||
| 40 | 40 | for (const w of [...waiters]) if (w.match(m)) waiters.splice(waiters.indexOf(w), 1), w.resolve(m); |
| 41 | 41 | }); |
| 42 | 42 | 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) => | |
| 44 | 44 | new Promise<Msg>((resolve, reject) => { |
| 45 | 45 | waiters.push({ match, resolve }); |
| 46 | 46 | setTimeout(() => reject(new Error(`nothing matched in ${ms} ms; heard ${JSON.stringify(heard.slice(-5))}`)), ms); |
| @@ -555,10 +555,11 @@ describe("lobby", () => { | ||
| 555 | 555 | await bob.next(drivingIs("alice")); |
| 556 | 556 | bob.send({ type: "join" }); |
| 557 | 557 | 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 | |
| 558 | 559 | const ended = await form(port, { action: "end" }, OWNER); |
| 559 | 560 | expect(ended.status).toBe(303); |
| 560 | 561 | expect(ended.headers.get("location")).toBe("/admin?done=ended"); |
| 561 | await alice.next(drivingIs("bob")); | |
| 562 | await handed; | |
| 562 | 563 | |
| 563 | 564 | const saved = await form(port, { action: "save", turnSeconds: "90", idleSeconds: "20", maxSpeedPercent: "40", maxQueue: "3", announcement: '<script>alert("x")</script>' }, OWNER); |
| 564 | 565 | 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)/); | |
This lookahead lets in the The negative lookahead |
||
| 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 @@ | ||
| 49 | 49 | * its goal is parked where it is, and it follows again once the leader backs off. |
| 50 | 50 | */ |
| 51 | 51 | |
| 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"; | |
| 61 | 53 | import type { FeetechBus } from "./bus.ts"; |
| 62 | import { ArmModel, gripperTicksToRadians, ticksToRadians, type Violation } from "./kinematics.ts"; | |
| 63 | import { clamp, type Frame } from "./mapping.ts"; | |
| 64 | import { MachineLog, perServo } from "./machine-log.ts"; | |
| 65 | import { Metrics } from "./metrics.ts"; | |
| 66 | import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts"; | |
| 67 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; | |
| 68 | import { JOINTS, registerName, STS3215, ticksToDegrees } from "./sts3215.ts"; | |
| 69 | import { largestStep, parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts"; | |
| 70 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, SAFETY_KEYS, SafetyError, TICKS_PER_DEGREE, validateSafety, type SafetySettings } from "./safety.ts"; | |
| 54 | import { createContext } from "./server/context.ts"; | |
| 55 | import { moveRoutes } from "./server/move.ts"; | |
| 56 | import { recordRoutes } from "./server/record.ts"; | |
| 57 | import { createRouter } from "./server/router.ts"; | |
| 58 | import { actor, safetyRoutes } from "./server/safety.ts"; | |
| 59 | import { settingsRoutes } from "./server/settings.ts"; | |
| 60 | import { staticPrefixes, staticRoutes } from "./server/static.ts"; | |
| 61 | import { statusRoutes } from "./server/status.ts"; | |
| 62 | import { calibrationPut, storageRoutes } from "./server/storage.ts"; | |
| 63 | import { attachStream } from "./server/stream.ts"; | |
| 64 | import { attachUpgrades } from "./server/upgrade.ts"; | |
| 65 | import { attachWatch } from "./server/watch.ts"; | |
outdated
This is a good thin entry. But it is the arm-local server, which only |
||
| 66 | import type { RunningServer, ServerOptions } from "./server/types.ts"; | |
| 71 | 67 | |
| 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 }; | |
| 156 | 71 | |
| 157 | 72 | 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 }); | |
| 1314 | 79 | |
| 1315 | 80 | await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve)); |
| 1316 | 81 | const address = http.address(); |
| @@ -1319,12 +84,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1319 | 84 | port, |
| 1320 | 85 | close: () => |
| 1321 | 86 | 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(); | |
| 1326 | 89 | wss.close(); |
| 1327 | watchers.close(); | |
| 90 | watch.watchers.close(); | |
| 1328 | 91 | http.close(() => resolve()); |
| 1329 | 92 | http.closeAllConnections(); // the viewer keeps idle keep-alive connections open; close() alone would wait for them |
| 1330 | 93 | }), |
Software/so101/src/server/clocks.ts added +15
| @@ -0,0 +1,15 @@ | ||
| 1 | /** Each sender's clock, by address: one device keeps one clock, so a burst it flushes on a new stream is still known as old. */ | |
| 2 | ||
| 3 | import type { IncomingMessage } from "node:http"; | |
| 4 | import { Clock } from "../command-gate.ts"; | |
| 5 | ||
| 6 | export function senderClocks(limit = 64): (req: IncomingMessage | undefined) => Clock { | |
| 7 | const clocks = new Map<string, Clock>(); | |
| 8 | return (req) => { | |
| 9 | const key = req?.socket.remoteAddress ?? ""; | |
| 10 | if (!clocks.has(key) && clocks.size >= limit) clocks.delete(clocks.keys().next().value!); | |
| 11 | const c = clocks.get(key) ?? new Clock(); | |
| 12 | clocks.set(key, c); | |
| 13 | return c; | |
| 14 | }; | |
| 15 | } |
Software/so101/src/server/context.ts added +76
| @@ -0,0 +1,76 @@ | ||
| 1 | /** Builds the context one arm server runs on: its paths, its limits read off the servos, its model, its settings and its stored stop. */ | |
| 2 | ||
| 3 | import { randomUUID } from "node:crypto"; | |
| 4 | import { dirname, join, resolve } from "node:path"; | |
| 5 | import type { FeetechBus } from "../bus.ts"; | |
| 6 | import { CommandGate } from "../command-gate.ts"; | |
| 7 | import { MachineLog, perServo } from "../machine-log.ts"; | |
| 8 | import { Metrics } from "../metrics.ts"; | |
| 9 | import { validateSafety, type SafetySettings } from "../safety.ts"; | |
| 10 | import { registerName, STS3215 } from "../sts3215.ts"; | |
| 11 | import { loadEStop } from "./safety.ts"; | |
| 12 | import { loadSettings } from "./settings.ts"; | |
| 13 | import type { Ctx, ServerOptions } from "./types.ts"; | |
| 14 | import { loadModel } from "./zone.ts"; | |
| 15 | ||
| 16 | export async function createContext(bus: FeetechBus, options: ServerOptions): Promise<Ctx> { | |
| 17 | const posesFile = resolve(options.posesFile ?? "poses.json"); | |
| 18 | const dataDir = dirname(posesFile); | |
| 19 | const estopFile = join(dataDir, "estop.json"); | |
| 20 | const settingsFile = join(dataDir, "settings.json"); | |
| 21 | const ids = options.ids ?? [1, 2, 3, 4, 5, 6]; | |
| 22 | let safetyBase: SafetySettings; | |
| 23 | try { | |
| 24 | safetyBase = validateSafety(options.safety ?? {}); | |
| 25 | } catch (err) { | |
| 26 | throw new Error(`safety options: ${(err as Error).message}`); | |
| 27 | } | |
| 28 | const limits = new Map<number, { min: number; max: number }>(); | |
| 29 | for (const id of ids) { | |
| 30 | limits.set(id, { | |
| 31 | min: await bus.read(id, STS3215.Min_Position_Limit), | |
| 32 | max: await bus.read(id, STS3215.Max_Position_Limit), | |
| 33 | }); | |
| 34 | } | |
| 35 | const machine = new MachineLog(); | |
| 36 | const goals = new Map<number, number>(); | |
| 37 | bus.onSyncWrite = (register, values) => { | |
| 38 | if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v); | |
| 39 | machine.note("sent", registerName(register), { values: perServo(values) }); | |
| 40 | }; | |
| 41 | return { | |
| 42 | bus, | |
| 43 | options, | |
| 44 | ids, | |
| 45 | posesFile, | |
| 46 | recordingsDir: join(dataDir, "recordings"), | |
| 47 | calibrationDir: join(dataDir, "calibration"), | |
| 48 | settingsFile, | |
| 49 | estopFile, | |
| 50 | streamTorque: options.streamTorque ?? 70, | |
| 51 | maxStep: options.maxStep ?? 400, | |
| 52 | easeSpeed: options.easeSpeed ?? 300, | |
| 53 | jamSampleMs: options.jamSampleMs ?? 100, | |
| 54 | watchEveryMs: options.watchEveryMs ?? 250, | |
| 55 | heartbeatMs: options.heartbeatMs ?? 1000, | |
| 56 | safetyBase, | |
| 57 | metrics: new Metrics(join(dataDir, "metrics.jsonl")), | |
| 58 | machine, | |
| 59 | gate: new CommandGate(), | |
| 60 | halts: new Map(), | |
| 61 | runId: randomUUID(), | |
| 62 | goals, | |
| 63 | limits, | |
| 64 | ...loadModel(options), | |
| 65 | settings: loadSettings(settingsFile, safetyBase), | |
| 66 | driver: null, | |
| 67 | current: null, | |
| 68 | estop: loadEStop(estopFile), | |
| 69 | recording: null, | |
| 70 | blocked: null, | |
| 71 | held: false, | |
| 72 | jams: new Map(), | |
| 73 | streamStats: null, | |
| 74 | wss: null, | |
| 75 | }; | |
| 76 | } |
Software/so101/src/server/driver.ts added +34
| @@ -0,0 +1,34 @@ | ||
| 1 | /** Who drives the arm: one thing at a time, and nothing at all while an emergency stop is in force. */ | |
| 2 | ||
| 3 | import type { WebSocket } from "ws"; | |
| 4 | import { HttpError } from "./http.ts"; | |
| 5 | import type { Ctx, Driver, EStop } from "./types.ts"; | |
| 6 | ||
| 7 | export const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d]; | |
| 8 | ||
| 9 | export const holding = (h: { by: string; at: string }) => `stopped (${h.by}, ${h.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until someone resumes`; | |
| 10 | ||
| 11 | export const stopped = (e: EStop) => `emergency stop (${e.by}, ${e.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until an admin clears it`; | |
| 12 | ||
| 13 | export const claim = (ctx: Ctx, kind: Driver): AbortController => { | |
| 14 | if (ctx.estop) throw new HttpError(423, stopped(ctx.estop)); | |
| 15 | if (ctx.gate.held) throw new HttpError(423, holding(ctx.gate.held)); | |
| 16 | if (ctx.driver) throw new HttpError(409, `${describe(ctx.driver)} is driving the arm`); | |
| 17 | ctx.driver = kind; | |
| 18 | ctx.current = new AbortController(); | |
| 19 | ctx.machine.note("state", "driver", { driving: kind }); | |
| 20 | return ctx.current; | |
| 21 | }; | |
| 22 | ||
| 23 | export const release = (ctx: Ctx) => { | |
| 24 | if (ctx.driver) ctx.machine.note("state", "driver", { driving: null }); | |
| 25 | ctx.driver = null; | |
| 26 | ctx.current = null; | |
| 27 | }; | |
| 28 | ||
| 29 | /** Close a stream's socket after halting it: what it has queued or under way stops now, not when the close handshake comes back. */ | |
| 30 | export const end = (ctx: Ctx, ws: WebSocket, code: number, why: string) => { | |
| 31 | ctx.halts.get(ws)?.(); | |
| 32 | ws.close(code, why); | |
| 33 | }; | |
| 34 |
Software/so101/src/server/http.ts added +90
| @@ -0,0 +1,90 @@ | ||
| 1 | /** The plumbing every route shares: answering in JSON, serving a file, reading a body, validating numbers and paths. */ | |
| 2 | ||
| 3 | import { mkdirSync, readFileSync, renameSync, statSync, writeFileSync } from "node:fs"; | |
| 4 | import type { IncomingMessage, ServerResponse } from "node:http"; | |
| 5 | import { dirname, extname, resolve } from "node:path"; | |
| 6 | import { clamp } from "../mapping.ts"; | |
| 7 | ||
| 8 | export class HttpError extends Error { | |
| 9 | readonly status: number; | |
| 10 | constructor(status: number, message: string) { | |
| 11 | super(message); | |
| 12 | this.status = status; | |
| 13 | } | |
| 14 | } | |
| 15 | ||
| 16 | const MAX_BODY = 1_000_000; | |
| 17 | const MIME: Record<string, string> = { ".js": "text/javascript", ".mjs": "text/javascript", ".json": "application/json", ".urdf": "application/xml", ".stl": "model/stl", ".dae": "model/vnd.collada+xml", ".png": "image/png", ".jpg": "image/jpeg" }; | |
| 18 | ||
| 19 | export const json = (res: ServerResponse, status: number, body: unknown) => { | |
| 20 | res.writeHead(status, { "content-type": "application/json" }); | |
| 21 | res.end(JSON.stringify(body)); | |
| 22 | }; | |
| 23 | ||
| 24 | export const file = (res: ServerResponse, path: string, cache: string) => { | |
| 25 | res.writeHead(200, { "content-type": MIME[extname(path).toLowerCase()] ?? "application/octet-stream", "cache-control": cache }); | |
| 26 | res.end(readFileSync(path)); | |
| 27 | }; | |
| 28 | ||
| 29 | export const readBody = (req: IncomingMessage) => | |
| 30 | new Promise<Record<string, unknown>>((resolve, reject) => { | |
| 31 | let data = ""; | |
| 32 | req.on("data", (c) => { | |
| 33 | data += c; | |
| 34 | if (data.length > MAX_BODY) { | |
| 35 | req.destroy(); | |
| 36 | reject(new HttpError(413, "body too large")); | |
| 37 | } | |
| 38 | }); | |
| 39 | req.on("end", () => { | |
| 40 | if (!data.trim()) return resolve({}); | |
| 41 | try { | |
| 42 | const parsed: unknown = JSON.parse(data); | |
| 43 | if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) throw new Error(); | |
| 44 | resolve(parsed as Record<string, unknown>); | |
| 45 | } catch { | |
| 46 | reject(new HttpError(400, "body must be a JSON object")); | |
| 47 | } | |
| 48 | }); | |
| 49 | req.on("error", reject); | |
| 50 | }); | |
| 51 | ||
| 52 | export const number = (value: unknown, fallback: number, min: number, max: number, what: string): number => { | |
| 53 | if (value === undefined) return fallback; | |
| 54 | if (typeof value !== "number" || !Number.isFinite(value)) throw new HttpError(400, `${what} must be a number`); | |
| 55 | return clamp(value, min, max); | |
| 56 | }; | |
| 57 | ||
| 58 | export const idList = (ids: readonly number[], value: unknown): number[] => { | |
| 59 | if (value === undefined) return [...ids]; | |
| 60 | if (!Array.isArray(value) || !value.length || !value.every((id) => Number.isInteger(id) && ids.includes(id as number))) { | |
| 61 | throw new HttpError(400, `ids must be a non-empty list drawn from ${ids.join(", ")}`); | |
| 62 | } | |
| 63 | return [...new Set(value as number[])]; | |
| 64 | }; | |
| 65 | ||
| 66 | export const writeAtomic = (path: string, text: string) => { | |
| 67 | mkdirSync(dirname(path), { recursive: true }); | |
| 68 | writeFileSync(`${path}.tmp`, text); | |
| 69 | renameSync(`${path}.tmp`, path); | |
| 70 | }; | |
| 71 | ||
| 72 | /** A file under a root, or null for anything that tries to leave it. */ | |
| 73 | export const safeFile = (root: string, rel: string): string | null => { | |
| 74 | if (rel.includes("\0") || rel.split("/").some((part) => part === "..")) return null; | |
| 75 | const path = resolve(root, rel); | |
| 76 | if (path !== root && !path.startsWith(root + "/")) return null; | |
| 77 | try { | |
| 78 | return statSync(path).isFile() ? path : null; | |
| 79 | } catch { | |
| 80 | return null; | |
| 81 | } | |
| 82 | }; | |
| 83 | ||
| 84 | export const decode = (text: string) => { | |
| 85 | try { | |
| 86 | return decodeURIComponent(text); | |
| 87 | } catch { | |
| 88 | return "\0"; // never a file | |
| 89 | } | |
| 90 | }; |
Software/so101/src/server/move.ts added +135
| @@ -0,0 +1,135 @@ | ||
| 1 | /** Supervised moves over HTTP: any joints, a saved pose, the gripper, torque, and stopping whatever drives the arm. */ | |
| 2 | ||
| 3 | import { MotionError, moveToPose } from "../motion.ts"; | |
| 4 | import { bound } from "../safety.ts"; | |
| 5 | import { STS3215 } from "../sts3215.ts"; | |
| 6 | import { claim, describe, end, holding, release, stopped } from "./driver.ts"; | |
| 7 | import { HttpError, json, number, readBody } from "./http.ts"; | |
| 8 | import { finishRecording } from "./record.ts"; | |
| 9 | import { actor, bounds, speedLimits, torqueCaps } from "./safety.ts"; | |
| 10 | import { loadPoses, poseNamed } from "./storage.ts"; | |
| 11 | import type { Ctx, Routes } from "./types.ts"; | |
| 12 | import { checkPath, describeViolation } from "./zone.ts"; | |
| 13 | ||
| 14 | /** A supervised move to explicit targets; the one way joints move over HTTP. */ | |
| 15 | const move = async (ctx: Ctx, targetIds: number[], requested: number[], body: Record<string, unknown>, source: string) => { | |
| 16 | const { bus, metrics, settings } = ctx; | |
Small behaviour drift. This copies |
||
| 17 | const controller = claim(ctx, "move"); | |
| 18 | const b = bounds(ctx); | |
| 19 | const positions = targetIds.map((id, i) => bound(b, id, requested[i]!) ?? requested[i]!); | |
| 20 | targetIds.forEach((id, i) => positions[i] !== requested[i] && ctx.machine.note("clamp", "joint limit", { id, asked: requested[i]!, sent: positions[i]!, source })); | |
| 21 | try { | |
| 22 | const from = await bus.syncRead(ctx.ids, STS3215.Present_Position); | |
| 23 | const to = new Map(from); | |
| 24 | targetIds.forEach((id, i) => to.set(id, positions[i]!)); | |
| 25 | const violation = checkPath(ctx, from, to); | |
| 26 | if (violation) { | |
| 27 | metrics.event("block", { source, what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), mode: settings.zone }); | |
| 28 | ctx.machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source }); | |
| 29 | if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(ctx, violation)}`, violation } }; | |
| 30 | } | |
| 31 | const speed = number(body.speed, 300, 20, speedLimits(ctx).speed, "speed"); | |
| 32 | if (typeof body.speed === "number" && body.speed > speed) ctx.machine.note("clamp", "speed limit", { asked: body.speed, sent: speed, source }); | |
| 33 | const result = await moveToPose(bus, targetIds, positions, { | |
| 34 | speed, | |
| 35 | torquePercent: number(body.torque, 70, 10, 100, "torque"), | |
| 36 | torqueCaps: torqueCaps(ctx), | |
| 37 | signal: controller.signal, | |
| 38 | }); | |
| 39 | if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, targetIds.map((id) => [id, 0] as const)); | |
| 40 | metrics.counters.move++; | |
| 41 | const adjusted = new Map(result.adjusted); | |
| 42 | targetIds.forEach((id, i) => positions[i] !== requested[i] && adjusted.set(id, positions[i]!)); | |
| 43 | return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(adjusted), ...(violation ? { warning: describeViolation(ctx, violation) } : {}) } }; | |
| 44 | } catch (err) { | |
| 45 | if (err instanceof MotionError) { | |
| 46 | metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "move", { source, error: err.message, stalled: err.stalled, missing: err.missing }); | |
| 47 | return { status: 409, body: { reached: false, error: err.message, stalled: err.stalled, missing: err.missing } }; | |
| 48 | } | |
| 49 | throw err; | |
| 50 | } finally { | |
| 51 | release(ctx); | |
| 52 | } | |
| 53 | }; | |
| 54 | ||
| 55 | export const moveRoutes: Routes = { | |
| 56 | "POST /pose/go": async ({ ctx, req, res }) => { | |
| 57 | const body = await readBody(req); | |
| 58 | const name = poseNamed(body.name); | |
| 59 | const poses = loadPoses(ctx); | |
| 60 | if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`); | |
| 61 | const pose = poses[name]!; | |
| 62 | const result = await move(ctx, [...pose.ids], [...pose.positions], body, `pose ${name}`); | |
| 63 | return json(res, result.status, result.body); | |
| 64 | }, | |
| 65 | "POST /move": async ({ ctx, req, res }) => { | |
| 66 | const body = await readBody(req); | |
| 67 | const targets = body.targets; | |
| 68 | if (typeof targets !== "object" || targets === null || Array.isArray(targets)) throw new HttpError(400, "targets must be an object of id: ticks"); | |
| 69 | const targetIds: number[] = []; | |
| 70 | const positions: number[] = []; | |
| 71 | for (const [key, value] of Object.entries(targets as Record<string, unknown>)) { | |
| 72 | const id = Number(key); | |
| 73 | if (!/^\d+$/.test(key) || !ctx.ids.includes(id)) throw new HttpError(400, `no servo ${key} on this arm`); | |
| 74 | if (!Number.isInteger(value)) throw new HttpError(400, `target for servo ${key} must be an integer`); | |
| 75 | targetIds.push(id); | |
| 76 | positions.push(value as number); | |
| 77 | } | |
| 78 | if (!targetIds.length) throw new HttpError(400, "targets is empty"); | |
| 79 | const result = await move(ctx, targetIds, positions, body, "move"); | |
| 80 | return json(res, result.status, result.body); | |
| 81 | }, | |
| 82 | "POST /grip": async ({ ctx, req, res }) => { | |
| 83 | // Not supervised on purpose: a supervised close stops at the first contact and holds with no force. | |
| 84 | // The servo's own torque cap does the squeezing, and its overload protection holds a steady grip. | |
| 85 | const { bus } = ctx; | |
| 86 | const body = await readBody(req); | |
| 87 | const id = 6; | |
| 88 | const lim = ctx.limits.get(id); | |
| 89 | if (!lim) throw new HttpError(400, "this arm has no gripper (servo 6)"); | |
| 90 | if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer"); | |
| 91 | const position = bound(bounds(ctx), id, body.position as number)!; | |
| 92 | const torque = Math.min(number(body.torque, 30, 10, 60, "torque"), ctx.settings.gripperTorquePercent); // the force limit, whatever is asked | |
| 93 | const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), speedLimits(ctx).speed); // and the speed limit | |
| 94 | if (position !== body.position) ctx.machine.note("clamp", "joint limit", { id, asked: body.position as number, sent: position, source: "grip" }); | |
| 95 | if (typeof body.torque === "number" && body.torque > torque) ctx.machine.note("clamp", "gripper force", { asked: body.torque, sent: torque, source: "grip" }); | |
| 96 | claim(ctx, "move"); | |
| 97 | try { | |
| 98 | const present = await bus.syncRead([id], STS3215.Present_Position); | |
| 99 | if (!present.has(id)) throw new HttpError(409, "no response from the gripper"); | |
| 100 | await bus.syncWrite(STS3215.Goal_Position, [[id, present.get(id)!]]); | |
| 101 | await bus.syncWrite(STS3215.Torque_Limit, [[id, Math.round(torque * 10)]]); // stays low while it holds; the next supervised gripper move puts the ceiling back | |
| 102 | await bus.syncWrite(STS3215.Goal_Velocity, [[id, speed]]); | |
| 103 | await bus.syncWrite(STS3215.Torque_Enable, [[id, 1]]); | |
| 104 | await bus.syncWrite(STS3215.Goal_Position, [[id, position]]); | |
| 105 | } finally { | |
| 106 | release(ctx); | |
| 107 | } | |
| 108 | return json(res, 200, { gripping: position, torque }); | |
| 109 | }, | |
| 110 | "POST /torque": async ({ ctx, req, res }) => { | |
| 111 | const { on } = await readBody(req); | |
| 112 | if (typeof on !== "boolean") throw new HttpError(400, "on must be true or false"); | |
| 113 | if (ctx.estop) throw new HttpError(423, stopped(ctx.estop)); | |
| 114 | if (ctx.gate.held && !on) throw new HttpError(423, holding(ctx.gate.held)); | |
| 115 | if (ctx.driver) throw new HttpError(409, `${describe(ctx.driver)} is driving the arm`); | |
| 116 | if (on) { | |
| 117 | const positions = await ctx.bus.syncRead(ctx.ids, STS3215.Present_Position); | |
| 118 | await ctx.bus.syncWrite(STS3215.Goal_Position, ctx.ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const)); | |
| 119 | } | |
| 120 | await ctx.bus.syncWrite(STS3215.Torque_Enable, ctx.ids.map((id) => [id, on ? 1 : 0] as const)); | |
| 121 | return json(res, 200, { torque: on }); | |
| 122 | }, | |
| 123 | "POST /stop": async ({ ctx, req, res }) => { | |
| 124 | const body = await readBody(req); | |
| 125 | // Held first, before anything awaits: a frame already on its way finds the gate shut. | |
| 126 | if (body.hold !== false && ctx.gate.hold(actor(body))) ctx.metrics.event("hold", { by: actor(body) }); | |
| 127 | const was = ctx.driver; | |
| 128 | if (was === "stream") for (const client of ctx.wss!.clients) end(ctx, client, 1000, "stopped from the viewer"); // its close handler holds the arm | |
| 129 | else if (was === "record") { | |
| 130 | const summary = await finishRecording(ctx); | |
| 131 | return json(res, 200, { stopped: was, ...summary }); | |
| 132 | } else ctx.current?.abort(); // the supervisor stops the move where it is | |
| 133 | return json(res, 200, { stopped: was }); | |
| 134 | }, | |
| 135 | }; |
Software/so101/src/server/record.ts added +107
| @@ -0,0 +1,107 @@ | ||
| 1 | /** Recording the joints while a person moves the arm, and playing a recording back inside the zone and the speed limit. */ | |
| 2 | ||
| 3 | import { MotionError, playTrajectory, recordTrajectory } from "../motion.ts"; | |
| 4 | import type { Violation } from "../kinematics.ts"; | |
| 5 | import { bound } from "../safety.ts"; | |
| 6 | import { STS3215 } from "../sts3215.ts"; | |
| 7 | import { largestStep, serializeTrajectory, type Trajectory } from "../trajectory.ts"; | |
| 8 | import { claim, release } from "./driver.ts"; | |
| 9 | import { HttpError, idList, json, number, readBody, writeAtomic } from "./http.ts"; | |
| 10 | import { bounds, speedLimits, torqueCaps } from "./safety.ts"; | |
| 11 | import { listRecordings, loadRecording, poseNamed, recordingPath } from "./storage.ts"; | |
| 12 | import type { Ctx, Recorded, Routes } from "./types.ts"; | |
| 13 | import { checkPath, describeViolation, toQ, zoneSettings } from "./zone.ts"; | |
| 14 | ||
| 15 | export const finishRecording = async (ctx: Ctx) => { | |
| 16 | if (!ctx.recording || !ctx.current) throw new HttpError(409, "nothing is being recorded"); | |
| 17 | const { name, done } = ctx.recording; | |
| 18 | const controller = ctx.current; | |
| 19 | ctx.recording = null; // taken: a second caller, racing this one, finds nothing to stop | |
| 20 | controller.abort(); | |
| 21 | const result = await done; | |
| 22 | release(ctx); | |
| 23 | if ("error" in result) throw new HttpError(500, `recording "${name}" failed: ${result.error}; nothing saved`); | |
| 24 | const { trajectory, filled } = result; | |
| 25 | if (!trajectory.frames.length) throw new HttpError(409, "nothing recorded; did the servos answer?"); | |
| 26 | writeAtomic(recordingPath(ctx, name), serializeTrajectory(trajectory)); | |
| 27 | ctx.metrics.event("record", { name, frames: trajectory.frames.length, seconds: trajectory.frames.length / trajectory.hz }); | |
| 28 | return { saved: name, frames: trajectory.frames.length, seconds: Number((trajectory.frames.length / trajectory.hz).toFixed(1)), filled }; | |
| 29 | }; | |
| 30 | ||
| 31 | export const recordRoutes: Routes = { | |
| 32 | "POST /record/start": async ({ ctx, req, res }) => { | |
| 33 | const body = await readBody(req); | |
| 34 | const name = poseNamed(body.name); | |
| 35 | const hz = number(body.hz, 30, 1, 100, "hz"); | |
| 36 | const wanted = idList(ctx.ids, body.ids); | |
| 37 | if (Object.hasOwn(listRecordings(ctx), name)) throw new HttpError(409, `recording "${name}" already exists; pick another name`); | |
| 38 | const controller = claim(ctx, "record"); | |
| 39 | const done: Promise<Recorded> = recordTrajectory(ctx.bus, wanted, { hz, signal: controller.signal }).then( | |
| 40 | (r) => r, | |
| 41 | (err: unknown) => ({ error: err instanceof Error ? err.message : String(err) }), // reported by /record/stop, never unhandled | |
| 42 | ); | |
| 43 | ctx.recording = { name, hz, done }; | |
| 44 | return json(res, 200, { recording: name, hz, ids: wanted }); | |
| 45 | }, | |
| 46 | "POST /record/stop": async ({ ctx, res }) => json(res, 200, await finishRecording(ctx)), | |
| 47 | "POST /play": async ({ ctx, req, res }) => { | |
| 48 | const { bus, metrics, model, settings } = ctx; | |
Same issue as in |
||
| 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 | }; |
The negative lookahead
(?!const wss|const client)exempts thefor (const client of …)loop inclose(), so the test is bent around the code it checks. Move the shutdown into a helper (for examplecloseAll(http, wss, watch)inupgrade.tsor its own module), drop the exemption, and the "no control flow in the entry" claim holds without exceptions.