Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Safety first: the server enforces joint limits, speed limits, an emergency stop and a dead-man hold - no client can move the arm outside them #31
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
quill/arm:quill/11-safety-first-the-server-enforces-joint-l → main 9 commits fork
11 files
changed +1066
−86
Software/README.md +43 −4
| @@ -131,14 +131,16 @@ The API, all JSON (the full list with bodies is at the top of | ||
| 131 | 131 | `so101/src/server.ts`): `GET /status`, `/limits`, `/poses`, `/recordings`, |
| 132 | 132 | `/calibration`; `POST /pose/save`, `/pose/delete`, `/pose/go`, `/move` |
| 133 | 133 | (explicit `{"targets": {"3": 2048}}`), `/grip`, `/torque`, `/stop`, |
| 134 | `/record/start`, `/record/stop`, `/recording/delete`, `/play`; | |
| 134 | `/estop`, `/estop/clear`, `/record/start`, `/record/stop`, | |
| 135 | `/recording/delete`, `/play`; | |
| 135 | 136 | `PUT /calibration/camera|handeye`; and the WebSocket `/follow` taking |
| 136 | 137 | `{"1": ticks, ...}` frames. One thing drives the arm at a time. Every move |
| 137 | 138 | goes through the joint supervisor; `/stop` ends a move, a playback, a |
| 138 | 139 | recording, or a stream, and the arm holds. The stream is clamped to the |
| 139 | follower's calibrated limits, eases over to the first frame, drops jumps no | |
| 140 | hand makes, and holds the arm after half a second of silence. There is no | |
| 141 | authentication: keep it on a LAN or behind Tailscale. | |
| 140 | joint limits, eases over to the first frame, drops jumps no hand makes, is | |
| 141 | held to the speed and acceleration limits, and holds the arm after a second | |
| 142 | of silence (see **Safety limits** below). There is no authentication: keep it | |
| 143 | on a LAN or behind Tailscale. | |
| 142 | 144 | |
| 143 | 145 | `GET /` is the browser app: up to two camera panes, a live 3D model of the |
| 144 | 146 | arm (its own URDF and meshes, drag a joint and the arm follows), a slider and |
| @@ -297,6 +299,9 @@ its live pose and the cameras. Driving needs a name and a turn. | ||
| 297 | 299 | the queue, delete messages, mute people and ban them (long-press or |
| 298 | 300 | right-click a chat line). |
| 299 | 301 | The owner can also press **Stop** during anyone's turn. |
| 302 | - The red **STOP** button (or Esc) is the emergency stop. | |
| 303 | The driver and the owner can press it. Only the owner can clear it, and | |
| 304 | only the owner can change the settings. | |
| 300 | 305 | - The owner's **word filter** (under Admin in the page) lists words and |
| 301 | 306 | phrases; a chat line holding one whole is refused with an error and never |
| 302 | 307 | posted. Case, accents and look-alike digits (`h3ck`) are folded; the owner is |
| @@ -432,6 +437,40 @@ and trim. On a stream, a joint that pushes hard without moving is a jam: it | ||
| 432 | 437 | is parked where it is and follows again once the leader backs off. Jams, |
| 433 | 438 | blocks, stalls, peak loads and temperatures go to `metrics.jsonl`. |
| 434 | 439 | |
| 440 | **Safety limits.** The arm server enforces these on every command, whoever | |
| 441 | sends it and whatever the page does (`src/safety.ts`). The defaults are | |
| 442 | conservative. The owner changes them in the settings panel, or with | |
| 443 | `PUT /settings`; they are kept in `settings.json`. | |
| 444 | ||
| 445 | | setting | default | what it does | | |
| 446 | | --- | --- | --- | | |
| 447 | | `jointLimitsDeg` | the calibrated range | per joint, `[min, max]` in degrees from the middle of its range; a target outside is pulled in (`null` removes the limit) | | |
| 448 | | `maxSpeedDegS` | 90 | no joint turns faster, on the stream, a move or a playback (a playback is slowed to fit) | | |
| 449 | | `maxAccelDegS2` | 360 | no joint speeds up or slows down faster; also written to the servos' own acceleration register | | |
| 450 | | `rateHz` | 60 | most commands per second; stream frames that come faster are merged, the latest winning | | |
| 451 | | `holdAfterMs` | 1000 | the dead-man: a stream silent this long makes the arm hold where it is | | |
| 452 | | `parkPose` | `park` | where the arm goes, slowly, when its driver's connection is lost (a stream that drops or stops answering pings, not one that is closed); with no pose of that name, the middle of every arm joint's range | | |
| 453 | | `parkSpeedDegS` | 20 | how fast the park move goes | | |
| 454 | | `gripperTorquePercent` | 35 | the gripper's force limit on the stream, `/grip`, moves and playbacks, and while it holds | | |
| 455 | ||
| 456 | The stream's targets go through a governor. At the command rate, it moves | |
| 457 | each joint's goal toward the target on a trapezoid: it never goes past the | |
| 458 | speed, never changes speed faster than the acceleration, and brakes in time | |
| 459 | not to overshoot. `POST /estop` halts whatever drives the arm (a stream is | |
| 460 | closed with code 4423), sets every goal to where the joint is, and turns the | |
| 461 | torque on. After that, every move, the torque and the stream are refused | |
| 462 | with 423 until `POST /estop/clear`. The stop is written to `estop.json`, so | |
| 463 | it survives a restart. Behind the relay, `by` is the signed-in name; the | |
| 464 | page can't set it. It is kept as one line of at most 64 characters, and | |
| 465 | pages show it as text, never as HTML. | |
| 466 | ||
| 467 | > **The roles hold only behind the relay.** The arm server has no sign-in. | |
| 468 | > Anyone who reaches its port can call `POST /estop/clear` and `PUT /settings`, | |
| 469 | > and can put any name in `by`. Admin-only clearing and settings, and the | |
| 470 | > driver's E-stop, are enforced by the relay (`src/relay.ts`). Keep the arm | |
| 471 | > server's port on the Pi's private network (Tailscale, or a firewall that only | |
| 472 | > lets the app's host in), and never expose it publicly. | |
| 473 | ||
| 435 | 474 | On the Pi, camera 1 is pinned by its `/dev/v4l/by-id` name in |
| 436 | 475 | `/etc/so101/camera1` and served by `[email protected]` on port 8080; |
| 437 | 476 | `[email protected]` serves the first other camera on 8081 as soon as it |
Software/so101/src/leader-arm.test.ts +3
| @@ -205,6 +205,9 @@ describe("driving the follower with it", () => { | ||
| 205 | 205 | ws.send(JSON.stringify({ 1: 2700, 2: 2148, 6: 2048 })); |
| 206 | 206 | expect(await until(async () => (await followerAt(port))[1] === 2700)).toBe(true); |
| 207 | 207 | ws.send(JSON.stringify({ 1: 4000, 2: 2148, 6: 2048 })); |
| 208 | // The speed governor ramps toward the target, so wait for the arm to arrive; it stops at the clamp, never past it. | |
| 209 | expect(await until(async () => (await followerAt(port))[1] === 2970)).toBe(true); | |
| 210 | ws.send(JSON.stringify({ 1: 4000, 2: 2148, 6: 2048 })); | |
| 208 | 211 | await sleep(150); |
| 209 | 212 | expect((await followerAt(port))[1]).toBe(2970); |
| 210 | 213 | await leader.close(); |
Software/so101/src/lobby.test.ts +50
| @@ -234,6 +234,56 @@ describe("lobby", () => { | ||
| 234 | 234 | expect((await late.first((m) => m.type === "chat")).messages).toEqual([]); |
| 235 | 235 | }); |
| 236 | 236 | |
| 237 | test("the driver and any admin may hit the emergency stop; only an admin clears it or changes the settings", async () => { | |
| 238 | const port = await start(); | |
| 239 | const alice = await join(port, ALICE); | |
| 240 | await join(port, BOB); | |
| 241 | alice.send({ type: "join" }); | |
| 242 | await alice.next(drivingIs("alice")); | |
| 243 | const post = (path: string, headers: Record<string, string>, body: unknown = {}, method = "POST") => | |
| 244 | fetch(`http://127.0.0.1:${port}${path}`, { method, headers: { "content-type": "application/json", ...headers }, body: JSON.stringify(body) }); | |
| 245 | expect((await post("/estop", {})).status).toBe(401); | |
| 246 | expect((await post("/estop", BOB)).status).toBe(403); | |
| 247 | const hit = (await (await post("/estop", ALICE, { by: "somebody else" })).json()) as { estop: { by: string } }; | |
| 248 | expect(hit.estop.by).toBe("alice"); // the relay says who, from the signed identity, never the page | |
| 249 | expect((await post("/estop", OWNER)).status).toBe(200); // an admin may, without the turn | |
| 250 | expect((await torque(port, ALICE)).status).toBe(423); // stopped: nothing moves | |
| 251 | expect((await post("/estop/clear", ALICE)).status).toBe(403); | |
| 252 | expect((await post("/settings", ALICE, { maxSpeedDegS: 360 }, "PUT")).status).toBe(403); | |
| 253 | expect((await post("/settings", OWNER, { maxSpeedDegS: 60 }, "PUT")).status).toBe(200); | |
| 254 | expect((await post("/settings", OWNER, { maxSpeedDegS: 90 }, "PUT")).status).toBe(200); | |
| 255 | expect((await post("/estop/clear", OWNER)).status).toBe(200); | |
| 256 | expect((await torque(port, ALICE)).status).toBe(200); | |
| 257 | }); | |
| 258 | ||
| 259 | test("a driver's browser that drops mid-stream parks the arm; one that closes the stream leaves it holding", async () => { | |
| 260 | const port = await start(); | |
| 261 | const alice = await join(port, ALICE); | |
| 262 | alice.send({ type: "join" }); | |
| 263 | await alice.next(drivingIs("alice")); | |
| 264 | const sim = `http://127.0.0.1:${simulation.port}`; | |
| 265 | const pan = async () => ((await (await fetch(`${sim}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 1)!.position; | |
| 266 | const driving = async () => ((await (await fetch(`${sim}/status`)).json()) as { driving: string | null }).driving; | |
| 267 | const stream = async () => { | |
| 268 | const ws = new WebSocket(`ws://127.0.0.1:${port}/follow`, { headers: ALICE }); | |
| 269 | sockets.push(ws); | |
| 270 | await new Promise((r) => ws.once("open", r)); | |
| 271 | ws.send(JSON.stringify({ "1": 2300 })); | |
| 272 | await sleep(80); | |
| 273 | return ws; | |
| 274 | }; | |
| 275 | const settle = async () => { | |
| 276 | for (let i = 0; i < 150 && (await driving()) !== null; i++) await sleep(20); | |
| 277 | }; | |
| 278 | (await stream()).close(); | |
| 279 | await settle(); | |
| 280 | expect(await pan()).toBe(2300); // closed on purpose: held | |
| 281 | (await stream()).terminate(); | |
| 282 | await sleep(50); | |
| 283 | await settle(); | |
| 284 | expect(await pan()).toBe(2048); // lost: parked at the middle of the range (this arm has no "park" pose) | |
| 285 | }); | |
| 286 | ||
| 237 | 287 | /** A fresh browser picks a name: the headers it sends from then on (its cookie, and the address the proxy saw). */ |
| 238 | 288 | const enter = async (port: number, name: string, ip = "198.51.100.1", headers: Record<string, string> = {}) => { |
| 239 | 289 | const r = await fetch(`http://127.0.0.1:${port}/join`, { method: "POST", headers: { "content-type": "application/json", "x-forwarded-for": ip, ...headers }, body: JSON.stringify({ name }) }); |
Software/so101/src/metrics.ts +2 −2
| @@ -7,7 +7,7 @@ | ||
| 7 | 7 | import { appendFileSync, mkdirSync, readFileSync } from "node:fs"; |
| 8 | 8 | import { dirname } from "node:path"; |
| 9 | 9 | |
| 10 | export type EventKind = "jam" | "block" | "stall" | "missing" | "stream" | "move" | "play" | "record" | "hot"; | |
| 10 | export type EventKind = "jam" | "block" | "stall" | "missing" | "stream" | "move" | "play" | "record" | "hot" | "estop" | "estop-clear" | "park"; | |
| 11 | 11 | |
| 12 | 12 | export interface MetricEvent { |
| 13 | 13 | readonly t: string; |
| @@ -27,7 +27,7 @@ const HOT = 55; | ||
| 27 | 27 | const COOL = 50; |
| 28 | 28 | |
| 29 | 29 | export class Metrics { |
| 30 | readonly counters: Record<EventKind, number> = { jam: 0, block: 0, stall: 0, missing: 0, stream: 0, move: 0, play: 0, record: 0, hot: 0 }; | |
| 30 | readonly counters: Record<EventKind, number> = { jam: 0, block: 0, stall: 0, missing: 0, stream: 0, move: 0, play: 0, record: 0, hot: 0, estop: 0, "estop-clear": 0, park: 0 }; | |
| 31 | 31 | readonly peaks = new Map<number, JointPeak>(); |
| 32 | 32 | readonly recent: MetricEvent[] = []; |
| 33 | 33 | readonly since = new Date().toISOString(); |
Software/so101/src/motion.ts +12 −6
| @@ -5,6 +5,8 @@ import { largestStep, type Trajectory } from "./trajectory.ts"; | ||
| 5 | 5 | export interface SupervisorOptions { |
| 6 | 6 | /** Torque cap while supervised, in percent of the servo's maximum. */ |
| 7 | 7 | torquePercent?: number; |
| 8 | /** Lower caps for some joints (the gripper's force limit), percent, servo id → cap. */ | |
| 9 | torqueCaps?: ReadonlyMap<number, number>; | |
| 8 | 10 | /** Targets are pulled this many ticks inside each servo's limits so a move never drives into a hard stop. */ |
| 9 | 11 | marginTicks?: number; |
| 10 | 12 | /** How close a joint must get to count as arrived, in ticks. */ |
| @@ -49,6 +51,7 @@ export class JointSupervisor { | ||
| 49 | 51 | readonly ids: readonly number[]; |
| 50 | 52 | readonly : FeetechBus; |
| 51 | 53 | readonly : number; |
| 54 | readonly : ReadonlyMap<number, number>; | |
| 52 | 55 | readonly : number; |
| 53 | 56 | readonly : number; |
| 54 | 57 | readonly : AbortSignal | undefined; |
| @@ -64,6 +67,7 @@ export class JointSupervisor { | ||
| 64 | 67 | this. = bus; |
| 65 | 68 | this.ids = ids; |
| 66 | 69 | this. = options.torquePercent ?? 70; |
| 70 | this. = options.torqueCaps ?? new Map(); | |
| 67 | 71 | this. = options.marginTicks ?? 30; |
| 68 | 72 | this. = options.tolerance ?? 15; |
| 69 | 73 | this. = options.signal; |
| @@ -89,7 +93,7 @@ export class JointSupervisor { | ||
| 89 | 93 | this..push([min, max]); |
| 90 | 94 | const ceiling = await this..read(id, STS3215.Max_Torque_Limit); |
| 91 | 95 | this..set(id, ceiling); |
| 92 | this..set(id, Math.min(Math.round(this. * 10), ceiling)); | |
| 96 | this..set(id, Math.min(Math.round(Math.min(this., this..get(id) ?? 100) * 10), ceiling)); | |
| 93 | 97 | } |
| 94 | 98 | const start = await this..syncRead(this.ids, STS3215.Present_Position); |
| 95 | 99 | const missing = this.ids.filter((id) => !start.has(id)); |
| @@ -174,9 +178,10 @@ export class JointSupervisor { | ||
| 174 | 178 | await this.release(); |
| 175 | 179 | } |
| 176 | 180 | |
| 177 | /** Put each servo's own torque cap (its Max_Torque_Limit) back; torque itself stays on. */ | |
| 181 | /** Put each servo's own torque cap (its Max_Torque_Limit) back, or its lower cap from `torqueCaps`; torque itself stays on. */ | |
| 178 | 182 | async release(): Promise<void> { |
| 179 | await this..syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, this..get(id) ?? 1000] as const)); | |
| 183 | const cap = (id: number) => Math.min(this..get(id) ?? 1000, Math.round((this..get(id) ?? 100) * 10)); | |
| 184 | await this..syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, cap(id)] as const)); | |
| 180 | 185 | } |
| 181 | 186 | } |
| 182 | 187 | |
| @@ -286,6 +291,7 @@ export interface PlayOptions { | ||
| 286 | 291 | /** Playback speed multiplier, within 0-2. */ |
| 287 | 292 | rate?: number; |
| 288 | 293 | torquePercent?: number; |
| 294 | torqueCaps?: ReadonlyMap<number, number>; | |
| 289 | 295 | signal?: AbortSignal; |
| 290 | 296 | onFrame?: (index: number) => void; |
| 291 | 297 | } |
| @@ -300,7 +306,7 @@ export interface PlayOptions { | ||
| 300 | 306 | export async function playTrajectory( |
| 301 | 307 | bus: FeetechBus, |
| 302 | 308 | trajectory: Trajectory, |
| 303 | { rate = 1, torquePercent = 70, signal, onFrame }: PlayOptions = {}, | |
| 309 | { rate = 1, torquePercent = 70, torqueCaps, signal, onFrame }: PlayOptions = {}, | |
| 304 | 310 | ): Promise<number> { |
| 305 | 311 | const step = largestStep(trajectory); |
| 306 | 312 | if (step > MAX_FRAME_STEP) { |
| @@ -308,8 +314,8 @@ export async function playTrajectory( | ||
| 308 | 314 | } |
| 309 | 315 | if (!(rate > 0 && rate <= 2)) throw new MotionError(`rate must be within 0-2, got ${rate}`); |
| 310 | 316 | const ids = [...trajectory.ids]; |
| 311 | await moveToPose(bus, ids, trajectory.frames[0]!, { speed: 300, torquePercent, signal }); | |
| 312 | const supervisor = new JointSupervisor(bus, ids, { torquePercent, signal, tolerance: 4096 }); | |
| 317 | await moveToPose(bus, ids, trajectory.frames[0]!, { speed: 300, torquePercent, torqueCaps, signal }); | |
| 318 | const supervisor = new JointSupervisor(bus, ids, { torquePercent, torqueCaps, signal, tolerance: 4096 }); | |
| 313 | 319 | let previous = [...(await supervisor.arm(254)).values()]; |
| 314 | 320 | const period = 1000 / (trajectory.hz * rate); |
| 315 | 321 | const started = performance.now(); |
Software/so101/src/relay.ts +37 −6
| @@ -10,7 +10,11 @@ | ||
| 10 | 10 | * |
| 11 | 11 | * With a lobby (src/lobby.ts), anyone may watch but only the person whose turn it is may command the arm: every |
| 12 | 12 | * request that is not a read, and the /follow stream, is refused for anyone else before it reaches an arm. When a |
| 13 | * turn ends, that person's streams are closed and whatever they started is stopped. | |
| 13 | * turn ends, that person's streams are closed and whatever they started is stopped. The driver and any admin may hit | |
| 14 | * the emergency stop (/estop) and /stop; only an admin may clear the stop or change the settings. | |
| 15 | * | |
| 16 | * A /follow stream whose browser goes away without closing it (or stops answering pings) is closed toward the arm | |
| 17 | * with LOST_CLOSE: the arm server parks the arm then, where a stream closed on purpose only holds. | |
| 14 | 18 | * |
| 15 | 19 | * The owner's speed cap (src/speed-cap.ts) is applied here too, to moves, poses, grips, playbacks and every stream frame. |
| 16 | 20 | * |
| @@ -85,6 +89,8 @@ export interface RelayOptions { | ||
| 85 | 89 | readTimeoutMs?: number; |
| 86 | 90 | /** Who may command the arm; without one, anyone who reaches the relay may (a run on one's own machine). */ |
| 87 | 91 | lobby?: Lobby | null; |
| 92 | /** How often each /follow browser is pinged; one that misses a ping is a lost connection. */ | |
| 93 | heartbeatMs?: number; | |
| 88 | 94 | /** Timings of the shared camera feeds. */ |
| 89 | 95 | camera?: FeedOptions; |
| 90 | 96 | /** Every driver's leader calibration; without one they are kept in memory. */ |
| @@ -110,6 +116,12 @@ const LOCAL = /^\/(?:index\.html|leader-map\.js)?$|^\/(?:model|vendor|printables | ||
| 110 | 116 | /** The page's static files: what a first visit downloads, and what gzip is for. */ |
| 111 | 117 | const STATIC = /^\/(?:index\.html)?$|^\/(?:model|vendor|ui)\//; |
| 112 | 118 | const LEADER = "/leader/calibration"; |
| 119 | /** Admins only: clearing an emergency stop and changing the settings (the safety limits among them). */ | |
| 120 | const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings"]); | |
| 121 | /** The driver's, and any admin's at any time. */ | |
| 122 | const ADMIN_TOO = new Set(["POST /stop", "POST /estop"]); | |
| 123 | /** The close code that tells the arm server its driver was lost, not done: it parks the arm. */ | |
| 124 | export const LOST_CLOSE = 4001; | |
| 113 | 125 | const MAX_JOIN_BODY = 1024; |
| 114 | 126 | const FLOOR = { |
| 115 | 127 | "x-content-type-options": "nosniff", |
| @@ -178,6 +190,7 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 178 | 190 | const probeEveryMs = options.probeEveryMs ?? 5000; |
| 179 | 191 | const readTimeoutMs = options.readTimeoutMs ?? 2500; |
| 180 | 192 | const lobby = options.lobby ?? null; |
| 193 | const heartbeatMs = options.heartbeatMs ?? 1000; | |
| 181 | 194 | const cameras = options.cameras ?? new CameraHub({ feed: options.camera, seed: remote ? [{ name: "Camera 1", source: remote.cameras[0]! }] : [] }); |
| 182 | 195 | const leaders = options.leaders ?? new LeaderCalibrations(); |
| 183 | 196 | |
| @@ -457,11 +470,19 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 457 | 470 | res.setHeader("allow", "POST"); |
| 458 | 471 | return json(res, 405, { error: "POST a name" }); |
| 459 | 472 | } |
| 473 | const route = `${req.method} ${url.pathname}`; | |
| 474 | let by = "the arm's page"; | |
| 460 | 475 | if (!reading && lobby) { |
| 461 | 476 | const who = lobby.who(req); |
| 462 | // Stop is the owner's at any time; everything else is the driver's alone. | |
| 463 | const refused = url.pathname === "/stop" && who?.owner ? null : lobby.refusal(who, req); | |
| 477 | const refused = ADMIN_ONLY.has(route) | |
| 478 | ? who?.owner | |
| 479 | ? null | |
| 480 | : { status: who ? 403 : 401, error: "only an admin can do that" } | |
| 481 | : ADMIN_TOO.has(route) && who?.owner | |
| 482 | ? null | |
| 483 | : lobby.refusal(who, req); | |
| 464 | 484 | if (refused) return json(res, refused.status, { error: refused.error }); |
| 485 | by = who!.name; | |
| 465 | 486 | } |
| 466 | 487 | if (req.method === "GET" && url.pathname === "/link") return json(res, 200, link()); |
| 467 | 488 | if (req.method === "GET" && url.pathname === "/status") return await status(res, path); |
| @@ -483,8 +504,9 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 483 | 504 | } |
| 484 | 505 | |
| 485 | 506 | if (req.method === "GET" && STATIC.test(url.pathname) && acceptsGzip(req.headers)) return await packed(res, await ask(simulation + path, req, undefined, null, false), path); |
| 486 | const raw = reading ? undefined : await readBody(req); | |
| 487 | const body = raw && lobby ? capBody(url.pathname, raw, lobby.maxSpeedPercent) : raw; | |
| 507 | // Who stopped or cleared the arm is the relay's to say, never the page's; every other command is held to the admin's speed cap. | |
| 508 | const raw = reading || route === "POST /estop" || route === "POST /estop/clear" ? undefined : await readBody(req); | |
| 509 | const body = reading ? undefined : !raw ? Buffer.from(JSON.stringify({ by })) : lobby ? capBody(url.pathname, raw, lobby.maxSpeedPercent) : raw; | |
| 488 | 510 | if (LOCAL.test(url.pathname) || !live || !remote) return await pass(res, await ask(simulation + path, req, body, null, false)); |
| 489 | 511 | try { |
| 490 | 512 | // A read must answer fast or fall back; a move or a playback answers when the arm arrives, and is only cut when the link is. |
| @@ -570,7 +592,16 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 570 | 592 | if (capped !== null) arm.send(capped); |
| 571 | 593 | }); |
| 572 | 594 | arm.on("close", (code, why) => browser.close(sendable(code) ? code : 1011, why.length ? why.toString() : "the arm server went away")); |
| 573 | browser.on("close", () => arm.close()); | |
| 595 | // A browser that closed the stream on purpose leaves the arm holding; one that vanished leaves it parking. | |
| 596 | browser.on("close", (code) => (code === 1000 || code === 1005 ? arm.close(1000) : arm.close(LOST_CLOSE, "the driver's connection was lost"))); | |
| 597 | let answered = true; | |
| 598 | browser.on("pong", () => (answered = true)); | |
| 599 | const heartbeat = setInterval(() => { | |
| 600 | if (!answered) return browser.terminate(); | |
| 601 | answered = false; | |
| 602 | if (browser.readyState === WebSocket.OPEN) browser.ping(); | |
| 603 | }, heartbeatMs); | |
| 604 | browser.on("close", () => clearInterval(heartbeat)); | |
| 574 | 605 | arm.on("error", () => browser.terminate()); |
| 575 | 606 | browser.on("error", () => arm.terminate()); |
| 576 | 607 | }; |
Software/so101/src/safety.test.ts added +115
| @@ -0,0 +1,115 @@ | ||
| 1 | import { describe, expect, test } from "bun:test"; | |
| 2 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, parseFrame, validateSafety, type Bounds } from "./safety.ts"; | |
| 3 | ||
| 4 | describe("safety settings", () => { | |
| 5 | test("conservative defaults, and every knob checked against its range", () => { | |
| 6 | expect(validateSafety({})).toEqual(DEFAULT_SAFETY); | |
| 7 | expect(DEFAULT_SAFETY.holdAfterMs).toBeLessThanOrEqual(1000); | |
| 8 | expect(DEFAULT_SAFETY.rateHz).toBeLessThanOrEqual(60); | |
| 9 | expect(validateSafety({ maxSpeedDegS: 45 }).maxSpeedDegS).toBe(45); | |
| 10 | expect(() => validateSafety({ maxSpeedDegS: 1000 })).toThrow("maxSpeedDegS must be 30-360"); | |
| 11 | expect(() => validateSafety({ rateHz: 120 })).toThrow("rateHz"); | |
| 12 | expect(() => validateSafety({ gripperTorquePercent: 100 })).toThrow("gripperTorquePercent"); | |
| 13 | expect(() => validateSafety({ holdAfterMs: "1000" })).toThrow("holdAfterMs"); | |
| 14 | expect(() => validateSafety({ parkPose: "../x" })).toThrow("parkPose"); | |
| 15 | }); | |
| 16 | ||
| 17 | test("joint limits name real joints, run min to max, and null puts the calibrated range back", () => { | |
| 18 | const s = validateSafety({ jointLimitsDeg: { elbow_flex: [-40, 60] } }); | |
| 19 | expect(s.jointLimitsDeg).toEqual({ elbow_flex: [-40, 60] }); | |
| 20 | expect(validateSafety({ jointLimitsDeg: { elbow_flex: null } }, s).jointLimitsDeg).toEqual({}); | |
| 21 | expect(() => validateSafety({ jointLimitsDeg: { knee: [0, 1] } })).toThrow('no joint "knee"'); | |
| 22 | expect(() => validateSafety({ jointLimitsDeg: { elbow_flex: [60, -40] } })).toThrow("min below max"); | |
| 23 | expect(() => validateSafety({ jointLimitsDeg: { elbow_flex: [0, 400] } })).toThrow("±180"); | |
| 24 | }); | |
| 25 | }); | |
| 26 | ||
| 27 | describe("joint bounds", () => { | |
| 28 | const calibrated = new Map<number, Bounds>([ | |
| 29 | [1, { min: 818, max: 3487 }], | |
| 30 | [3, { min: 900, max: 3100 }], | |
| 31 | ]); | |
| 32 | ||
| 33 | test("the calibrated range less a margin, narrowed by an admin's limit, never widened", () => { | |
| 34 | const b = jointBounds(calibrated, validateSafety({ jointLimitsDeg: { elbow_flex: [-30, 500 / 11.4] } })); | |
| 35 | expect(b.get(1)).toEqual({ min: 848, max: 3457 }); | |
| 36 | // elbow: middle 2000, -30° = -341 ticks; the upper limit lies beyond the calibrated end, so the end (less margin) wins | |
| 37 | expect(b.get(3)).toEqual({ min: 1659, max: 2499 }); | |
| 38 | const wide = jointBounds(calibrated, validateSafety({ jointLimitsDeg: { elbow_flex: [-170, 170] } })); | |
| 39 | expect(wide.get(3)).toEqual({ min: 930, max: 3070 }); | |
| 40 | }); | |
| 41 | ||
| 42 | test("a target is pulled inside; a servo that is not on this arm has no bound", () => { | |
| 43 | const b = jointBounds(calibrated, DEFAULT_SAFETY); | |
| 44 | expect(bound(b, 1, 100)).toBe(848); | |
| 45 | expect(bound(b, 1, 9999)).toBe(3457); | |
| 46 | expect(bound(b, 1, 2000.6)).toBe(2001); | |
| 47 | expect(bound(b, 9, 2000)).toBeNull(); | |
| 48 | }); | |
| 49 | }); | |
| 50 | ||
| 51 | describe("the governor", () => { | |
| 52 | const bounds = new Map<number, Bounds>([[1, { min: 0, max: 4095 }]]); | |
| 53 | const limits = { speed: 1000, accel: 4000 }; | |
| 54 | ||
| 55 | test("a jump becomes a ramp: never faster than the speed, never a sharper change than the acceleration, no overshoot", () => { | |
| 56 | const g = new Governor(() => bounds, () => limits); | |
| 57 | g.reset(new Map([[1, 1000]])); | |
| 58 | g.aim(1, 3000); | |
| 59 | const dt = 1 / 60; | |
| 60 | let p = 1000; | |
| 61 | let v = 0; | |
| 62 | let steps = 0; | |
| 63 | while (g.moving && steps < 1000) { | |
| 64 | const out = g.step(dt); | |
| 65 | steps++; | |
| 66 | const next = out.find(([id]) => id === 1)?.[1] ?? p; | |
| 67 | const nv = (next - p) / dt; | |
| 68 | expect(Math.abs(next - p)).toBeLessThanOrEqual(limits.speed * dt + 1); // +1: rounding to whole ticks | |
| 69 | expect(Math.abs(nv - v)).toBeLessThanOrEqual(limits.accel * dt + 2 / dt); | |
| 70 | expect(next).toBeLessThanOrEqual(3000); | |
| 71 | p = next; | |
| 72 | v = nv; | |
| 73 | } | |
| 74 | expect(p).toBe(3000); | |
| 75 | // 2000 ticks at 1000 ticks/s with 0.25 s ramps each way: about 2.25 s | |
| 76 | expect(steps * dt).toBeGreaterThan(2); | |
| 77 | expect(steps * dt).toBeLessThan(2.6); | |
| 78 | }); | |
| 79 | ||
| 80 | test("goals are pulled inside the bounds; a pinned joint stops dead; unknown joints are ignored", () => { | |
| 81 | const g = new Governor(() => new Map([[1, { min: 900, max: 1100 }]]), () => limits); | |
| 82 | g.reset(new Map([[1, 1000]])); | |
| 83 | expect(g.aim(1, 5000)).toBe(1100); | |
| 84 | expect(g.aim(7, 1000)).toBeNull(); | |
| 85 | g.step(0.05); | |
| 86 | g.pin(1, 1003); | |
| 87 | expect(g.moving).toBe(false); | |
| 88 | expect(g.step(0.05)).toEqual([]); | |
| 89 | }); | |
| 90 | ||
| 91 | test("a slow cap holds until the joint arrives", () => { | |
| 92 | const g = new Governor(() => bounds, () => limits); | |
| 93 | g.reset(new Map([[1, 1000]])); | |
| 94 | g.aim(1, 1500, 100); | |
| 95 | for (let i = 0; i < 30; i++) g.step(1 / 60); | |
| 96 | const [[, p]] = g.step(1 / 60) as [readonly [number, number]]; | |
| 97 | expect(p).toBeLessThanOrEqual(1000 + 100 * (31 / 60) + 1); | |
| 98 | }); | |
| 99 | ||
| 100 | test("the limits in servo units, and the acceleration register never 0 (which would mean no limit)", () => { | |
| 101 | expect(motionLimits({ ...DEFAULT_SAFETY, maxSpeedDegS: 90, maxAccelDegS2: 360 })).toEqual({ speed: 1024, accel: 4096 }); | |
| 102 | expect(accelRegister(4096)).toBe(41); | |
| 103 | expect(accelRegister(1)).toBe(1); | |
| 104 | expect(accelRegister(1e9)).toBe(254); | |
| 105 | }); | |
| 106 | }); | |
| 107 | ||
| 108 | describe("frames off the wire", () => { | |
| 109 | test("an object of id: number; everything else is nothing", () => { | |
| 110 | expect(parseFrame('{"1": 2048, "6": 2000.5}')).toEqual(new Map([[1, 2048], [6, 2000.5]])); | |
| 111 | expect(parseFrame('{"1": "2048", "x": 1, "2": null, "__proto__": 5, "3": 1e999}')).toEqual(new Map()); | |
| 112 | expect(parseFrame("[1,2]")).toBeNull(); | |
| 113 | expect(parseFrame("nope")).toBeNull(); | |
| 114 | }); | |
| 115 | }); |
Software/so101/src/safety.ts added +236
| @@ -0,0 +1,236 @@ | ||
| 1 | /** | |
| 2 | * The envelope no client can move the arm outside: per-joint position limits, a speed and an acceleration | |
| 3 | * ceiling, a command rate, the dead-man's timings, where to park, and how hard the gripper may squeeze. | |
| 4 | * Plain data in, plain data out; the server (src/server.ts) applies it to every move, playback and stream. | |
| 5 | * | |
| 6 | * Angles are degrees from the middle of each servo's calibrated range (the model's zero, LeRobot's convention); | |
| 7 | * the servo itself counts 4096 ticks per turn. | |
| 8 | */ | |
| 9 | ||
| 10 | import { clamp } from "./mapping.ts"; | |
| 11 | import { JOINTS } from "./sts3215.ts"; | |
| 12 | ||
| 13 | export interface SafetySettings { | |
| 14 | /** Fastest any joint may turn, degrees per second. */ | |
| 15 | maxSpeedDegS: number; | |
| 16 | /** Fastest any joint may speed up or slow down, degrees per second squared. */ | |
| 17 | maxAccelDegS2: number; | |
| 18 | /** Most commands per second sent to the arm; stream frames that arrive faster are merged. */ | |
| 19 | rateHz: number; | |
| 20 | /** The dead-man: a stream silent this long makes the arm hold where it is, ms. */ | |
| 21 | holdAfterMs: number; | |
| 22 | /** The saved pose the arm parks in when its driver's connection is lost; the middle of every range when no pose has that name. */ | |
| 23 | parkPose: string; | |
| 24 | /** How fast the park move goes, degrees per second. */ | |
| 25 | parkSpeedDegS: number; | |
| 26 | /** Torque cap on the gripper whatever drives it, percent of the servo's maximum. */ | |
| 27 | gripperTorquePercent: number; | |
| 28 | /** Per joint, the narrower range it may be sent to, degrees from its middle; joints not named keep their calibrated range. */ | |
| 29 | jointLimitsDeg: Record<string, [number, number]>; | |
| 30 | } | |
| 31 | ||
| 32 | export const DEFAULT_SAFETY: SafetySettings = { | |
| 33 | maxSpeedDegS: 90, | |
| 34 | maxAccelDegS2: 360, | |
| 35 | rateHz: 60, | |
| 36 | holdAfterMs: 1000, | |
| 37 | parkPose: "park", | |
| 38 | parkSpeedDegS: 20, | |
| 39 | gripperTorquePercent: 35, | |
| 40 | jointLimitsDeg: {}, | |
| 41 | }; | |
| 42 | ||
| 43 | /** Each number knob and the range an admin may set it within. */ | |
| 44 | const RANGES = { | |
| 45 | maxSpeedDegS: [30, 360], | |
| 46 | maxAccelDegS2: [30, 3600], | |
| 47 | rateHz: [5, 60], | |
| 48 | holdAfterMs: [100, 5000], | |
| 49 | parkSpeedDegS: [5, 60], | |
| 50 | gripperTorquePercent: [10, 60], | |
| 51 | } as const satisfies Record<string, readonly [number, number]>; | |
| 52 | ||
| 53 | export const SAFETY_KEYS = [...Object.keys(RANGES), "parkPose", "jointLimitsDeg"] as const; | |
| 54 | ||
| 55 | export const TICKS_PER_DEGREE = 4096 / 360; | |
| 56 | /** No target comes closer than this to a servo's calibrated end, ticks. */ | |
| 57 | export const END_MARGIN = 30; | |
| 58 | ||
| 59 | export class SafetyError extends Error {} | |
| 60 | ||
| 61 | /** The safety part of a settings object, checked field by field; throws SafetyError naming the first bad one. */ | |
| 62 | export function validateSafety(input: Partial<Record<keyof SafetySettings, unknown>>, base: SafetySettings = DEFAULT_SAFETY): SafetySettings { | |
| 63 | const out: SafetySettings = { ...base, jointLimitsDeg: { ...base.jointLimitsDeg } }; | |
| 64 | for (const [key, [lo, hi]] of Object.entries(RANGES) as Array<[keyof typeof RANGES, readonly [number, number]]>) { | |
| 65 | const value = input[key]; | |
| 66 | if (value === undefined) continue; | |
| 67 | if (typeof value !== "number" || !Number.isFinite(value) || value < lo || value > hi) throw new SafetyError(`${key} must be ${lo}-${hi}`); | |
| 68 | out[key] = value; | |
| 69 | } | |
| 70 | if (input.parkPose !== undefined) { | |
| 71 | if (typeof input.parkPose !== "string" || !/^[A-Za-z0-9][A-Za-z0-9_.-]{0,63}$/.test(input.parkPose)) throw new SafetyError("parkPose must be a pose name"); | |
| 72 | out.parkPose = input.parkPose; | |
| 73 | } | |
| 74 | if (input.jointLimitsDeg !== undefined) { | |
| 75 | const limits = input.jointLimitsDeg; | |
| 76 | if (typeof limits !== "object" || limits === null || Array.isArray(limits)) throw new SafetyError("jointLimitsDeg must be an object of joint: [min, max]"); | |
| 77 | for (const [name, range] of Object.entries(limits)) { | |
| 78 | if (!Object.values(JOINTS).includes(name)) throw new SafetyError(`no joint "${name}"`); | |
| 79 | if (range === null) { | |
| 80 | delete out.jointLimitsDeg[name]; // back to the calibrated range | |
| 81 | continue; | |
| 82 | } | |
| 83 | if (!Array.isArray(range) || range.length !== 2 || !range.every((d) => typeof d === "number" && Number.isFinite(d) && Math.abs(d) <= 180) || range[0] >= range[1]) { | |
| 84 | throw new SafetyError(`the limit for ${name} must be [min, max] in degrees, min below max, within ±180`); | |
| 85 | } | |
| 86 | out.jointLimitsDeg[name] = [range[0], range[1]]; | |
| 87 | } | |
| 88 | } | |
| 89 | return out; | |
| 90 | } | |
| 91 | ||
| 92 | export interface Bounds { | |
| 93 | readonly min: number; | |
| 94 | readonly max: number; | |
| 95 | } | |
| 96 | ||
| 97 | /** | |
| 98 | * Where each joint may be sent, in ticks: its calibrated range less the end margin, narrowed by the admin's | |
| 99 | * limit when there is one. A configured limit that misses the range altogether pins the joint at the nearer end. | |
| 100 | */ | |
| 101 | export function jointBounds(calibrated: ReadonlyMap<number, Bounds>, safety: SafetySettings): Map<number, Bounds> { | |
| 102 | const out = new Map<number, Bounds>(); | |
| 103 | for (const [id, range] of calibrated) { | |
| 104 | let min = range.min + END_MARGIN; | |
| 105 | let max = range.max - END_MARGIN; | |
| 106 | if (max < min) min = max = Math.round((range.min + range.max) / 2); | |
| 107 | const limit = safety.jointLimitsDeg[JOINTS[id] ?? ""]; | |
| 108 | if (limit) { | |
| 109 | const middle = (range.min + range.max) / 2; | |
| 110 | const lo = Math.round(middle + limit[0] * TICKS_PER_DEGREE); | |
| 111 | const hi = Math.round(middle + limit[1] * TICKS_PER_DEGREE); | |
| 112 | [min, max] = [clamp(lo, min, max), clamp(hi, min, max)]; | |
| 113 | } | |
| 114 | out.set(id, { min, max }); | |
| 115 | } | |
| 116 | return out; | |
| 117 | } | |
| 118 | ||
| 119 | /** A target pulled inside a joint's bounds; a joint with no bounds is not this arm's and gets null. */ | |
| 120 | export function bound(bounds: ReadonlyMap<number, Bounds>, id: number, ticks: number): number | null { | |
| 121 | const b = bounds.get(id); | |
| 122 | return b ? clamp(Math.round(ticks), b.min, b.max) : null; | |
| 123 | } | |
| 124 | ||
| 125 | /** The limits in the servo's own units: ticks per second, ticks per second squared. */ | |
| 126 | export function motionLimits(safety: SafetySettings): { speed: number; accel: number } { | |
| 127 | return { speed: Math.round(safety.maxSpeedDegS * TICKS_PER_DEGREE), accel: Math.round(safety.maxAccelDegS2 * TICKS_PER_DEGREE) }; | |
| 128 | } | |
| 129 | ||
| 130 | /** The STS3215 Acceleration register counts 100 ticks/s² per unit; 0 would mean "no limit", so it is never written. */ | |
| 131 | export function accelRegister(accelTicks: number): number { | |
| 132 | return clamp(Math.ceil(accelTicks / 100), 1, 254); | |
| 133 | } | |
| 134 | ||
| 135 | interface Track { | |
| 136 | /** Where the arm was last sent, ticks (fractional: the integer goes on the wire). */ | |
| 137 | p: number; | |
| 138 | /** Its velocity, ticks per second, signed. */ | |
| 139 | v: number; | |
| 140 | goal: number; | |
| 141 | /** A ceiling below the global speed for this joint until it arrives (easing over to a far leader). */ | |
| 142 | slow: number | null; | |
| 143 | } | |
| 144 | ||
| 145 | /** | |
| 146 | * Turns targets that may jump anywhere into goals that never exceed the speed or the acceleration: | |
| 147 | * each tick moves every joint toward its goal along a trapezoid, braking in time not to overshoot. | |
| 148 | * The caller sends what `step` returns to the servos at the command rate. | |
| 149 | */ | |
| 150 | export class Governor { | |
| 151 | readonly : () => ReadonlyMap<number, Bounds>; | |
| 152 | readonly : () => { speed: number; accel: number }; | |
| 153 | readonly = new Map<number, Track>(); | |
| 154 | ||
| 155 | constructor(bounds: () => ReadonlyMap<number, Bounds>, limits: () => { speed: number; accel: number }) { | |
| 156 | this. = bounds; | |
| 157 | this. = limits; | |
| 158 | } | |
| 159 | ||
| 160 | /** Start over from where the joints are: at rest, aiming nowhere else. */ | |
| 161 | reset(positions: ReadonlyMap<number, number>): void { | |
| 162 | this..clear(); | |
| 163 | for (const [id, p] of positions) this.pin(id, p); | |
| 164 | } | |
| 165 | ||
| 166 | /** One joint stops where it is (a jam, a hold): no velocity carried over. */ | |
| 167 | pin(id: number, p: number): void { | |
| 168 | this..set(id, { p, v: 0, goal: p, slow: null }); | |
| 169 | } | |
| 170 | ||
| 171 | /** A new goal for a joint, pulled inside its bounds; `slow` caps its speed until it gets there. Unknown joints are ignored. */ | |
| 172 | aim(id: number, goal: number, slow: number | null = null): number | null { | |
| 173 | const track = this..get(id); | |
| 174 | const g = bound(this.(), id, goal); | |
| 175 | if (!track || g === null) return null; | |
| 176 | track.goal = g; | |
| 177 | if (slow !== null) track.slow = Math.min(track.slow ?? Infinity, slow); | |
| 178 | return g; | |
| 179 | } | |
| 180 | ||
| 181 | goal(id: number): number | undefined { | |
| 182 | return this..get(id)?.goal; | |
| 183 | } | |
| 184 | ||
| 185 | /** Whether any joint has somewhere left to go. */ | |
| 186 | get moving(): boolean { | |
| 187 | for (const t of this..values()) if (t.p !== t.goal || t.v !== 0) return true; | |
| 188 | return false; | |
| 189 | } | |
| 190 | ||
| 191 | /** Advance every joint by `dt` seconds; returns the goals that changed on the wire, rounded to ticks. */ | |
| 192 | step(dt: number): Array<readonly [number, number]> { | |
| 193 | const { speed, accel } = this.(); | |
| 194 | const out: Array<readonly [number, number]> = []; | |
| 195 | for (const [id, t] of this.) { | |
| 196 | if (t.p === t.goal && t.v === 0) continue; | |
| 197 | const before = Math.round(t.p); | |
| 198 | const vmax = Math.min(speed, t.slow ?? Infinity); | |
| 199 | const error = t.goal - t.p; | |
| 200 | // The fastest speed from which the joint can still stop at the goal, slowing by accel·dt each tick: v²/2a + v·dt/2 = |error|. | |
| 201 | const brake = accel * (Math.sqrt((dt * dt) / 4 + (2 * Math.abs(error)) / accel) - dt / 2); | |
| 202 | const wanted = Math.sign(error) * Math.min(vmax, brake); | |
| 203 | const v = clamp(wanted, t.v - accel * dt, t.v + accel * dt); | |
| 204 | const next = t.p + v * dt; | |
| 205 | if ((t.goal - next) * Math.sign(error) <= 0 || Math.abs(t.goal - next) < 0.5) { | |
| 206 | // Arrived (or would cross the goal this tick): settle on it. | |
| 207 | t.p = t.goal; | |
| 208 | t.v = 0; | |
| 209 | t.slow = null; | |
| 210 | } else { | |
| 211 | t.p = next; | |
| 212 | t.v = v; | |
| 213 | } | |
| 214 | const after = Math.round(t.p); | |
| 215 | if (after !== before) out.push([id, after]); | |
| 216 | } | |
| 217 | return out; | |
| 218 | } | |
| 219 | } | |
| 220 | ||
| 221 | /** Joint frames off the wire: an object of servo id → ticks. Anything else is null; values that are not finite numbers are left out. */ | |
| 222 | export function parseFrame(data: string): Map<number, number> | null { | |
| 223 | let parsed: unknown; | |
| 224 | try { | |
| 225 | parsed = JSON.parse(data); | |
| 226 | } catch { | |
| 227 | return null; | |
| 228 | } | |
| 229 | if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) return null; | |
| 230 | const out = new Map<number, number>(); | |
| 231 | for (const [key, value] of Object.entries(parsed)) { | |
| 232 | if (!/^\d{1,3}$/.test(key) || typeof value !== "number" || !Number.isFinite(value)) continue; | |
| 233 | out.set(Number(key), value); | |
| 234 | } | |
| 235 | return out; | |
| 236 | } |
Software/so101/src/server.test.ts +214 −6
| @@ -1,5 +1,6 @@ | ||
| 1 | 1 | import { afterAll, beforeAll, describe, expect, test } from "bun:test"; |
| 2 | 2 | import { existsSync, mkdtempSync, readFileSync, writeFileSync } from "node:fs"; |
| 3 | import { connect } from "node:net"; | |
| 3 | 4 | import { tmpdir } from "node:os"; |
| 4 | 5 | import { join } from "node:path"; |
| 5 | 6 | import WebSocket from "ws"; |
| @@ -54,9 +55,10 @@ describe("so101 serve", () => { | ||
| 54 | 55 | expect(goals).toContainEqual([2, 843]); |
| 55 | 56 | expect(goals).toContainEqual([6, 3495]); |
| 56 | 57 | expect(goals).not.toContainEqual([1, 3000]); |
| 57 | // closing the stream holds at the present position and restores the torque cap | |
| 58 | const caps = fake.writes.filter((w) => w.address === 48).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); | |
| 59 | expect(caps.at(-1)).toBe(1000); | |
| 58 | // closing the stream holds at the present position and restores the torque cap; the gripper keeps its force limit | |
| 59 | const cap = (id: number) => fake.writes.filter((w) => w.address === 48 && w.id === id).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)).at(-1); | |
| 60 | expect(cap(1)).toBe(1000); | |
| 61 | expect(cap(6)).toBe(350); | |
| 60 | 62 | }); |
| 61 | 63 | |
| 62 | 64 | test("a silent stream makes the arm hold", async () => { |
| @@ -100,6 +102,14 @@ describe("viewer page", () => { | ||
| 100 | 102 | expect(await r.text()).toContain("Fullscreen"); |
| 101 | 103 | }); |
| 102 | 104 | |
| 105 | test("never builds HTML from people's text: event details and E-stop names go in as text nodes", async () => { | |
| 106 | const html = await (await fetch(`${base}/`)).text(); | |
| 107 | const sinks = html.split("\n").filter((line) => /\.innerHTML\s*=/.test(line)); | |
| 108 | expect(sinks.length).toBeGreaterThan(0); | |
| 109 | for (const line of sinks) expect(line).not.toMatch(/detail|\.by\b|estop|chat|\bname\b/); | |
| 110 | expect(html).toContain('$("events").replaceChildren('); | |
| 111 | }); | |
| 112 | ||
| 103 | 113 | test("names its modules by their content, serves them for good, and serves nothing else from src/ui", async () => { |
| 104 | 114 | const page = await (await fetch(`${base}/`)).text(); |
| 105 | 115 | expect(page).not.toContain("__VERSION__"); |
| @@ -324,7 +334,7 @@ describe("a bus that fails mid-session", () => { | ||
| 324 | 334 | expect(Object.keys(list)).toEqual(["race"]); |
| 325 | 335 | }); |
| 326 | 336 | |
| 327 | test("/grip owns the arm while it writes, and the next supervised gripper move puts the servo's own cap back", async () => { | |
| 337 | test("/grip owns the arm while it writes, and the next supervised gripper move puts the gripper's force limit back", async () => { | |
| 328 | 338 | const ws = new WebSocket(`ws://127.0.0.1:${srv.port}/follow`); |
| 329 | 339 | await new Promise((r) => ws.once("open", r)); |
| 330 | 340 | expect((await call("/grip", { position: 2500 })).status).toBe(409); // the stream drives |
| @@ -336,7 +346,7 @@ describe("a bus that fails mid-session", () => { | ||
| 336 | 346 | const memory = faulty.servos.get(6)!; |
| 337 | 347 | expect(memory[48]! + memory[49]! * 256).toBe(300); |
| 338 | 348 | expect((await call("/move", { targets: { "6": 2600 } })).status).toBe(200); |
| 339 | expect(memory[48]! + memory[49]! * 256).toBe(1000); // Max_Torque_Limit, not the cap a grip left behind | |
| 349 | expect(memory[48]! + memory[49]! * 256).toBe(350); // the gripper's force limit (35 %), not the cap a grip left behind nor the servo's full 1000 | |
| 340 | 350 | }); |
| 341 | 351 | |
| 342 | 352 | test("/move rejects ids that are not plain integers", async () => { |
| @@ -506,7 +516,7 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 506 | 516 | }; |
| 507 | 517 | await send(mid(2) + 200, 10); // safe, eases |
| 508 | 518 | await send(ticks(2, 80), 4); // dives at the table: dropped as a jump, or refused by the zone |
| 509 | await send(mid(2) - 500, 25); // safe, but 700 ticks from where the joint was held | |
| 519 | await send(mid(2) - 500, 40); // safe, but 700 ticks from where the joint was held; merged to the 60 Hz command rate, still 20 and more | |
| 510 | 520 | await sleep(150); |
| 511 | 521 | const s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; blocked: unknown; stream: { dropped: number } }; |
| 512 | 522 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) - 500); |
| @@ -605,3 +615,201 @@ describe("settings on disk", () => { | ||
| 605 | 615 | await Promise.race([s2.close(), sleep(300)]); |
| 606 | 616 | }); |
| 607 | 617 | }); |
| 618 | ||
| 619 | describe("the safety envelope", () => { | |
| 620 | // Pan 818-3487 (middle 2152.5), lift 813-3202, gripper 1995-3525; no model, so no zone: only the envelope is at work. | |
| 621 | let bus: FakeBus; | |
| 622 | let srv: RunningServer; | |
| 623 | let url: string; | |
| 624 | let dir: string; | |
| 625 | const call = (path: string, body: unknown = {}, method = "POST") => fetch(`${url}${path}`, { method, body: JSON.stringify(body) }); | |
| 626 | const status = async () => (await (await fetch(`${url}/status`)).json()) as { driving: string | null; estop: { by: string } | null; joints: Array<{ id: number; position: number }>; stream: { frames: number; merged: number } | null }; | |
| 627 | const position = async (id: number) => (await status()).joints.find((j) => j.id === id)!.position; | |
| 628 | const goals = (id: number, from = 0) => bus.writes.slice(from).filter((w) => w.id === id && w.address === 42).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); | |
| 629 | const idle = async () => { | |
| 630 | for (let i = 0; i < 200; i++) { | |
| 631 | if ((await status()).driving === null) return; | |
| 632 | await sleep(20); | |
| 633 | } | |
| 634 | throw new Error("the arm never went idle"); | |
| 635 | }; | |
| 636 | const open = async () => { | |
| 637 | const ws = new WebSocket(`ws://127.0.0.1:${srv.port}/follow`); | |
| 638 | await new Promise((r) => ws.once("open", r)); | |
| 639 | return ws; | |
| 640 | }; | |
| 641 | const closed = (ws: WebSocket) => new Promise<number>((r) => ws.once("close", (code) => r(code))); | |
| 642 | const start = () => startServer(new FeetechBus(bus), { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(dir, "poses.json"), watchdogMs: 150, heartbeatMs: 80 }); | |
| 643 | beforeAll(async () => { | |
| 644 | dir = mkdtempSync(join(tmpdir(), "so101-safety-")); | |
| 645 | bus = new FakeBus([1, 2, 6]); | |
| 646 | bus.servos.get(1)!.set([0x32, 0x03, 0x9f, 0x0d], 9); | |
| 647 | bus.servos.get(2)!.set([0x2d, 0x03, 0x82, 0x0c], 9); | |
| 648 | bus.servos.get(6)!.set([0xcb, 0x07, 0xc5, 0x0d], 9); | |
| 649 | srv = await start(); | |
| 650 | url = `http://127.0.0.1:${srv.port}`; | |
| 651 | }); | |
| 652 | afterAll(() => Promise.race([srv.close(), sleep(500)])); | |
| 653 | ||
| 654 | test("out-of-range commands are pulled inside the admin's joint limits, on a move and on the stream", async () => { | |
| 655 | expect((await call("/settings", { jointLimitsDeg: { shoulder_pan: [-20, 20] } }, "PUT")).status).toBe(200); | |
| 656 | const r = (await (await call("/move", { targets: { "1": 3400 } })).json()) as { final: Record<string, number>; adjusted: Record<string, number> }; | |
| 657 | expect(r.adjusted["1"]).toBe(2380); // 2152.5 + 20° × 4096/360 | |
| 658 | expect(r.final["1"]).toBe(2380); | |
| 659 | const before = bus.writes.length; | |
| 660 | const ws = await open(); | |
| 661 | ws.send(JSON.stringify({ "1": 100 })); | |
| 662 | await sleep(60); | |
| 663 | ws.close(); | |
| 664 | await idle(); | |
| 665 | expect(goals(1, before)).toContain(1925); // 2152.5 - 20° | |
| 666 | expect(Math.min(...goals(1, before))).toBeGreaterThanOrEqual(1925); | |
| 667 | expect((await call("/settings", { jointLimitsDeg: { shoulder_pan: null } }, "PUT")).status).toBe(200); | |
| 668 | expect((await call("/settings", { maxSpeedDegS: 9000 }, "PUT")).status).toBe(400); | |
| 669 | }); | |
| 670 | ||
| 671 | test("too-fast commands are clamped: the stream's goals ramp up within the speed and acceleration limits", async () => { | |
| 672 | const ws = await open(); | |
| 673 | ws.send(JSON.stringify({ "1": 2000 })); // the first frame eases over | |
| 674 | await sleep(60); | |
| 675 | const before = bus.writes.length; | |
| 676 | const keep = setInterval(() => ws.send(JSON.stringify({ "1": 2390 })), 20); // 390 ticks at once, again and again | |
| 677 | for (let i = 0; i < 100 && (await position(1)) !== 2390; i++) await sleep(20); | |
| 678 | clearInterval(keep); | |
| 679 | const steps = goals(1, before); | |
| 680 | expect(steps.at(-1)).toBe(2390); | |
| 681 | expect(steps.length).toBeGreaterThan(10); // a ramp, not a jump | |
| 682 | const deltas = steps.map((g, i) => g - (steps[i - 1] ?? 2000)); | |
| 683 | expect(Math.max(...deltas)).toBeLessThanOrEqual(Math.ceil((1024 * 2) / 60) + 1); // 90°/s, a late tick at most twice the 60 Hz step | |
| 684 | expect(deltas[0]).toBeLessThanOrEqual(5); // it starts from rest: 360°/s² allows ~1 tick in the first tick | |
| 685 | ws.close(); | |
| 686 | await idle(); | |
| 687 | }); | |
| 688 | ||
| 689 | test("frames faster than the command rate are merged, the latest winning", async () => { | |
| 690 | const ws = await open(); | |
| 691 | for (let i = 0; i < 100; i++) ws.send(JSON.stringify({ "2": 2000 + (i % 7) })); | |
| 692 | await sleep(80); | |
| 693 | const s = await status(); | |
| 694 | expect(s.stream!.merged).toBeGreaterThanOrEqual(90); | |
| 695 | expect(s.stream!.frames).toBeLessThanOrEqual(5); | |
| 696 | ws.close(); | |
| 697 | await idle(); | |
| 698 | }); | |
| 699 | ||
| 700 | test("silence makes the arm hold where it is, even halfway through a governed move", async () => { | |
| 701 | const ws = await open(); | |
| 702 | ws.send(JSON.stringify({ "1": 2000 })); | |
| 703 | await sleep(60); | |
| 704 | ws.send(JSON.stringify({ "1": 2390 })); // then nothing: the dead-man holds after 150 ms | |
| 705 | await sleep(400); | |
| 706 | const held = await position(1); | |
| 707 | expect(held).toBeGreaterThan(2000); | |
| 708 | expect(held).toBeLessThan(2390); | |
| 709 | const count = goals(1).length; | |
| 710 | await sleep(200); | |
| 711 | expect(goals(1).length).toBe(count); // nothing more is sent | |
| 712 | ws.close(); | |
| 713 | await idle(); | |
| 714 | }); | |
| 715 | ||
| 716 | test("a stream closed on purpose holds; a lost one parks the arm slowly at the park pose", async () => { | |
| 717 | await call("/move", { targets: { "1": 1800, "2": 1900 } }); | |
| 718 | expect((await call("/pose/save", { name: "park", ids: [1, 2] })).status).toBe(200); | |
| 719 | await call("/move", { targets: { "1": 2000, "2": 2000 } }); | |
| 720 | let ws = await open(); | |
| 721 | ws.send(JSON.stringify({ "1": 2050 })); | |
| 722 | await sleep(60); | |
| 723 | ws.close(); // on purpose | |
| 724 | await idle(); | |
| 725 | expect(await position(1)).toBe(2050); | |
| 726 | ws = await open(); | |
| 727 | ws.send(JSON.stringify({ "1": 2100 })); | |
| 728 | await sleep(60); | |
| 729 | ws.terminate(); // the connection just drops | |
| 730 | let parking = false; | |
| 731 | for (let i = 0; i < 100; i++) { | |
| 732 | const s = await status(); | |
| 733 | parking ||= s.driving === "park"; | |
| 734 | if (parking && s.driving === null) break; | |
| 735 | await sleep(20); | |
| 736 | } | |
| 737 | expect(parking).toBe(true); | |
| 738 | expect(await position(1)).toBe(1800); | |
| 739 | expect(await position(2)).toBe(1900); | |
| 740 | const velocity = bus.writes.filter((w) => w.id === 1 && w.address === 46).at(-1)!; | |
| 741 | expect(velocity.bytes[0]! | (velocity.bytes[1]! << 8)).toBeLessThanOrEqual(Math.round(20 * (4096 / 360))); // the park speed | |
| 742 | const m = (await (await fetch(`${url}/metrics`)).json()) as { recent: Array<{ kind: string; detail: { parked?: boolean } }> }; | |
| 743 | expect(m.recent.some((e) => e.kind === "park" && e.detail.parked === true)).toBe(true); | |
| 744 | }); | |
| 745 | ||
| 746 | test("a client that stops answering pings is a lost connection, and the arm parks", async () => { | |
| 747 | await call("/move", { targets: { "1": 2000 } }); | |
| 748 | // A bare socket: it shakes hands, sends one frame, then never answers anything, like a browser on a dead network. | |
| 749 | const socket = connect(srv.port, "127.0.0.1"); | |
| 750 | await new Promise((r) => socket.once("connect", r)); | |
| 751 | socket.write(`GET /follow HTTP/1.1\r\nhost: 127.0.0.1\r\nupgrade: websocket\r\nconnection: Upgrade\r\nsec-websocket-key: ${Buffer.from("0123456789abcdef").toString("base64")}\r\nsec-websocket-version: 13\r\n\r\n`); | |
| 752 | await new Promise((r) => socket.once("data", r)); // 101 Switching Protocols | |
| 753 | const payload = Buffer.from(JSON.stringify({ "1": 2050 })); | |
| 754 | socket.write(Buffer.concat([Buffer.from([0x81, 0x80 | payload.length, 0, 0, 0, 0]), payload])); // masked with a zero key | |
| 755 | const gone = new Promise((r) => socket.once("close", r)); | |
| 756 | let parking = false; | |
| 757 | for (let i = 0; i < 100 && !parking; i++) { | |
| 758 | parking = (await status()).driving === "park"; | |
| 759 | await sleep(10); | |
| 760 | } | |
| 761 | await gone; // the server gave up on it after one unanswered ping | |
| 762 | expect(parking).toBe(true); | |
| 763 | await idle(); | |
| 764 | expect(await position(1)).toBe(1800); | |
| 765 | }); | |
| 766 | ||
| 767 | test("the gripper's force is limited whatever is asked", async () => { | |
| 768 | const r = (await (await call("/grip", { position: 2500, torque: 60 })).json()) as { torque: number }; | |
| 769 | expect(r.torque).toBe(35); | |
| 770 | const memory = bus.servos.get(6)!; | |
| 771 | expect(memory[48]! + memory[49]! * 256).toBe(350); | |
| 772 | }); | |
| 773 | ||
| 774 | test("the emergency stop halts a stream, holds with torque, refuses every move until an admin clears it, and survives a restart", async () => { | |
| 775 | const ws = await open(); | |
| 776 | ws.send(JSON.stringify({ "1": 2100 })); | |
| 777 | await sleep(60); | |
| 778 | const ended = closed(ws); | |
| 779 | const r = (await (await call("/estop", { by: "tester" })).json()) as { estop: { by: string }; stopped: string }; | |
| 780 | expect(r.estop.by).toBe("tester"); | |
| 781 | expect(r.stopped).toBe("stream"); | |
| 782 | expect(await ended).toBe(4423); | |
| 783 | await idle(); | |
| 784 | for (const id of [1, 2, 6]) expect(bus.servos.get(id)![40]).toBe(1); // torque on: held | |
| 785 | expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(423); | |
| 786 | expect((await call("/torque", { on: false })).status).toBe(423); | |
| 787 | expect((await call("/grip", { position: 2500 })).status).toBe(423); | |
| 788 | expect((await call("/pose/go", { name: "park" })).status).toBe(423); | |
| 789 | const again = new WebSocket(`ws://127.0.0.1:${srv.port}/follow`); | |
| 790 | expect(await closed(again)).toBe(4423); | |
| 791 | expect((await status()).estop?.by).toBe("tester"); | |
| 792 | // A restart keeps it. | |
| 793 | await Promise.race([srv.close(), sleep(500)]); | |
| 794 | srv = await start(); | |
| 795 | url = `http://127.0.0.1:${srv.port}`; | |
| 796 | expect((await status()).estop?.by).toBe("tester"); | |
| 797 | expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(423); | |
| 798 | expect(((await (await call("/estop/clear", { by: "admin" })).json()) as { cleared: boolean }).cleared).toBe(true); | |
| 799 | expect(existsSync(join(dir, "estop.json"))).toBe(false); | |
| 800 | expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(200); | |
| 801 | }); | |
| 802 | ||
| 803 | test("the name an E-stop is credited to stays one line of plain data, markup and all, in /status and /metrics", async () => { | |
| 804 | const name = '<img src=x onerror="alert(1)">'; | |
| 805 | await call("/estop", { by: ` ${name}\n\u0000\u202eadmin ` }); | |
| 806 | const by = `${name} admin`; | |
| 807 | expect((await status()).estop?.by).toBe(by); | |
| 808 | expect((await (await fetch(`${url}/status`)).text())).toContain(JSON.stringify(by)); // JSON-escaped, never interpreted | |
| 809 | await call("/estop/clear", { by: "x".repeat(200) }); | |
| 810 | const recent = ((await (await fetch(`${url}/metrics`)).json()) as { recent: Array<{ kind: string; detail: { by?: string } }> }).recent; | |
| 811 | expect(recent.findLast((e) => e.kind === "estop")?.detail.by).toBe(by); | |
| 812 | expect(recent.findLast((e) => e.kind === "estop-clear")?.detail.by).toBe("x".repeat(64)); | |
| 813 | expect((await status()).estop).toBeNull(); | |
| 814 | }); | |
| 815 | }); |
Software/so101/src/server.ts +281 −53
| @@ -13,6 +13,8 @@ | ||
| 13 | 13 | * POST /grip {"position": 2000, "torque": 30, "speed"?: 300} close the gripper toward a position at a low torque cap and hold |
| 14 | 14 | * POST /torque {"on": true|false} |
| 15 | 15 | * POST /stop end whatever drives the arm: a move, a playback, a recording, or a stream; the arm holds |
| 16 | * POST /estop {"by"?: "name"} emergency stop: halt everything, hold with torque, refuse every move until cleared | |
| 17 | * POST /estop/clear {"by"?: "name"} | |
| 16 | 18 | * GET /recordings the saved recordings with their length |
| 17 | 19 | * POST /record/start {"name": "wave", "hz"?: 30} torque off, sample the joints until /record/stop |
| 18 | 20 | * POST /record/stop save the recording |
| @@ -20,7 +22,8 @@ | ||
| 20 | 22 | * POST /play {"name": "wave", "rate"?: 1, "torque"?: 70, "release"?: false} |
| 21 | 23 | * GET /calibration {"camera": {...}|null, "handeye": {...}|null, ...} what the vision kit published |
| 22 | 24 | * PUT /calibration/<kind> store that JSON next to the poses (kind: camera, camera2, handeye, ...) |
| 23 | * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"} | |
| 25 | * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"}, | |
| 26 | * and the safety limits of src/safety.ts (speed, acceleration, rate, dead-man, park, gripper force, joint limits) | |
| 24 | 27 | * PUT /settings change any of those |
| 25 | 28 | * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events |
| 26 | 29 | * GET /model.json the arm model: joints, the table plane, where the URDF is served |
| @@ -30,7 +33,8 @@ | ||
| 30 | 33 | * GET /printables the sheets to print for the vision quest (board, cube markers, gripper marker); GET /printables/<file> |
| 31 | 34 | * WS /follow client sends {"1": ticks, "2": ticks, ...} frames; the server drives the arm |
| 32 | 35 | * |
| 33 | * One thing drives the arm at a time. Every target, whether a move or a stream frame, is run through | |
| 36 | * One thing drives the arm at a time. Every target is pulled inside the joint limits and every motion held to the speed and | |
| 37 | * acceleration limits (src/safety.ts); a stream that goes silent holds, one whose connection is lost parks. Every target, whether a move or a stream frame, is run through | |
| 34 | 38 | * the arm's own geometry first: a pose that would push a part into the table or the base is refused |
| 35 | 39 | * (or only reported, in "warn" mode). On the stream, a joint that pushes hard without moving is a jam: |
| 36 | 40 | * its goal is parked where it is, and it follows again once the leader backs off. |
| @@ -50,7 +54,8 @@ import { Metrics } from "./metrics.ts"; | ||
| 50 | 54 | import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts"; |
| 51 | 55 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; |
| 52 | 56 | import { JOINTS, STS3215, ticksToDegrees } from "./sts3215.ts"; |
| 53 | import { parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts"; | |
| 57 | import { largestStep, parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts"; | |
| 58 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, parseFrame, SAFETY_KEYS, SafetyError, TICKS_PER_DEGREE, validateSafety, type SafetySettings } from "./safety.ts"; | |
| 54 | 59 | |
| 55 | 60 | export interface ServerOptions { |
| 56 | 61 | host?: string; |
| @@ -66,8 +71,12 @@ export interface ServerOptions { | ||
| 66 | 71 | streamTorque?: number; |
| 67 | 72 | /** Largest per-frame change accepted on the stream, ticks. */ |
| 68 | 73 | maxStep?: number; |
| 69 | /** With no frame for this long, the arm is told to hold where it is. */ | |
| 74 | /** With no frame for this long, the arm is told to hold where it is; overrides the holdAfterMs setting. */ | |
| 70 | 75 | watchdogMs?: number; |
| 76 | /** How often a stream's client is pinged; one that misses a ping is a lost connection, and the arm parks. */ | |
| 77 | heartbeatMs?: number; | |
| 78 | /** Starting values for the safety settings, before what is stored (src/safety.ts). */ | |
| 79 | safety?: Partial<SafetySettings>; | |
| 71 | 80 | /** Speed used to catch up with the first frame of a stream, ticks/s. */ |
| 72 | 81 | easeSpeed?: number; |
| 73 | 82 | /** How often the stream's jam watch samples position and load, ms. */ |
| @@ -79,9 +88,9 @@ export interface RunningServer { | ||
| 79 | 88 | close(): Promise<void>; |
| 80 | 89 | } |
| 81 | 90 | |
| 82 | export type Driver = "stream" | "move" | "play" | "record"; | |
| 91 | export type Driver = "stream" | "move" | "play" | "record" | "park"; | |
| 83 | 92 | |
| 84 | export interface Settings { | |
| 93 | export interface Settings extends SafetySettings { | |
| 85 | 94 | zone: "enforce" | "warn" | "off"; |
| 86 | 95 | floorMarginMm: number; |
| 87 | 96 | baseMarginMm: number; |
| @@ -89,7 +98,27 @@ export interface Settings { | ||
| 89 | 98 | jointOffsetsDeg: Record<string, number>; |
| 90 | 99 | } |
| 91 | 100 | |
| 92 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {} }; | |
| 101 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 102 | ||
| 103 | /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */ | |
| 104 | export interface EStop { | |
| 105 | by: string; | |
| 106 | at: string; | |
| 107 | } | |
| 108 | ||
| 109 | /** | |
| 110 | * Who an E-stop or its clearing is credited to: the body's `by`, one line of at most 64 characters, kept as text | |
| 111 | * (pages show it with textContent). It is only a label: on this server it is unauthenticated, and the relay sets it. | |
| 112 | */ | |
| 113 | export function actor(body: Record<string, unknown>): string { | |
| 114 | const by = typeof body.by === "string" ? body.by.replace(/[\p{Cc}\p{Cf}\s]+/gu, " ").trim().slice(0, 64).trim() : ""; | |
| 115 | return by || "someone at the arm"; | |
| 116 | } | |
| 117 | ||
| 118 | /** Close codes a stream ends with on purpose; any other end is a lost connection. */ | |
| 119 | const CLEAN_CLOSE = new Set([1000, 1005]); | |
| 120 | /** The close code for a stream ended by an emergency stop. */ | |
| 121 | export const ESTOP_CLOSE = 4423; | |
| 93 | 122 | |
| 94 | 123 | class HttpError extends Error { |
| 95 | 124 | readonly status: number; |
| @@ -120,10 +149,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 120 | 149 | const settingsFile = join(dataDir, "settings.json"); |
| 121 | 150 | const streamTorque = options.streamTorque ?? 70; |
| 122 | 151 | const maxStep = options.maxStep ?? 400; |
| 123 | const watchdogMs = options.watchdogMs ?? 500; | |
| 124 | 152 | const easeSpeed = options.easeSpeed ?? 300; |
| 125 | 153 | const jamSampleMs = options.jamSampleMs ?? 100; |
| 126 | const margin = 30; | |
| 154 | const heartbeatMs = options.heartbeatMs ?? 1000; | |
| 155 | const estopFile = join(dataDir, "estop.json"); | |
| 156 | let safetyBase: SafetySettings; | |
| 157 | try { | |
| 158 | safetyBase = validateSafety(options.safety ?? {}); | |
| 159 | } catch (err) { | |
| 160 | throw new Error(`safety options: ${(err as Error).message}`); | |
| 161 | } | |
| 127 | 162 | const metrics = new Metrics(join(dataDir, "metrics.jsonl")); |
| 128 | 163 | |
| 129 | 164 | const limits = new Map<number, { min: number; max: number }>(); |
| @@ -147,7 +182,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 147 | 182 | } |
| 148 | 183 | } else console.error(`model: no ${URDF} in ${modelDir ?? "(no model dir)"}; no safe zone and no 3D view`); |
| 149 | 184 | |
| 150 | let settings: Settings = { ...DEFAULT_SETTINGS }; | |
| 185 | let settings: Settings = { ...DEFAULT_SETTINGS, ...safetyBase }; | |
| 151 | 186 | if (existsSync(settingsFile)) { |
| 152 | 187 | // Field by field: one bad value must not throw away the zone mode or the other trims. |
| 153 | 188 | try { |
| @@ -207,6 +242,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 207 | 242 | let driver: Driver | null = null; |
| 208 | 243 | let current: AbortController | null = null; |
| 209 | 244 | const claim = (kind: Driver): AbortController => { |
| 245 | if (estop) throw new HttpError(423, stopped(estop)); | |
| 210 | 246 | if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`); |
| 211 | 247 | driver = kind; |
| 212 | 248 | current = new AbortController(); |
| @@ -216,7 +252,99 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 216 | 252 | driver = null; |
| 217 | 253 | current = null; |
| 218 | 254 | }; |
| 219 | const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording" })[d]; | |
| 255 | const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording", park: "the park move" })[d]; | |
| 256 | ||
| 257 | // ------------------------------------------------------------------ the safety envelope (src/safety.ts) | |
| 258 | /** Where each joint may be sent, ticks: the calibrated range, less the margin, narrowed by the admin's limits. */ | |
| 259 | const bounds = () => jointBounds(limits, settings); | |
| 260 | const speedLimits = () => motionLimits(settings); | |
| 261 | /** The gripper's force limit, for every supervised move. */ | |
| 262 | const torqueCaps = () => new Map([[6, settings.gripperTorquePercent]]); | |
| 263 | /** The servo registers that enforce the same speed and acceleration in hardware, whatever the server sends. */ | |
| 264 | const servoLimits = async (targetIds: readonly number[]) => { | |
| 265 | const { speed, accel } = speedLimits(); | |
| 266 | await bus.syncWrite(STS3215.Goal_Velocity, targetIds.map((id) => [id, speed] as const)); | |
| 267 | await bus.syncWrite(STS3215.Acceleration, targetIds.map((id) => [id, accelRegister(accel)] as const)); | |
| 268 | }; | |
| 269 | let estop: EStop | null = null; | |
| 270 | try { | |
| 271 | const stored = JSON.parse(readFileSync(estopFile, "utf8")) as Partial<EStop>; | |
| 272 | if (typeof stored.by === "string" && typeof stored.at === "string") estop = { by: stored.by, at: stored.at }; | |
| 273 | else estop = { by: "an unreadable record", at: new Date().toISOString() }; // a stop on disk that cannot be read still stops | |
| 274 | } catch (err) { | |
| 275 | if ((err as NodeJS.ErrnoException).code !== "ENOENT") estop = { by: "an unreadable record", at: new Date().toISOString() }; | |
| 276 | } | |
| 277 | const stopped = (e: EStop) => `emergency stop (${e.by}, ${e.at.slice(0, 19).replace("T", " ")} UTC): the arm holds until an admin clears it`; | |
| 278 | /** Torque on, goal where each joint is: the arm stays put and resists. */ | |
| 279 | const holdHere = async () => { | |
| 280 | const positions = await bus.syncRead(ids, STS3215.Present_Position); | |
| 281 | const here = ids.filter((id) => positions.has(id)); | |
| 282 | await bus.syncWrite(STS3215.Goal_Position, here.map((id) => [id, positions.get(id)!] as const)); | |
| 283 | await bus.syncWrite(STS3215.Torque_Enable, here.map((id) => [id, 1] as const)); | |
| 284 | }; | |
| 285 | /** Halt everything and hold with torque; nothing moves the arm until `clearEStop`. Hitting it again only holds again. */ | |
| 286 | const engageEStop = async (by: string): Promise<EStop> => { | |
| 287 | if (!estop) { | |
| 288 | estop = { by, at: new Date().toISOString() }; | |
| 289 | writeAtomic(estopFile, JSON.stringify(estop)); | |
| 290 | metrics.event("estop", { by }); | |
| 291 | } | |
| 292 | const was = driver; | |
| 293 | if (was === "stream") for (const client of wss.clients) client.close(ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks | |
| 294 | else if (was === "record" && recording) { | |
| 295 | const { done } = recording; | |
| 296 | recording = null; // nothing is saved: the recording ends where it was cut | |
| 297 | current?.abort(); | |
| 298 | void done.then(release); | |
| 299 | } else current?.abort(); // a move, a playback or a park stops where it is | |
| 300 | await holdHere(); | |
| 301 | return estop; | |
| 302 | }; | |
| 303 | const clearEStop = (by: string) => { | |
| 304 | if (!estop) return false; | |
| 305 | estop = null; | |
| 306 | try { | |
| 307 | unlinkSync(estopFile); | |
| 308 | } catch { | |
| 309 | /* already gone */ | |
| 310 | } | |
| 311 | metrics.event("estop-clear", { by }); | |
| 312 | return true; | |
| 313 | }; | |
| 314 | ||
| 315 | /** | |
| 316 | * The driver's connection is gone: go slowly to the park pose (the saved pose named in the settings, or the middle | |
| 317 | * of every arm joint's range), under supervision, unless the arm is stopped, busy, or the way there is in the zone. | |
| 318 | */ | |
| 319 | const park = async (why: string) => { | |
| 320 | if (estop || driver) return; | |
| 321 | const controller = claim("park"); | |
| 322 | try { | |
| 323 | const pose = loadPoses()[settings.parkPose]; | |
| 324 | const b = bounds(); | |
| 325 | const targets: Array<[number, number]> = pose | |
| 326 | ? pose.ids.flatMap((id, i) => (b.has(id) ? [[id, bound(b, id, pose.positions[i]!)!] as [number, number]] : [])) | |
| 327 | : ids.filter((id) => id !== 6 && b.has(id)).map((id) => [id, bound(b, id, (limits.get(id)!.min + limits.get(id)!.max) / 2)!]); | |
| 328 | if (!targets.length) return; | |
| 329 | const from = await bus.syncRead(ids, STS3215.Present_Position); | |
| 330 | const violation = checkPath(from, new Map([...from, ...targets])); | |
| 331 | if (violation && settings.zone === "enforce") { | |
| 332 | metrics.event("park", { why, parked: false, error: `refused: ${describeViolation(violation)}` }); | |
| 333 | return; | |
| 334 | } | |
| 335 | await moveToPose(bus, targets.map(([id]) => id), targets.map(([, t]) => t), { | |
| 336 | speed: Math.round(settings.parkSpeedDegS * TICKS_PER_DEGREE), | |
| 337 | torquePercent: 50, | |
| 338 | torqueCaps: torqueCaps(), | |
| 339 | signal: controller.signal, | |
| 340 | }); | |
| 341 | metrics.event("park", { why, parked: true, pose: pose ? settings.parkPose : "middle" }); | |
| 342 | } catch (err) { | |
| 343 | metrics.event("park", { why, parked: false, error: err instanceof Error ? err.message : String(err) }); | |
| 344 | } finally { | |
| 345 | release(); | |
| 346 | } | |
| 347 | }; | |
| 220 | 348 | /** What the stream is up against right now, for /status. */ |
| 221 | 349 | let blocked: Violation | null = null; |
| 222 | 350 | const jams = new Map<number, { position: number; direction: number }>(); |
| @@ -343,7 +471,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 343 | 471 | return [...new Set(value as number[])]; |
| 344 | 472 | }; |
| 345 | 473 | function validateSettings(s: Partial<Settings>): Settings { |
| 346 | const out: Settings = { ...DEFAULT_SETTINGS, jointOffsetsDeg: {} }; | |
| 474 | let safety: SafetySettings; | |
| 475 | try { | |
| 476 | safety = validateSafety(Object.fromEntries(SAFETY_KEYS.map((key) => [key, s[key as keyof SafetySettings]])), safetyBase); | |
| 477 | } catch (err) { | |
| 478 | if (err instanceof SafetyError) throw new HttpError(400, err.message); | |
| 479 | throw err; | |
| 480 | } | |
| 481 | const out: Settings = { ...DEFAULT_SETTINGS, ...safety, jointOffsetsDeg: {} }; | |
| 347 | 482 | if (s.zone !== undefined) { |
| 348 | 483 | if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"'); |
| 349 | 484 | out.zone = s.zone; |
| @@ -392,12 +527,15 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 392 | 527 | blocked, |
| 393 | 528 | jammed: [...jams.keys()], |
| 394 | 529 | stream: streamStats, |
| 530 | estop, | |
| 395 | 531 | }; |
| 396 | 532 | }; |
| 397 | 533 | |
| 398 | 534 | /** A supervised move to explicit targets; the one way joints move over HTTP. */ |
| 399 | const move = async (targetIds: number[], positions: number[], body: Record<string, unknown>, source: string) => { | |
| 535 | const move = async (targetIds: number[], requested: number[], body: Record<string, unknown>, source: string) => { | |
| 400 | 536 | const controller = claim("move"); |
| 537 | const b = bounds(); | |
| 538 | const positions = targetIds.map((id, i) => bound(b, id, requested[i]!) ?? requested[i]!); | |
| 401 | 539 | try { |
| 402 | 540 | const from = await bus.syncRead(ids, STS3215.Present_Position); |
| 403 | 541 | const to = new Map(from); |
| @@ -408,13 +546,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 408 | 546 | if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(violation)}`, violation } }; |
| 409 | 547 | } |
| 410 | 548 | const result = await moveToPose(bus, targetIds, positions, { |
| 411 | speed: number(body.speed, 300, 20, 1500, "speed"), | |
| 549 | speed: number(body.speed, 300, 20, speedLimits().speed, "speed"), | |
| 412 | 550 | torquePercent: number(body.torque, 70, 10, 100, "torque"), |
| 551 | torqueCaps: torqueCaps(), | |
| 413 | 552 | signal: controller.signal, |
| 414 | 553 | }); |
| 415 | 554 | if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, targetIds.map((id) => [id, 0] as const)); |
| 416 | 555 | metrics.counters.move++; |
| 417 | return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(result.adjusted), ...(violation ? { warning: describeViolation(violation) } : {}) } }; | |
| 556 | const adjusted = new Map(result.adjusted); | |
| 557 | targetIds.forEach((id, i) => positions[i] !== requested[i] && adjusted.set(id, positions[i]!)); | |
| 558 | return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(adjusted), ...(violation ? { warning: describeViolation(violation) } : {}) } }; | |
| 418 | 559 | } catch (err) { |
| 419 | 560 | if (err instanceof MotionError) { |
| 420 | 561 | metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "move", { source, error: err.message, stalled: err.stalled, missing: err.missing }); |
| @@ -536,7 +677,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 536 | 677 | return json(res, 200, { ...settings, model: model !== null }); |
| 537 | 678 | case "PUT /settings": { |
| 538 | 679 | const body = await readBody(req); |
| 539 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: { ...settings.jointOffsetsDeg, ...((body.jointOffsetsDeg as Record<string, number> | undefined) ?? {}) } }); | |
| 680 | const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => { | |
| 681 | const patch = body[key]; | |
| 682 | return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]); | |
| 683 | }; | |
| 684 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg") } as Partial<Settings>); | |
| 540 | 685 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 541 | 686 | return json(res, 200, { ...settings, model: model !== null }); |
| 542 | 687 | } |
| @@ -605,9 +750,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 605 | 750 | const lim = limits.get(id); |
| 606 | 751 | if (!lim) throw new HttpError(400, "this arm has no gripper (servo 6)"); |
| 607 | 752 | if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer"); |
| 608 | const position = clamp(body.position as number, lim.min + margin, lim.max - margin); | |
| 609 | const torque = number(body.torque, 30, 10, 60, "torque"); | |
| 610 | const speed = Math.round(number(body.speed, 300, 20, 300, "speed")); | |
| 753 | const position = bound(bounds(), id, body.position as number)!; | |
| 754 | const torque = Math.min(number(body.torque, 30, 10, 60, "torque"), settings.gripperTorquePercent); // the force limit, whatever is asked | |
| 755 | const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), speedLimits().speed); // and the speed limit | |
| 611 | 756 | claim("move"); |
| 612 | 757 | try { |
| 613 | 758 | const present = await bus.syncRead([id], STS3215.Present_Position); |
| @@ -625,6 +770,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 625 | 770 | case "POST /torque": { |
| 626 | 771 | const { on } = await readBody(req); |
| 627 | 772 | if (typeof on !== "boolean") throw new HttpError(400, "on must be true or false"); |
| 773 | if (estop) throw new HttpError(423, stopped(estop)); | |
| 628 | 774 | if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`); |
| 629 | 775 | if (on) { |
| 630 | 776 | const positions = await bus.syncRead(ids, STS3215.Present_Position); |
| @@ -642,6 +788,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 642 | 788 | } else current?.abort(); // the supervisor stops the move where it is |
| 643 | 789 | return json(res, 200, { stopped: was }); |
| 644 | 790 | } |
| 791 | case "POST /estop": { | |
| 792 | const by = actor(await readBody(req)); | |
| 793 | const was = driver; | |
| 794 | return json(res, 200, { estop: await engageEStop(by), stopped: was }); | |
| 795 | } | |
| 796 | case "POST /estop/clear": { | |
| 797 | return json(res, 200, { cleared: clearEStop(actor(await readBody(req))) }); | |
| 798 | } | |
| 645 | 799 | case "POST /record/start": { |
| 646 | 800 | const body = await readBody(req); |
| 647 | 801 | const name = poseNamed(body.name); |
| @@ -672,7 +826,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 672 | 826 | case "POST /play": { |
| 673 | 827 | const body = await readBody(req); |
| 674 | 828 | const name = poseNamed(body.name); |
| 675 | const trajectory = loadRecording(name); | |
| 829 | const recorded = loadRecording(name); | |
| 830 | // Every frame inside the joints' bounds, and the whole playback no faster than the speed limit allows. | |
| 831 | const b = bounds(); | |
| 832 | const trajectory: Trajectory = { ...recorded, frames: recorded.frames.map((f) => f.map((p, i) => bound(b, recorded.ids[i]!, p) ?? p)) }; | |
| 833 | const step = largestStep(trajectory); | |
| 834 | const asked = number(body.rate, 1, 0.1, 2, "rate"); | |
| 835 | const rate = step > 0 ? Math.min(asked, speedLimits().speed / (step * trajectory.hz)) : asked; | |
| 676 | 836 | const controller = claim("play"); |
| 677 | 837 | try { |
| 678 | 838 | if (model && settings.zone !== "off") { |
| @@ -702,13 +862,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 702 | 862 | } |
| 703 | 863 | } |
| 704 | 864 | const played = await playTrajectory(bus, trajectory, { |
| 705 | rate: number(body.rate, 1, 0.1, 2, "rate"), | |
| 865 | rate, | |
| 706 | 866 | torquePercent: number(body.torque, 70, 10, 100, "torque"), |
| 867 | torqueCaps: torqueCaps(), | |
| 707 | 868 | signal: controller.signal, |
| 708 | 869 | }); |
| 709 | 870 | if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, trajectory.ids.map((id) => [id, 0] as const)); |
| 710 | metrics.event("play", { name, played, frames: trajectory.frames.length }); | |
| 711 | return json(res, 200, { played, frames: trajectory.frames.length }); | |
| 871 | metrics.event("play", { name, played, frames: trajectory.frames.length, rate }); | |
| 872 | return json(res, 200, { played, frames: trajectory.frames.length, rate: Number(rate.toFixed(3)) }); | |
| 712 | 873 | } catch (err) { |
| 713 | 874 | if (err instanceof MotionError) { |
| 714 | 875 | metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "play", { source: `play ${name}`, error: err.message, stalled: err.stalled, missing: err.missing }); |
| @@ -728,16 +889,26 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 728 | 889 | }); |
| 729 | 890 | |
| 730 | 891 | // ------------------------------------------------------------------ the stream |
| 731 | const wss = new WebSocketServer({ server: http, path: "/follow" }); | |
| 892 | // Frames are merged down to the command rate; each joint's target is pulled inside its bounds, judged by the zone, | |
| 893 | // and handed to the governor, which moves the goals toward it within the speed and acceleration limits. | |
| 894 | // Silence makes the arm hold (the dead-man); a connection that is lost rather than closed makes it park. | |
| 895 | const wss = new WebSocketServer({ server: http, path: "/follow", maxPayload: 4096 }); | |
| 732 | 896 | wss.on("connection", (ws: WebSocket) => { |
| 897 | if (estop) { | |
| 898 | ws.close(ESTOP_CLOSE, stopped(estop)); | |
| 899 | return; | |
| 900 | } | |
| 733 | 901 | if (driver) { |
| 734 | 902 | ws.close(1013, `${describe(driver)} is driving the arm`); |
| 735 | 903 | return; |
| 736 | 904 | } |
| 737 | 905 | claim("stream"); |
| 738 | 906 | const startedAt = Date.now(); |
| 739 | const stats = { frames: 0, dropped: 0, refused: 0 }; | |
| 907 | const stats = { frames: 0, dropped: 0, refused: 0, merged: 0, clamped: 0 }; | |
| 740 | 908 | streamStats = stats; |
| 909 | const holdAfterMs = options.watchdogMs ?? settings.holdAfterMs; | |
| 910 | const period = 1000 / settings.rateHz; | |
| 911 | const governor = new Governor(bounds, speedLimits); | |
| 741 | 912 | let last: Map<number, number> | null = null; |
| 742 | 913 | /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */ |
| 743 | 914 | const drops = new Map<number, number>(); |
| @@ -746,6 +917,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 746 | 917 | /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */ |
| 747 | 918 | let present = new Map<number, number>(); |
| 748 | 919 | let armed = false; |
| 920 | let arming = false; | |
| 749 | 921 | let timer: ReturnType<typeof setTimeout> | undefined; |
| 750 | 922 | let chain: Promise<void> = Promise.resolve(); |
| 751 | 923 | const run = (fn: () => Promise<void>) => (chain = chain.then(fn).catch((err: unknown) => console.error(`stream: ${err instanceof Error ? err.message : String(err)}`))); |
| @@ -754,6 +926,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 754 | 926 | run(async () => { |
| 755 | 927 | const positions = await bus.syncRead(ids, STS3215.Present_Position); |
| 756 | 928 | await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const)); |
| 929 | governor.reset(positions); | |
| 757 | 930 | last = null; |
| 758 | 931 | }); |
| 759 | 932 | const arm = () => |
| @@ -762,10 +935,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 762 | 935 | present = new Map(positions); |
| 763 | 936 | const ready = ids.filter((id) => positions.has(id)); |
| 764 | 937 | await bus.syncWrite(STS3215.Goal_Position, ready.map((id) => [id, positions.get(id)!] as const)); |
| 765 | await bus.syncWrite(STS3215.Torque_Limit, ready.map((id) => [id, streamTorque * 10] as const)); | |
| 766 | await bus.syncWrite(STS3215.Acceleration, ready.map((id) => [id, 254] as const)); | |
| 767 | await bus.syncWrite(STS3215.Goal_Velocity, ready.map((id) => [id, 0] as const)); | |
| 938 | await bus.syncWrite(STS3215.Torque_Limit, ready.map((id) => [id, Math.round((id === 6 ? Math.min(streamTorque, settings.gripperTorquePercent) : streamTorque) * 10)] as const)); | |
| 939 | await servoLimits(ready); | |
| 768 | 940 | await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const)); |
| 941 | governor.reset(positions); | |
| 769 | 942 | last = null; // the first frame eases the arm over, however far away the leader is |
| 770 | 943 | heldDepth = evaluate(positions)?.violation?.depth ?? 0; |
| 771 | 944 | armed = true; |
| @@ -785,6 +958,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 785 | 958 | const travel = Math.max(...w.map((s) => s.p)) - Math.min(...w.map((s) => s.p)); |
| 786 | 959 | if (!(pushing && travel < STALL_SPAN_TICKS && Math.abs(target - p) > 30)) return false; |
| 787 | 960 | await bus.syncWrite(STS3215.Goal_Position, [[id, p]]); |
| 961 | governor.pin(id, p); | |
| 788 | 962 | jams.set(id, { position: p, direction: Math.sign(target - p) }); |
| 789 | 963 | last?.set(id, p); |
| 790 | 964 | metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10 }); |
| @@ -804,25 +978,43 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 804 | 978 | jamSampleMs, |
| 805 | 979 | ); |
| 806 | 980 | |
| 807 | ws.on("message", (data) => { | |
| 808 | let frame: Frame; | |
| 809 | try { | |
| 810 | frame = JSON.parse(String(data)); | |
| 811 | } catch { | |
| 812 | return; | |
| 813 | } | |
| 814 | clearTimeout(timer); | |
| 815 | timer = setTimeout(() => void hold(), watchdogMs); | |
| 816 | if (!armed) void arm(); | |
| 817 | if (easing) return; // frames that arrive while catching up are superseded by the next one | |
| 981 | // The governor's clock: at the command rate, the goals move one step toward the targets. | |
| 982 | let ticking = false; | |
| 983 | let lastTick = performance.now(); | |
| 984 | const ticker = setInterval(() => { | |
| 985 | const now = performance.now(); | |
| 986 | const dt = Math.min(now - lastTick, 2 * period) / 1000; // a late tick never turns into a jump | |
| 987 | lastTick = now; | |
| 988 | if (!armed || easing || ticking || !governor.moving) return; | |
| 989 | ticking = true; | |
| 990 | void run(async () => { | |
| 991 | try { | |
| 992 | const step = governor.step(dt); | |
| 993 | if (step.length) await bus.syncWrite(STS3215.Goal_Position, step); | |
| 994 | } finally { | |
| 995 | ticking = false; | |
| 996 | } | |
| 997 | }); | |
| 998 | }, period); | |
| 999 | ||
| 1000 | // The heartbeat: a client that does not answer a ping before the next one is gone, however quiet its socket. | |
| 1001 | let answered = true; | |
| 1002 | ws.on("pong", () => (answered = true)); | |
| 1003 | const heartbeat = setInterval(() => { | |
| 1004 | if (!answered) return ws.terminate(); | |
| 1005 | answered = false; | |
| 1006 | ws.ping(); | |
| 1007 | }, heartbeatMs); | |
| 1008 | ||
| 1009 | const follow = (frame: ReadonlyMap<number, number>) => | |
| 818 | 1010 | run(async () => { |
| 1011 | const b = bounds(); | |
| 819 | 1012 | const targets: Array<readonly [number, number]> = []; |
| 820 | 1013 | let ease = last === null; // after a hold, everything eases |
| 821 | for (const [key, value] of Object.entries(frame)) { | |
| 822 | const id = Number(key); | |
| 823 | const lim = limits.get(id); | |
| 824 | if (!lim || !Number.isFinite(value)) continue; | |
| 825 | const target = clamp(Math.round(value), lim.min + margin, lim.max - margin); | |
| 1014 | for (const [id, value] of frame) { | |
| 1015 | const target = bound(b, id, value); | |
| 1016 | if (target === null) continue; | |
| 1017 | if (target !== Math.round(value)) stats.clamped++; | |
| 826 | 1018 | const jam = jams.get(id); |
| 827 | 1019 | if (jam) { |
| 828 | 1020 | if ((target - jam.position) * jam.direction > -10) continue; // still pushing into the obstacle: wait for the leader to back off |
| @@ -860,11 +1052,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 860 | 1052 | heldDepth = zone?.violation?.depth ?? 0; |
| 861 | 1053 | approach(zone); |
| 862 | 1054 | if (ease) { |
| 863 | // Catch up slowly with every joint in this frame, then hand over to the full-rate stream. | |
| 1055 | // Catch up slowly with every joint in this frame, the servos' own speed held low, then hand over to the governor. | |
| 864 | 1056 | // A joint that jams on the way is parked and no longer waited for. |
| 865 | 1057 | easing = true; |
| 866 | 1058 | try { |
| 867 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, easeSpeed] as const)); | |
| 1059 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, speedLimits().speed)] as const)); | |
| 868 | 1060 | await bus.syncWrite(STS3215.Goal_Position, targets); |
| 869 | 1061 | const deadline = Date.now() + 4000; |
| 870 | 1062 | const wanted = new Map(targets); |
| @@ -881,35 +1073,71 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 881 | 1073 | if (worst <= 25) break; |
| 882 | 1074 | await new Promise((r) => setTimeout(r, 50)); |
| 883 | 1075 | } |
| 884 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, 0] as const)); | |
| 1076 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, speedLimits().speed] as const)); | |
| 885 | 1077 | } finally { |
| 886 | 1078 | easing = false; |
| 887 | 1079 | } |
| 888 | 1080 | last = new Map([...(last ?? []), ...targets]); |
| 1081 | for (const [id, t] of targets) governor.pin(id, jams.get(id)?.position ?? t); | |
| 889 | 1082 | for (const [id, jam] of jams) last.set(id, jam.position); |
| 890 | 1083 | return; |
| 891 | 1084 | } |
| 892 | await bus.syncWrite(STS3215.Goal_Position, targets); | |
| 893 | for (const [id, t] of targets) last!.set(id, t); | |
| 1085 | for (const [id, t] of targets) { | |
| 1086 | governor.aim(id, t); | |
| 1087 | last!.set(id, t); | |
| 1088 | } | |
| 894 | 1089 | }); |
| 895 | }); | |
| 896 | ws.on("close", () => { | |
| 1090 | ||
| 1091 | // Frames faster than the command rate are merged, the latest value per joint winning, and taken at the next slot. | |
| 1092 | let pending: Map<number, number> | null = null; | |
| 1093 | let pacer: ReturnType<typeof setTimeout> | undefined; | |
| 1094 | let lastTaken = 0; | |
| 1095 | const take = () => { | |
| 1096 | pacer = undefined; | |
| 1097 | if (!pending) return; | |
| 1098 | // While the arm catches up, the frame waits (later ones merge into it) and is taken once it has: never dropped, | |
| 1099 | // or a leader that sends one last frame and stops would leave the arm short of it. | |
| 1100 | if (easing) return void (pacer = setTimeout(take, period)); | |
| 1101 | const frame = pending; | |
| 1102 | pending = null; | |
| 1103 | lastTaken = Date.now(); | |
| 1104 | follow(frame); | |
| 1105 | }; | |
| 1106 | ws.on("message", (data) => { | |
| 1107 | const frame = parseFrame(String(data)); | |
| 1108 | if (!frame) return; | |
| 897 | 1109 | clearTimeout(timer); |
| 1110 | timer = setTimeout(() => void hold(), holdAfterMs); | |
| 1111 | if (!arming) { | |
| 1112 | arming = true; | |
| 1113 | void arm(); | |
| 1114 | } | |
| 1115 | if (pending) stats.merged++; | |
| 1116 | pending = new Map([...(pending ?? []), ...frame]); | |
| 1117 | pacer ??= setTimeout(take, Math.max(0, lastTaken + period - Date.now())); | |
| 1118 | }); | |
| 1119 | ws.on("close", (code) => { | |
| 1120 | clearTimeout(timer); | |
| 1121 | clearTimeout(pacer); | |
| 898 | 1122 | clearInterval(watch); |
| 1123 | clearInterval(ticker); | |
| 1124 | clearInterval(heartbeat); | |
| 1125 | const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE; | |
| 899 | 1126 | void run(async () => { |
| 900 | 1127 | try { |
| 901 | 1128 | if (armed) { |
| 902 | 1129 | const positions = await bus.syncRead(ids, STS3215.Present_Position); |
| 903 | 1130 | await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const)); |
| 904 | await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, 1000] as const)); | |
| 1131 | await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, id === 6 ? settings.gripperTorquePercent * 10 : 1000] as const)); | |
| 905 | 1132 | } |
| 906 | 1133 | } finally { |
| 907 | 1134 | blocked = null; |
| 908 | 1135 | jams.clear(); |
| 909 | 1136 | streamStats = null; |
| 910 | metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats }); | |
| 1137 | metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats, ...(lost ? { lost: code } : {}) }); | |
| 911 | 1138 | release(); // whatever the bus did, the arm is free for the next driver |
| 912 | 1139 | } |
| 1140 | if (lost && armed) void park(`the stream was lost (${code})`); | |
| 913 | 1141 | }); |
| 914 | 1142 | }); |
| 915 | 1143 | }); |
Software/so101/src/viewer.html +73 −9
| @@ -58,6 +58,10 @@ | ||
| 58 | 58 | /* the view: cameras and the 3D model, always on black */ |
| 59 | 59 | #app { grid-area: view; position: relative; min-height: 0; background: #000; color: #e8eaed; overflow: hidden; } |
| 60 | 60 | #stage { position: absolute; inset: 0; display: flex; gap: 2px; } |
| 61 | /* the emergency stop's held state, and the banner that says who pressed it */ | |
| 62 | .estop:disabled { opacity: .5; cursor: not-allowed; } .estop.on { background: #7d1410; box-shadow: 0 0 0 3px var(); } | |
| 63 | #banner.stop { display: block; border: 2px solid var(); background: rgba(80,10,8,.92); color: #fff; font-weight: 600; } | |
| 64 | #banner button { margin-left: 10px; min-height: 32px; padding: 2px 10px; } | |
| 61 | 65 | .pane { flex: 1; min-width: 0; position: relative; background: #000; display: none; } |
| 62 | 66 | .pane.on { display: block; } |
| 63 | 67 | .pane img { width: 100%; height: 100%; object-fit: contain; display: block; } |
| @@ -244,7 +248,8 @@ | ||
| 244 | 248 | <div class="who"><div id="driverLine" class="none">Connecting…</div><div id="clock"></div><div id="capLine"></div></div> |
| 245 | 249 | <div id="turnActions"></div> |
| 246 | 250 | <button id="sheetToggle" hidden aria-controls="sheet" aria-expanded="true">Hide</button> |
| 247 | <button id="stop" class="estop" hidden title="Stop the arm (space)" aria-label="Emergency stop">Stop</button> | |
| 251 | <button id="stop" class="danger" hidden title="End what the arm is doing and hold (space)">Stop</button> | |
| 252 | <button id="estop" class="estop" title="Emergency stop (Esc): halt, hold with torque, until an admin clears it" aria-label="Emergency stop">Stop</button> | |
| 248 | 253 | </section> |
| 249 | 254 | |
| 250 | 255 | <section id="sheet" aria-label="Chat, queue and controls"> |
| @@ -336,6 +341,21 @@ | ||
| 336 | 341 | <p>The model takes the middle of each calibrated range as zero, the LeRobot convention. The real zero can be a few degrees off, which puts the model a centimetre or two away from the arm at the gripper. Compare the 3D view with the camera and adjust a trim until they match. The zone uses the trimmed model.</p> |
| 337 | 342 | <div class="trim" id="trims"></div> |
| 338 | 343 | </div> |
| 344 | <div class="card"> | |
| 345 | <h3>Safety limits</h3> | |
| 346 | <p>The server enforces these on every command, whoever sends it; only an admin can change them. Angles are degrees from the middle of each joint's calibrated range. When the driver's connection is lost, the arm moves slowly to the pose named below (save one with that name), or to the middle of every range.</p> | |
| 347 | <div class="trim" id="safety"> | |
| 348 | <label for="maxSpeedDegS">top speed, °/s</label><input type="number" id="maxSpeedDegS" min="30" max="360" step="5" inputmode="numeric"> | |
| 349 | <label for="maxAccelDegS2">acceleration, °/s²</label><input type="number" id="maxAccelDegS2" min="30" max="3600" step="10" inputmode="numeric"> | |
| 350 | <label for="rateHz">commands per second</label><input type="number" id="rateHz" min="5" max="60" step="1" inputmode="numeric"> | |
| 351 | <label for="holdAfterMs">hold after silence, ms</label><input type="number" id="holdAfterMs" min="100" max="5000" step="50" inputmode="numeric"> | |
| 352 | <label for="parkPose">park pose</label><input type="text" id="parkPose" maxlength="64" pattern="[A-Za-z0-9][A-Za-z0-9_.\-]{0,63}" autocomplete="off"> | |
| 353 | <label for="parkSpeedDegS">park speed, °/s</label><input type="number" id="parkSpeedDegS" min="5" max="60" step="1" inputmode="numeric"> | |
| 354 | <label for="gripperTorquePercent">gripper force, %</label><input type="number" id="gripperTorquePercent" min="10" max="60" step="1" inputmode="numeric"> | |
| 355 | </div> | |
| 356 | <h3>Joint limits, degrees (empty: the calibrated range)</h3> | |
| 357 | <div class="trim" id="jointLimits"></div> | |
| 358 | </div> | |
| 339 | 359 | <div class="row"><button id="metricsBtn">Metrics</button><button id="calib">3D calibration</button></div> |
| 340 | 360 | </div> |
| 341 | 361 | <nav id="tabs" role="tablist" aria-label="Sheet"> |
| @@ -742,6 +762,19 @@ | ||
| 742 | 762 | if (r.saved) loadRecordings(); |
| 743 | 763 | } |
| 744 | 764 | $("stop").onclick = stop; |
| 765 | // The emergency stop: the server halts whatever drives the arm and holds it with torque on until an admin clears it. | |
| 766 | async function eStop() { | |
| 767 | const r = await api("/estop", {}); | |
| 768 | msg(r.ok ? "EMERGENCY STOP: the arm holds; an admin must clear it" : r.error, true); | |
| 769 | if (stream.ws) stream.ws.close(); | |
| 770 | if (leaderStreaming()) stopLeaderDrive(); | |
| 771 | } | |
| 772 | async function clearEStop() { | |
| 773 | if (!confirm("Clear the emergency stop? The arm stays where it is; the driver can move it again.")) return; | |
| 774 | const r = await api("/estop/clear", {}); | |
| 775 | msg(r.ok ? "the emergency stop is cleared" : r.error, !r.ok); | |
| 776 | } | |
| 777 | $("estop").onclick = eStop; | |
| 745 | 778 | $("hold").onclick = async () => { const r = await api("/torque", { on: true }); msg(r.ok ? "holding: torque on" : r.error, !r.ok); }; |
| 746 | 779 | $("releaseBtn").onclick = async () => { |
| 747 | 780 | if (!confirm("Release torque? Hold the arm so it does not fall.")) return; |
| @@ -1163,6 +1196,7 @@ | ||
| 1163 | 1196 | addEventListener("keydown", (e) => { |
| 1164 | 1197 | if (e.target.closest("input, select, textarea, dialog")) return; |
| 1165 | 1198 | if (e.key === "f" || e.key === "F") fullscreen(); |
| 1199 | if (e.key === "Escape" && (mayDrive() || isOwner())) { e.preventDefault(); eStop(); } | |
| 1166 | 1200 | if (e.key === " " && (mayDrive() || isOwner())) { e.preventDefault(); if (e.target instanceof HTMLButtonElement) e.target.blur(); stop(); } |
| 1167 | 1201 | }); |
| 1168 | 1202 | for (const b of document.querySelectorAll("[data-close]")) b.onclick = () => { $(b.dataset.close).hidden = true; }; |
| @@ -1178,6 +1212,23 @@ | ||
| 1178 | 1212 | input.onchange = () => saveSettings({ jointOffsetsDeg: { [name]: Number(input.value) || 0 } }); |
| 1179 | 1213 | trims.append(el("label", { htmlFor: input.id, textContent: name }), input); |
| 1180 | 1214 | } |
| 1215 | for (const key of ["maxSpeedDegS", "maxAccelDegS2", "rateHz", "holdAfterMs", "parkPose", "parkSpeedDegS", "gripperTorquePercent"]) { | |
| 1216 | const input = $(key); | |
| 1217 | input.value = settings[key]; | |
| 1218 | input.onchange = () => { if (input.checkValidity()) saveSettings({ [key]: input.type === "number" ? Number(input.value) : input.value.trim() }); }; | |
| 1219 | } | |
| 1220 | const jl = $("jointLimits"); jl.innerHTML = ""; | |
| 1221 | for (const name of Object.values(NAMES)) { | |
| 1222 | const pair = settings.jointLimitsDeg?.[name] ?? ["", ""]; | |
| 1223 | const inputs = pair.map((v, i) => el("input", { type: "number", min: -180, max: 180, step: 1, value: v, placeholder: i ? "max" : "min", style: "width:70px" })); | |
| 1224 | const save = () => { | |
| 1225 | const [lo, hi] = inputs.map((x) => x.value.trim()); | |
| 1226 | if (!lo && !hi) return saveSettings({ jointLimitsDeg: { [name]: null } }); | |
| 1227 | if (lo && hi) saveSettings({ jointLimitsDeg: { [name]: [Number(lo), Number(hi)] } }); | |
| 1228 | }; | |
| 1229 | for (const x of inputs) x.onchange = save; | |
| 1230 | jl.append(el("label", { textContent: name }), el("span", { className: "row" }, ...inputs)); | |
| 1231 | } | |
| 1181 | 1232 | $("zoneNote").textContent = settings.model ? "" : "No model on this server: the zone cannot be checked."; |
| 1182 | 1233 | } |
| 1183 | 1234 | async function saveSettings(patch) { |
| @@ -1201,7 +1252,11 @@ | ||
| 1201 | 1252 | ["closest to the table", m.closest ? `${(m.closest.z * 1000).toFixed(0)} mm (${m.closest.link.replace(/_link$/, "")}, ${m.closest.at.slice(11, 19)})` : "—"], |
| 1202 | 1253 | ].map(([k, v]) => `<b>${k}</b><span>${v}</span>`).join(""); |
| 1203 | 1254 | $("peaks").innerHTML = "<tr><th>joint</th><th>peak load</th><th>peak temperature</th></tr>" + Object.entries(m.peaks).map(([id, p]) => `<tr><td>${NAMES[id] ?? id}</td><td>${p.load.toFixed(0)} %</td><td>${p.temperature} °C</td></tr>`).join(""); |
| 1204 | $("events").innerHTML = [...(m.recent ?? [])].reverse().map((e) => `<div>${e.t.slice(5, 19).replace("T", " ")} <b>${e.kind}</b> ${Object.entries(e.detail).map(([k, v]) => `${k}=${typeof v === "object" ? JSON.stringify(v) : v}`).join(" ")}</div>`).join("") || "<div>none yet</div>"; | |
| 1255 | // Built from text nodes: an event's detail carries people's names (who pressed the E-stop) and error strings. | |
| 1256 | const recent = [...(m.recent ?? [])].reverse(); | |
| 1257 | $("events").replaceChildren(...(recent.length ? recent.map((e) => el("div", {}, | |
| 1258 | `${String(e.t).slice(5, 19).replace("T", " ")} `, el("b", { textContent: String(e.kind) }), | |
| 1259 | ` ${Object.entries(e.detail ?? {}).map(([k, v]) => `${k}=${typeof v === "object" ? JSON.stringify(v) : v}`).join(" ")}`)) : [el("div", { textContent: "none yet" })])); | |
| 1205 | 1260 | $("metricsPanel").hidden = false; |
| 1206 | 1261 | } |
| 1207 | 1262 | $("metricsBtn").onclick = showMetrics; |
| @@ -1218,21 +1273,21 @@ | ||
| 1218 | 1273 | const say = (m) => { if (lobby.ws?.readyState === 1) lobby.ws.send(JSON.stringify(m)); }; |
| 1219 | 1274 | let drove = false; |
| 1220 | 1275 | function applyLock() { |
| 1221 | const can = mayDrive(), owner = isOwner(); | |
| 1222 | $("t-controls").hidden = !can; | |
| 1276 | const can = mayDrive() && !status?.estop, owner = isOwner(); | |
| 1277 | $("t-controls").hidden = !mayDrive(); | |
| 1223 | 1278 | $("t-admin").hidden = !owner; |
| 1224 | 1279 | $("t-chat").hidden = $("t-queue").hidden = !relayed; |
| 1225 | 1280 | for (const id of ["joints", "poses", "drive", "reach"]) $(id).inert = !can; |
| 1226 | 1281 | $("stop").hidden = !(can || owner); |
| 1227 | // The arm takes its settings from whoever drives: the owner changes them on their own turn. | |
| 1228 | for (const id of ["zoneRow", "trims"]) $(id).inert = !can; | |
| 1229 | if (!can && owner && settings?.model !== false) $("zoneNote").textContent = "Take control to change the zone and the trims: the arm takes them from its driver."; | |
| 1230 | else if (settings?.model !== false) $("zoneNote").textContent = ""; | |
| 1282 | $("estop").disabled = !(mayDrive() || owner); | |
| 1283 | $("estop").title = $("estop").disabled ? "Emergency stop: the driver and the admins can press it" : "Emergency stop (Esc): halt, hold with torque, until an admin clears it"; | |
| 1284 | // The server takes settings, the safety limits among them, from admins only. | |
| 1285 | for (const id of ["zoneRow", "trims", "safety", "jointLimits"]) $(id).inert = !owner; | |
| 1231 | 1286 | viewer.toggleAttribute("disable-dragging", !can); |
| 1232 | 1287 | if (!can) reachEnd(); |
| 1233 | 1288 | if (!can && leaderStreaming()) stopLeaderDrive(); // the server closes it too; the page need not wait for that |
| 1234 | 1289 | // Your turn just came: open the controls. It just ended: back to the chat. |
| 1235 | if (can !== drove) { drove = can; if (relayed) showTab(can ? "controls" : "chat"); } | |
| 1290 | if (mayDrive() !== drove) { drove = mayDrive(); if (relayed) showTab(drove ? "controls" : "chat"); } | |
| 1236 | 1291 | else if ($(`t-${tab}`).hidden) showTab(tab); |
| 1237 | 1292 | } |
| 1238 | 1293 | const serverNow = () => Date.now() + lobby.offset; |
| @@ -1512,6 +1567,12 @@ | ||
| 1512 | 1567 | if (beyond > 0) return `${part} is ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`; |
| 1513 | 1568 | return `${part} is ${(-beyond).toFixed(0)} mm ${v.what === "table" ? "above the table" : "from the base"}, inside the ${marginMm} mm margin`; |
| 1514 | 1569 | }; |
| 1570 | if (d.estop) { | |
| 1571 | b.className = "stop"; | |
| 1572 | b.replaceChildren(`EMERGENCY STOP by ${d.estop.by} at ${new Date(d.estop.at).toLocaleTimeString()}. The arm holds with torque on until an admin clears it.`); | |
| 1573 | if (isOwner()) b.append(el("button", { textContent: "Clear the stop", className: "danger", onclick: clearEStop })); | |
| 1574 | return; | |
| 1575 | } | |
| 1515 | 1576 | if (d.jammed?.length) { b.className = "bad"; b.textContent = `jam: ${d.jammed.map((id) => NAMES[id] ?? id).join(", ")} pushed against something and stopped — back the leader off`; } |
| 1516 | 1577 | else if (d.blocked) { b.className = d.zone === "enforce" ? "bad" : "warn"; b.textContent = `${d.zone === "enforce" ? "held at the boundary" : "warning"}: ${say(d.blocked)}`; } |
| 1517 | 1578 | else if (d.violation) { b.className = "warn"; b.textContent = `the arm is ${say(d.violation)} (zone: ${d.zone})`; } |
| @@ -1522,10 +1583,13 @@ | ||
| 1522 | 1583 | try { |
| 1523 | 1584 | const d = await api("/status"); |
| 1524 | 1585 | if (!d.ok) throw new Error(); |
| 1586 | const wasStopped = Boolean(status?.estop); | |
| 1525 | 1587 | rtts.push(performance.now() - started); if (rtts.length > 10) rtts.shift(); // the link's round trip, for the camera plan |
| 1526 | 1588 | status = d; |
| 1589 | if (Boolean(d.estop) !== wasStopped) applyLock(); | |
| 1527 | 1590 | $("dot").className = "dot on"; |
| 1528 | 1591 | $("state").textContent = d.driving === null ? "idle" : d.driving === "record" ? `recording ${d.recording}` : leaderStreaming() ? `leader · ${leader.frames} frames` : d.driving; |
| 1592 | $("estop").classList.toggle("on", Boolean(d.estop)); $("estop").textContent = d.estop ? "Stopped" : "Stop"; | |
| 1529 | 1593 | $("record").textContent = d.driving === "record" ? "■ Stop recording" : "● Record"; |
| 1530 | 1594 | for (const id of ["hold", "releaseBtn", "play"]) $(id).disabled = d.driving !== null; |
| 1531 | 1595 | if (tab === "controls") render(d); |
new above
offline