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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

Letting go means go there: a release lands on the target instead of parking the arm short and snapping the model back #460

Parked opened this in answer to
Closed without merging

vesper/arm:vesper/437-letting-go-means-go-there-a-release-land → main 4 commits fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
14 files changed +785 −109
Software/README.md +17
@@ -180,6 +180,23 @@ simulated bus for working on the page. The page expects MJPEG streams on
180180 ports 8080 and 8081 of the same host (`?stream=<url>` overrides the first),
181181 or, behind the hosted app's relay, at `/camera/1/stream` and `/camera/2/stream`.
182182
183 **Letting go means go there.** When you let go of a slider, a jog button or
184 a model joint, the page sends a release, `{"type": "release", "seq", "t",
185 "joints"}`, carrying the last target, at once (never merged into a later
186 frame). The arm server then lets the speed and acceleration limits carry the
187 arm all the way there, and only stops driving once every joint is within 12
188 ticks of its target, or when the governor's own estimate of the remaining
189 travel plus 500 ms has run out. Only an abnormal end stops the arm where it
190 is: a lost link, STOP or the emergency stop, a turn that ends, or a stream
191 closed for breaking the limits (the relay tells the arm server so with close
192 code 4409). A new press while the arm is still landing takes over at once.
193 After you let go, the page keeps showing your target for the predicted travel
194 time plus 300 ms, longer while the arm is still closing in. If the arm stops
195 short, the page says why, for example `held: gripper is 4 mm below the table`.
196 While you hold a press still, the page repeats the target every 250 ms, so
197 the dead-man (`holdAfterMs`) never parks a held target. It still fires when
198 the page itself goes quiet.
199
183200 On the forge (`forge.json` at the repository root), the app is that same
184201 simulated arm: no servo board is attached, so every move arrives at once. It
185202 reads `PORT` and `HOST`, and keeps poses, recordings, settings and metrics in
Software/so101/src/command-gate.test.ts +21
@@ -18,6 +18,27 @@ describe("parseCommand", () => {
1818 expect(parseCommand("nope")).toBeNull();
1919 });
2020 });
21 describe("release", () => {
22 const release = (seq: number, t: number, joints: Record<string, unknown>) => JSON.stringify({ type: "release", seq, t, joints });
23 test("parsed with its joints and marked; anything not a number in the joints is left out", () => {
24 expect(parseCommand(release(4, 1000, { 1: 2050, 2: "x" }))).toEqual({ seq: 4, t: 1000, epoch: null, joints: new Map([[1, 2050]]), release: true });
25 expect(parseCommand(JSON.stringify({ type: "release", joints: { 1: 2050 } }))).toEqual({ seq: null, t: null, epoch: null, joints: new Map([[1, 2050]]), release: true });
26 expect(parseCommand(JSON.stringify({ type: "release", joints: [1] }))).toBeNull();
27 expect(parseCommand(JSON.stringify({ type: "other", 1: 2000 }))?.release).toBeUndefined();
28 });
29 test("passes the same gate as a frame: out of order or held, it is dropped like one; late, it is not", () => {
30 const gate = new CommandGate();
31 const s = gate.open(new Clock(), true);
32 expect(s.admit(release(1, 1000, { 1: 2050 }), 1000)).toEqual({ ok: true, joints: new Map([[1, 2050]]), release: true });
33 expect(s.admit(release(1, 1000, { 1: 2050 }), 1001)).toEqual({ ok: false, drop: "replayed" });
34 expect(s.admit(frame(2, 1000), 1000 + MAX_AGE_MS + 1)).toEqual({ ok: false, drop: "stale" });
35 // Late as that frame, a release still counts: it is the last target, and nothing newer from that hand follows it.
36 expect(s.admit(release(3, 1000, { 1: 2050 }), 1000 + MAX_AGE_MS + 2)).toEqual({ ok: true, joints: new Map([[1, 2050]]), release: true });
37 gate.hold("someone");
38 expect(s.admit(release(4, 1300, { 1: 2050 }), 1300)).toEqual({ ok: false, drop: "held" });
39 });
40 });
41
2142
2243 describe("CommandGate", () => {
2344 test("passes fresh frames in order, and drops one out of order or sent twice", () => {
Software/so101/src/command-gate.ts +37 −10
@@ -6,7 +6,8 @@
66 *
77 * held the arm is stopped: STOP holds it until someone resumes, and nothing sent meanwhile counts
88 * replayed its seq is not above the last one this stream sent (a duplicate, or out of order)
9 * stale it is older than MAX_AGE_MS: it sat in a buffer somewhere and is no longer what the hand meant
9 * stale it is older than MAX_AGE_MS: it sat in a buffer somewhere and is no longer what the hand meant.
10 * A release is never stale: it is the gesture's last target, and nothing newer from that hand follows it
1011 * before-resume it was sent before the latest resume, however it got here
1112 * before-stop its epoch is below the epoch of the last STOP this gate was told of (see below)
1213 * unstamped it has no seq and t where they are required (behind the relay; the CLI leader on a LAN sends none)
@@ -16,12 +17,15 @@
1617 * frame comes late by however long it waited, whatever the clocks say. Every REANCHOR_MS the smallest gap is taken
1718 * again from that window alone, so a page whose clock is stepped does not have every frame called stale for good.
1819 * A stream still open when the arm resumes sends nothing more that counts: the page opens a new one.
20 *
21 * A finger that lets go sends `{"type": "release", "seq": n, "t": ms, "joints": {"1": ticks, ...}}`: the last target,
22 * meaning "go there and stop". It passes the same gate as a frame, and comes out with `release: true`.
1923 * Pure: text and times in, verdicts out.
2024 */
2125
2226 import { mkdirSync, readFileSync, renameSync, writeFileSync } from "node:fs";
2327 import { dirname } from "node:path";
24 import { parseFrame } from "./safety.ts";
28 import { jointsOf } from "./safety.ts";
2529
2630 export const MAX_AGE_MS = 200;
2731 const REANCHOR_MS = 30_000;
@@ -34,7 +38,7 @@ export interface Held {
3438 }
3539
3640 export type Drop = "malformed" | "unstamped" | "replayed" | "held" | "stale" | "before-resume" | "before-stop";
37 export type Verdict = { ok: true; joints: Map<number, number> } | { ok: false; drop: Drop };
41 export type Verdict = { ok: true; joints: Map<number, number>; release?: true } | { ok: false; drop: Drop };
3842
3943 export interface Command {
4044 seq: number | null;
@@ -42,15 +46,36 @@ export interface Command {
4246 /** The relay's STOP epoch when it sent the frame; null from a sender that has none (the CLI leader on a LAN). */
4347 epoch: number | null;
4448 joints: Map<number, number>;
49 /** Set on a release: the last target of a gesture, to be reached, never merged away. */
50 release?: true;
4551 }
4652
47 /** The frame's joints, and its seq and t when both are well formed; null for anything that is not a frame. */
53 /** The frame's joints, and its seq and t when both are well formed; null for anything that is not a frame or a release. */
4854 export function parseCommand(text: string): Command | null {
49 const joints = parseFrame(text);
55 let parsed: unknown;
56 try {
57 parsed = JSON.parse(text);
58 } catch {
59 return null;
60 }
61 if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) return null;
62 const { seq, t, epoch, type, joints: inner } = parsed as { seq?: unknown; t?: unknown; epoch?: unknown; type?: unknown; joints?: unknown };
63 const release = type === "release";
64 const joints = jointsOf(release ? inner : parsed);
5065 if (!joints) return null;
51 const { seq, t, epoch } = JSON.parse(text) as { seq?: unknown; t?: unknown; epoch?: unknown };
5266 const stamped = Number.isSafeInteger(seq) && (seq as number) >= 0 && typeof t === "number" && Number.isFinite(t) && t > 0;
53 return { seq: stamped ? (seq as number) : null, t: stamped ? (t as number) : null, epoch: Number.isSafeInteger(epoch) && (epoch as number) >= 0 ? (epoch as number) : null, joints };
67 return {
68 seq: stamped ? (seq as number) : null,
69 t: stamped ? (t as number) : null,
70 epoch: Number.isSafeInteger(epoch) && (epoch as number) >= 0 ? (epoch as number) : null,
71 joints,
72 ...(release ? { release: true as const } : {}),
73 };
74 }
75
76 /** A release as text, for a sender that has already checked its joints (the relay, toward the arm server, adds its epoch). */
77 export function releaseText(joints: ReadonlyMap<number, number>, extra: Readonly<Record<string, number>> = {}): string {
78 return JSON.stringify({ type: "release", ...extra, joints: Object.fromEntries(joints) });
5479 }
5580
5681 /** One sender's clock, seen from here: when a frame it stamped would have arrived had nothing held it up. */
@@ -172,6 +197,8 @@ export class CommandGate {
172197 }
173198 }
174199
200 const pass = (c: Command): Verdict => (c.release ? { ok: true, joints: c.joints, release: true } : { ok: true, joints: c.joints });
201
175202 export class GateStream {
176203 private lastSeq = -1;
177204 private readonly gate: CommandGate;
@@ -191,15 +218,15 @@ export class GateStream {
191218 if (c.epoch !== null && c.epoch < this.gate.stopEpoch) return { ok: false, drop: "before-stop" };
192219 if (c.seq === null || c.t === null) {
193220 if (this.stamped) return { ok: false, drop: "unstamped" };
194 return this.gate.held ? { ok: false, drop: "held" } : { ok: true, joints: c.joints };
221 return this.gate.held ? { ok: false, drop: "held" } : pass(c);
195222 }
196223 if (c.seq <= this.lastSeq) return { ok: false, drop: "replayed" };
197224 this.lastSeq = c.seq;
198225 const late = this.clock.observe(c.t, now);
199226 if (this.gate.held) return { ok: false, drop: "held" };
200 if (late > MAX_AGE_MS) return { ok: false, drop: "stale" };
227 if (late > MAX_AGE_MS && !c.release) return { ok: false, drop: "stale" };
201228 if (this.clock.here(c.t) < this.gate.since) return { ok: false, drop: "before-resume" };
202 return { ok: true, joints: c.joints };
229 return pass(c);
203230 }
204231
205232 /** @internal Nothing this stream sent up to now may count after a resume. */
Software/so101/src/frame-limit.test.ts +2 −2
@@ -5,7 +5,7 @@ import { tmpdir } from "node:os";
55 import { join } from "node:path";
66 import WebSocket, { WebSocketServer } from "ws";
77 import { FeetechBus } from "./bus.ts";
8 import { stamper } from "./command-gate.ts";
8 import { HELD_CLOSE, stamper } from "./command-gate.ts";
99 import { FakeBus } from "./fake-bus.ts";
1010 import { BURST, FLOOD_CLOSE, FLOOD_MS, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts";
1111 import { LOST_CLOSE, parseRemote, startRelay, type RunningRelay } from "./relay.ts";
@@ -187,7 +187,7 @@ describe("the relay's /follow", () => {
187187 expect(await code).toBe(TOO_BIG_CLOSE);
188188 await sleep(30);
189189 expect(arm.got.map(unstamped)).toEqual(['{"1":2050}']);
190 expect(arm.ended).toEqual([1000]); // not LOST_CLOSE: the arm server holds rather than parks
190 expect(arm.ended).toEqual([HELD_CLOSE]); // not LOST_CLOSE, nor a clean close that would land a release: the arm server holds where it is
191191 expect(arm.ended).not.toContain(LOST_CLOSE);
192192 } finally {
193193 arm.stop();
Software/so101/src/relay.ts +29 −14
@@ -21,12 +21,13 @@
2121 * while held, a stream is refused (HELD_CLOSE) and no frame goes through; /resume opens it once the arm server agrees,
2222 * and only a frame sent after that moves the arm. Every frame from the page carries "seq" and "t": one without them,
2323 * out of order, older than 200 ms, or sent before the resume is dropped here, never kept. What goes on to the arm
24 * server is the joints alone. A turn that ends stops what drives without holding ("hold": false). Before any of that, a
24 * server is the joints alone, and a release (the last target of a gesture) as a release. A turn that ends stops what drives without holding ("hold": false). Before any of that, a
2525 * frame over 1 KiB closes the stream, and frames past 200 a second are dropped and, kept up, close it (src/frame-limit.ts):
2626 * the arm holds then.
2727 *
2828 * A /follow stream whose browser goes away without closing it (or stops answering pings) is closed toward the arm
29 * with LOST_CLOSE: the arm server parks the arm then, where a stream closed on purpose only holds.
29 * with LOST_CLOSE: the arm server parks the arm then, where a stream closed on purpose finishes to its release (or
30 * holds), and one this relay closed (a STOP, a turn over, a broken limit) goes with HELD_CLOSE and holds where it is.
3031 *
3132 * The owner's speed cap (src/speed-cap.ts) is applied here too, to moves, poses, grips, playbacks and every stream frame.
3233 *
@@ -81,7 +82,7 @@ import { proxyFor, whyNot } from "./egress.ts";
8182 import { CalibrationError } from "./leader-map.js";
8283 import type { Lobby } from "./lobby.ts";
8384 import { capBody, FrameCap } from "./speed-cap.ts";
84 import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts";
85 import { Clock, CommandGate, HELD_CLOSE, parseCommand, releaseText } from "./command-gate.ts";
8586 import { FLOOD_CLOSE, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts";
8687
8788 export interface Remote {
@@ -700,7 +701,10 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
700701 // but it parks only while not held, and the STOP it is sent at once holds it and aborts a park under way; frames
701702 // that did get out carry the old epoch, which it refuses once it has the STOP.
702703 for (const arm of armSides.values()) arm.terminate();
703 for (const browser of streams.keys()) browser.close(HELD_CLOSE, `stopped by ${by}`);
704 for (const browser of streams.keys()) {
705 ended.add(browser);
706 browser.close(HELD_CLOSE, `stopped by ${by}`);
707 }
704708 }
705709 // 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.
706710 const raw = reading || route === "POST /estop" || route === "POST /estop/clear" || route === "POST /stop" || route === "POST /resume" ? undefined : await readBody(req);
@@ -758,6 +762,8 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
758762 const sendable = (code: number) => (code >= 1000 && code <= 1003) || (code >= 1007 && code <= 1014) || (code >= 3000 && code <= 4999);
759763 /** Each open /follow stream and whose it is, so a turn that ends can close it. */
760764 const streams = new Map<WebSocket, string>();
765 /** Streams this relay closed itself: toward the arm, they hold where it is and never land on a release. */
766 const ended = new WeakSet<WebSocket>();
761767 /** The arm-side socket of each stream, so a STOP can cut what it has queued but not yet written. */
762768 const armSides = new Map<WebSocket, WebSocket>();
763769 /**
@@ -825,7 +831,7 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
825831 machine.note("clamp", "not passed on", { why: "too many frames" });
826832 return browser.close(FLOOD_CLOSE, "too many frames: the stream was closed");
827833 }
828 if (pace === "drop") return void metrics.count("follow_frames_dropped");
834 if (pace === "drop" && !parseCommand(String(raw))?.release) return void metrics.count("follow_frames_dropped"); // a release is never dropped for pace
829835 if (driver !== null) {
830836 // The turn may have ended while this frame was on its way, before the close reaches the page: it is not passed on.
831837 if (lobby && !lobby.turns.isDriver(driver) && !lobby.local) return void machine.note("clamp", "not passed on", { why: "the turn ended" });
@@ -835,25 +841,30 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
835841 if (!verdict.ok) return void machine.note("clamp", "not passed on", { why: verdict.drop });
836842 if (arm.readyState !== WebSocket.OPEN) return;
837843 if (real && mode() !== "live") return void machine.note("clamp", "not passed on", { why: RECONNECTING });
838 const data = JSON.stringify(Object.fromEntries(verdict.joints));
844 const text = JSON.stringify(Object.fromEntries(verdict.joints));
839845 const percent = lobby?.maxSpeedPercent ?? 100;
840 const text = String(data);
846 const what = verdict.release ? "release" : "follow frame";
847 // A release goes on as one, so the arm server lands on it rather than stopping where the arm is.
848 const out = (frame: string) => (verdict.release ? releaseText(parseFrame(frame) ?? new Map(), { epoch }) : stamped(frame));
841849 if (percent >= 100) {
842850 if (lobby) cap.cap(text, percent); // keeps each joint's last target, so a cap set mid-stream starts from there
843 machine.note("sent", "follow frame", { values: values(text) });
844 arm.send(stamped(data));
851 machine.note("sent", what, { values: values(text) });
852 arm.send(out(text));
845853 return;
846854 }
847855 if (!seeded) seed();
848856 const capped = cap.cap(text, percent);
849857 if (capped !== text) machine.note("clamp", "speed cap", { percent, asked: values(text), sent: capped === null ? {} : values(capped) });
850858 if (capped === null) return;
851 machine.note("sent", "follow frame", { values: values(capped) });
852 arm.send(stamped(capped));
859 machine.note("sent", what, { values: values(capped) });
860 arm.send(out(capped));
853861 });
854862 arm.on("close", (code, why) => browser.close(sendable(code) ? code : 1011, why.length ? why.toString() : "the arm server went away"));
855 // A browser that closed the stream on purpose leaves the arm holding; one that vanished leaves it parking.
856 browser.on("close", (code) => (overLimit || code === 1000 || code === 1005 || code === HELD_CLOSE ? arm.close(1000) : arm.close(LOST_CLOSE, "the driver's connection was lost")));
863 // A browser that closed the stream on purpose lets the arm finish to its release, or holds; one this relay ended
864 // (a STOP, a turn over, a broken limit) holds where it is; one that vanished leaves it parking.
865 browser.on("close", (code) =>
866 overLimit || ended.has(browser) || code === HELD_CLOSE ? arm.close(HELD_CLOSE, "ended by the relay") : code === 1000 || code === 1005 ? arm.close(1000) : arm.close(LOST_CLOSE, "the driver's connection was lost"),
867 );
857868 let answered = true;
858869 browser.on("pong", () => (answered = true));
859870 const heartbeat = setInterval(() => {
@@ -960,7 +971,11 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
960971 const settingsChanged = lobby?.onSettingsChange(follow);
961972 const arrived = lobby?.onArrive((tell) => (pushed ? tell(pushed) : void push()));
962973 const turnEnded = lobby?.onTurnEnd((who) => {
963 for (const [browser, id] of streams) if (id === who.id) browser.close(1000, "your turn ended");
974 for (const [browser, id] of streams) {
975 if (id !== who.id) continue;
976 ended.add(browser);
977 browser.close(1000, "your turn ended");
978 }
964979 const base = remote && onArm() ? remote.http : simulation;
965980 fetch(`${base}/stop`, { method: "POST", headers: { "content-type": "application/json" }, body: JSON.stringify({ by: who.name, hold: false }), signal: AbortSignal.timeout(readTimeoutMs) }).then(
966981 (r) => r.body?.cancel(),
Software/so101/src/release-landing.test.ts added +279
@@ -0,0 +1,279 @@
1 import { afterEach, describe, expect, test } from "bun:test";
2 import { mkdtempSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import WebSocket from "ws";
6 import { Accounts } from "./accounts.ts";
7 import { FeetechBus } from "./bus.ts";
8 import { FakeBus } from "./fake-bus.ts";
9 import { sign } from "./identity.ts";
10 import { Lobby } from "./lobby.ts";
11 import { startRelay, type RunningRelay } from "./relay.ts";
12 import { DEFAULT_SAFETY, TICKS_PER_DEGREE } from "./safety.ts";
13 import { LAND_NEAR, startServer, type RunningServer } from "./server.ts";
14 import { STS3215 } from "./sts3215.ts";
15 import { Frames } from "./ui/frames.js";
16 import { arrivalMs, Intent } from "./ui/intent.js";
17
18 // Letting go means "go there and stop": the page's own Frames and Intent, a real arm server and its Governor, the fake bus.
19
20 const IDS = [1, 2, 6] as const;
21 const FROM = 2048;
22 const TARGET = FROM + Math.round(40 * TICKS_PER_DEGREE); // 40° of shoulder pan
23 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
24
25 interface Rig {
26 fake: FakeBus;
27 server: RunningServer;
28 /** Every Goal_Position sync write, with when it went out. */
29 goals: Array<{ at: number; values: ReadonlyArray<readonly [number, number]> }>;
30 /** Where joint 1 is now, ticks. */
31 pan(): number;
32 }
33
34 let rigs: Rig[] = [];
35 let relay: RunningRelay | undefined;
36 let lobby: Lobby | undefined;
37 const sockets: WebSocket[] = [];
38 afterEach(async () => {
39 for (const s of sockets.splice(0)) s.terminate();
40 await Promise.race([relay?.close(), sleep(500)]);
41 lobby?.close();
42 relay = lobby = undefined;
43 await Promise.all(rigs.splice(0).map((r) => Promise.race([r.server.close(), sleep(500)])));
44 });
45
46 async function rig(): Promise<Rig> {
47 const fake = new FakeBus([...IDS]);
48 for (const [id, min, max] of [[1, 818, 3487], [2, 813, 3202], [6, 1995, 3525]] as const) fake.servos.get(id)!.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9);
49 const bus = new FeetechBus(fake);
50 const goals: Rig["goals"] = [];
51 const write = bus.syncWrite.bind(bus);
52 bus.syncWrite = (async (register: typeof STS3215.Goal_Position, values: ReadonlyArray<readonly [number, number]>) => {
53 if (register === STS3215.Goal_Position) goals.push({ at: performance.now(), values: [...values] });
54 return write(register, values);
55 }) as typeof bus.syncWrite;
56 const server = await startServer(bus, { host: "127.0.0.1", port: 0, ids: [...IDS], posesFile: join(mkdtempSync(join(tmpdir(), "so101-land-")), "poses.json") });
57 const pan = () => { const m = fake.servos.get(1)!; return m[56]! | (m[57]! << 8); };
58 const r = { fake, server, goals, pan };
59 rigs.push(r);
60 return r;
61 }
62
63 const connect = (url: string, headers: Record<string, string> = {}) =>
64 new Promise<WebSocket>((resolve, reject) => {
65 const ws = new WebSocket(url, { headers });
66 sockets.push(ws);
67 ws.once("open", () => resolve(ws));
68 ws.once("error", reject);
69 });
70
71 /** The page's stream: Frames on a 33 ms slot writing to the socket. */
72 const page = (ws: WebSocket) => new Frames((data) => ws.readyState === WebSocket.OPEN && ws.send(data));
73
74 /** A drag of joint 1 from FROM to TARGET in 200 ms, a frame every ~33 ms, the way a pointer feeds Frames. */
75 async function drag(frames: Frames, intent?: Intent) {
76 for (let k = 1; k <= 6; k++) {
77 const ticks = Math.round(FROM + ((TARGET - FROM) * k) / 6);
78 intent?.set(1, ticks, performance.now());
79 frames.send({ 1: ticks });
80 await sleep(33);
81 }
82 }
83
84 /** A write of every joint's goal at once: the "hold where you are" write, never a governor step (which names only joints that move). */
85 const holdWrites = (r: Rig, from: number) => r.goals.filter((g) => g.at >= from && g.values.length === IDS.length);
86
87 async function until(ok: () => boolean, ms: number) {
88 const end = performance.now() + ms;
89 while (performance.now() < end && !ok()) await sleep(5);
90 return ok();
91 }
92
93 describe("a release lands on the target", () => {
94 test("drag 40° in 200 ms and let go: the arm reaches the target, nothing parks it short first, and the model never leaves the target", async () => {
95 const r = await rig();
96 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
97 const frames = page(ws);
98 const intent = new Intent();
99 await sleep(50);
100 const started = performance.now();
101 await drag(frames, intent);
102 // Let go, as the page does: the release now, the intent held for the predicted arrival, the socket closed 250 ms later.
103 const speed = DEFAULT_SAFETY.maxSpeedDegS * TICKS_PER_DEGREE, accel = DEFAULT_SAFETY.maxAccelDegS2 * TICKS_PER_DEGREE;
104 intent.release(1, performance.now(), arrivalMs(TARGET - r.pan(), speed, accel), r.pan());
105 const short = r.pan();
106 frames.release();
107 setTimeout(() => ws.close(), 250);
108 expect(short).toBeLessThan(TARGET - LAND_NEAR); // the Governor is still on its way when the finger lifts
109
110 let arrivedAt = Infinity;
111 const shown: Array<number | null> = [];
112 const end = performance.now() + 2000;
113 while (performance.now() < end) {
114 const p = r.pan();
115 shown.push(intent.target(1, p, performance.now()));
116 if (Math.abs(p - TARGET) <= LAND_NEAR && arrivedAt === Infinity) arrivedAt = performance.now();
117 if (arrivedAt !== Infinity && performance.now() - arrivedAt > 400) break;
118 await sleep(10);
119 }
120 expect(arrivedAt).toBeLessThan(Infinity);
121 expect(Math.abs(r.pan() - TARGET)).toBeLessThanOrEqual(LAND_NEAR);
122 // The arm took the stream (one hold write when it armed); after that, none until it got there, nor after.
123 expect(holdWrites(r, started).length).toBe(1);
124 expect(holdWrites(r, started)[0]!.at).toBeLessThan(started + 50);
125 // Pan's goals only ever climb toward the target.
126 const pans = r.goals.filter((g) => g.at >= started).flatMap((g) => g.values.filter(([id]) => id === 1).map(([, v]) => v));
127 expect(pans.every((v, i) => i === 0 || v >= pans[i - 1]!)).toBe(true);
128 expect(pans.at(-1)).toBe(TARGET);
129 // The model: the target until the arm is there, then the arm (which is there). Never anything else, never a miss.
130 expect(shown.every((v) => v === TARGET || v === null)).toBe(true);
131 expect(shown.indexOf(null)).toBeGreaterThan(0);
132 expect(shown.slice(shown.indexOf(null)).every((v) => v === null)).toBe(true);
133 expect(intent.misses()).toEqual([]);
134 });
135
136 test("through the relay, the release goes on as a release and the arm lands the same", async () => {
137 const r = await rig();
138 const KEY = "test-identity-key";
139 const identity = `u-alice|alice|user|${Date.now() + 3_600_000}`;
140 const ALICE = { "x-forge-identity": identity, "x-forge-signature": sign(identity, KEY) };
141 lobby = new Lobby({ identityKey: KEY, accounts: new Accounts() });
142 relay = await startRelay({ port: 0, simulation: `http://127.0.0.1:${r.server.port}`, lobby });
143 const live = await connect(`ws://127.0.0.1:${relay.port}/live`, ALICE);
144 const driving = new Promise<void>((ok) => live.on("message", (d) => JSON.parse(String(d)).driver?.name === "alice" && ok()));
145 live.send(JSON.stringify({ type: "join" }));
146 await driving;
147 const ws = await connect(`ws://127.0.0.1:${relay.port}/follow`, ALICE);
148 const frames = page(ws);
149 await sleep(50);
150 const started = performance.now();
151 await drag(frames);
152 frames.release();
153 setTimeout(() => ws.close(), 250);
154 expect(await until(() => Math.abs(r.pan() - TARGET) <= LAND_NEAR, 2000)).toBe(true);
155 await sleep(300);
156 expect(holdWrites(r, started + 50)).toEqual([]);
157 expect(Math.abs(r.pan() - TARGET)).toBeLessThanOrEqual(LAND_NEAR);
158 });
159
160 test("a 90° flick in 100 ms is followed along the Governor's profile to the target, no frame dropped for jumping too far", async () => {
161 const r = await rig();
162 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
163 const frames = page(ws);
164 const flick = FROM + Math.round(90 * TICKS_PER_DEGREE);
165 await sleep(50);
166 // A slow phone's pointer: three slots, the last two jumping ~45° each, then the finger is off.
167 frames.send({ 1: FROM + 10 });
168 await sleep(33);
169 frames.send({ 1: FROM + 512 });
170 await sleep(33);
171 frames.send({ 1: flick });
172 await sleep(34);
173 frames.release();
174 setTimeout(() => ws.close(), 250);
175 let dropped = 0;
176 const end = performance.now() + 3000;
177 while (performance.now() < end && Math.abs(r.pan() - flick) > LAND_NEAR) {
178 const s = (await (await fetch(`http://127.0.0.1:${r.server.port}/status`)).json()) as { stream: { dropped: number } | null };
179 dropped = Math.max(dropped, s.stream?.dropped ?? 0);
180 await sleep(50);
181 }
182 expect(Math.abs(r.pan() - flick)).toBeLessThanOrEqual(LAND_NEAR);
183 expect(dropped).toBe(0);
184 // Followed, not jumped: pan's goals climb in governor steps, none further than the top speed allows in a couple of ticks.
185 const pans = r.goals.flatMap((g) => g.values.filter(([id]) => id === 1).map(([, v]) => v)).filter((v) => v > FROM + 10);
186 const steps = pans.slice(1).map((v, i) => v - pans[i]!);
187 expect(steps.every((d) => d >= 0)).toBe(true);
188 expect(Math.max(...steps)).toBeLessThanOrEqual(Math.ceil((2 * DEFAULT_SAFETY.maxSpeedDegS * TICKS_PER_DEGREE) / DEFAULT_SAFETY.rateHz) + 1);
189 });
190
191 test("a press held still for 3 s keeps its target: the page's keepalive never lets the dead-man park it", async () => {
192 const r = await rig();
193 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
194 const frames = page(ws);
195 await sleep(50);
196 await drag(frames);
197 const started = performance.now();
198 const deadman: boolean[] = [];
199 const end = started + 3000;
200 while (performance.now() < end) {
201 frames.keep(); // the page calls it every 250 ms while pressed
202 const s = (await (await fetch(`http://127.0.0.1:${r.server.port}/status`)).json()) as { deadman: boolean };
203 deadman.push(s.deadman);
204 await sleep(100);
205 }
206 expect(deadman.length).toBeGreaterThan(10);
207 expect(deadman.every((d) => d === false)).toBe(true);
208 expect(holdWrites(r, started)).toEqual([]);
209 expect(Math.abs(r.pan() - TARGET)).toBeLessThanOrEqual(LAND_NEAR);
210 });
211
212 test("without the keepalive the dead-man does fire after holdAfterMs (what the keepalive is for)", async () => {
213 const r = await rig();
214 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
215 const frames = page(ws);
216 await sleep(50);
217 await drag(frames);
218 await sleep(DEFAULT_SAFETY.holdAfterMs + 300);
219 const s = (await (await fetch(`http://127.0.0.1:${r.server.port}/status`)).json()) as { deadman: boolean };
220 expect(s.deadman).toBe(true);
221 });
222 });
223
224 describe("only an abnormal end stops where the arm is", () => {
225 test("a lost link during the move parks at present", async () => {
226 const r = await rig();
227 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
228 const frames = page(ws);
229 await sleep(50);
230 const started = performance.now();
231 await drag(frames);
232 frames.release();
233 await sleep(20);
234 ws.terminate(); // not a close: the link is gone
235 expect(await until(() => holdWrites(r, started + 50).length > 0, 1000)).toBe(true);
236 const [hold] = holdWrites(r, started + 50);
237 const pan = hold!.values.find(([id]) => id === 1)![1];
238 expect(pan).toBeLessThan(TARGET - LAND_NEAR); // held where it was, short of the target
239 });
240
241 test("STOP during the move holds at once, a landing included", async () => {
242 const r = await rig();
243 const ws = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
244 const frames = page(ws);
245 await sleep(50);
246 const started = performance.now();
247 await drag(frames);
248 frames.release();
249 ws.close(); // a clean close: the arm is landing
250 await sleep(60);
251 const stoppedAt = performance.now();
252 const res = await fetch(`http://127.0.0.1:${r.server.port}/stop`, { method: "POST", headers: { "content-type": "application/json" }, body: "{}" });
253 expect(((await res.json()) as { stopped: string }).stopped).toBe("stream");
254 expect(await until(() => holdWrites(r, started + 50).length > 0, 500)).toBe(true);
255 const here = r.pan();
256 expect(here).toBeLessThan(TARGET - LAND_NEAR);
257 await sleep(400);
258 expect(r.goals.filter((g) => g.at > stoppedAt + 100 && g.values.some(([id]) => id === 1)).length).toBe(0);
259 expect(r.pan()).toBe(here);
260 });
261
262 test("a new press while the last one lands takes over at once, never refused as busy", async () => {
263 const r = await rig();
264 const first = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
265 const frames = page(first);
266 await sleep(50);
267 await drag(frames);
268 frames.release();
269 first.close();
270 await sleep(60);
271 const second = await connect(`ws://127.0.0.1:${r.server.port}/follow`);
272 const closed = new Promise<number>((ok) => second.once("close", (code) => ok(code)));
273 const next = page(second);
274 next.send({ 1: FROM });
275 next.release();
276 expect(await Promise.race([closed, sleep(200).then(() => 0)])).toBe(0);
277 expect(await until(() => Math.abs(r.pan() - FROM) <= LAND_NEAR, 3000)).toBe(true);
278 });
279 });
Software/so101/src/safety.test.ts +26 −1
@@ -1,5 +1,5 @@
11 import { describe, expect, test } from "bun:test";
2 import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, parseFrame, validateSafety, type Bounds } from "./safety.ts";
2 import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, parseFrame, travelSeconds, validateSafety, type Bounds } from "./safety.ts";
33
44 describe("safety settings", () => {
55 test("conservative defaults, and every knob checked against its range", () => {
@@ -113,3 +113,28 @@ describe("frames off the wire", () => {
113113 expect(parseFrame("nope")).toBeNull();
114114 });
115115 });
116
117 describe("the governor's estimate", () => {
118 const bounds = new Map<number, Bounds>([[1, { min: 818, max: 3487 }]]);
119 const limits = motionLimits(DEFAULT_SAFETY);
120 test("travel at rest is the trapezoid, or the triangle when top speed is never reached", () => {
121 expect(travelSeconds(1024, 1024, 4096)).toBeCloseTo(1.25, 5);
122 expect(travelSeconds(-256, 1024, 4096)).toBeCloseTo(0.5, 5);
123 expect(travelSeconds(0, 1024, 4096)).toBe(0);
124 });
125 test("eta bounds the steps the governor actually takes to get there, mid-move included", () => {
126 const g = new Governor(() => bounds, () => limits);
127 g.reset(new Map([[1, 2048]]));
128 g.aim(1, 2503);
129 let steps = 0;
130 for (let k = 0; k < 6; k++, steps++) g.step(1 / 60);
131 const eta = g.eta();
132 expect(eta).toBeGreaterThan(0);
133 for (; g.moving && steps < 600; steps++) g.step(1 / 60);
134 expect(g.goal(1)).toBe(2503);
135 expect((steps - 6) / 60).toBeLessThanOrEqual(eta + 2 / 60);
136 g.aim(1, 2048); // heading the other way: it stops first
137 g.step(1 / 60);
138 expect(g.eta()).toBeGreaterThan(travelSeconds(455, limits.speed, limits.accel) - 0.05);
139 });
140 });
Software/so101/src/safety.ts +29
@@ -188,6 +188,23 @@ export class Governor {
188188 return false;
189189 }
190190
191 /**
192 * Seconds until every joint is at its goal at the current limits: a joint heading the wrong way stops first,
193 * one already heading there is timed as if from rest (an upper bound).
194 */
195 eta(): number {
196 const { speed, accel } = this.#limits();
197 let worst = 0;
198 for (const t of this.#tracks.values()) {
199 const vmax = Math.min(speed, t.slow ?? Infinity);
200 const error = t.goal - t.p;
201 const away = t.v !== 0 && Math.sign(t.v) !== Math.sign(error);
202 const seconds = away ? Math.abs(t.v) / accel + travelSeconds(Math.abs(error) + (t.v * t.v) / (2 * accel), vmax, accel) : travelSeconds(error, vmax, accel);
203 worst = Math.max(worst, seconds);
204 }
205 return worst;
206 }
207
191208 /** Advance every joint by `dt` seconds; returns the goals that changed on the wire, rounded to ticks. */
192209 step(dt: number): Array<readonly [number, number]> {
193210 const { speed, accel } = this.#limits();
@@ -226,6 +243,11 @@ export function parseFrame(data: string): Map<number, number> | null {
226243 } catch {
227244 return null;
228245 }
246 return jointsOf(parsed);
247 }
248
249 /** The joints in an already parsed frame, as `parseFrame` reads them. */
250 export function jointsOf(parsed: unknown): Map<number, number> | null {
229251 if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) return null;
230252 const out = new Map<number, number>();
231253 for (const [key, value] of Object.entries(parsed)) {
@@ -234,3 +256,10 @@ export function parseFrame(data: string): Map<number, number> | null {
234256 }
235257 return out;
236258 }
259
260 /** Seconds a joint at rest takes to travel `distance` ticks at `speed` ticks/s top and `accel` ticks/s², stopping at the end. */
261 export function travelSeconds(distance: number, speed: number, accel: number): number {
262 const d = Math.abs(distance);
263 if (d === 0) return 0;
264 return d <= (speed * speed) / accel ? 2 * Math.sqrt(d / accel) : d / speed + speed / accel;
265 }
Software/so101/src/server.ts +113 −47
@@ -41,6 +41,9 @@
4141 * GET /printables the sheets to print for the vision quest (board, cube markers, gripper marker); GET /printables/<file>
4242 * WS /follow client sends {"1": ticks, "2": ticks, ...} frames; the server drives the arm. A frame may carry
4343 * "seq" and "t" (src/command-gate.ts): then a stale, replayed or pre-resume one is dropped.
44 * {"type": "release", "joints": {...}} is a finger letting go: on a clean close after it the
45 * governor finishes to those targets (within LAND_NEAR ticks, or its estimate plus LAND_GRACE_MS)
46 * before the stream lets go of the arm; a lost link, STOP, E-stop or HELD_CLOSE holds where it is.
4447 * A frame over 1 KiB closes the stream; frames past 200 a second are dropped, and a stream that
4548 * keeps that up is closed (src/frame-limit.ts)
4649 * WS /watch read-only: the server sends the /status body every watchEveryMs, one bus read shared by every watcher;
@@ -92,8 +95,6 @@ export interface ServerOptions {
9295 printablesDir?: string;
9396 /** Torque cap while a stream drives the arm, percent. */
9497 streamTorque?: number;
95 /** Largest per-frame change accepted on the stream, ticks. */
96 maxStep?: number;
9798 /** With no frame for this long, the arm is told to hold where it is; overrides the holdAfterMs setting. */
9899 watchdogMs?: number;
99100 /** How often a stream's client is pinged; one that misses a ping is a lost connection, and the arm parks. */
@@ -144,8 +145,14 @@ export function actor(body: Record<string, unknown>): string {
144145 return by || "someone at the arm";
145146 }
146147
147 /** Close codes a stream ends with on purpose; any other end is a lost connection. */
148 /** Close codes a stream ends with on purpose; any other end is a lost connection. After a release, these let the arm land. */
148149 const CLEAN_CLOSE = new Set([1000, 1005]);
150 /** After a release and a clean close, a joint this close to its last target has arrived, ticks. */
151 export const LAND_NEAR = 12;
152 /** How much longer than the governor's own estimate a landing may take before the arm holds where it got to, ms. */
153 export const LAND_GRACE_MS = 500;
154 /** However far the move, a landing never outlasts this, ms. */
155 const LAND_MAX_MS = 10_000;
149156 /** The close code for a stream ended by an emergency stop. */
150157 export const ESTOP_CLOSE = 4423;
151158
@@ -162,8 +169,6 @@ const URDF = "so101_new_calib.urdf";
162169 /** Load, in percent of the servo's maximum, that counts as pushing against something. */
163170 const JAM_LOAD = 45;
164171 const JAM_WINDOW = 6;
165 /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */
166 const RESUME_AFTER_DROPS = 20;
167172 const VENDORED = new Set(["three", "urdf-loader"]);
168173 /** A module the page loads from src/ui: plain JavaScript, never its tests. */
169174 const isUiModule = (name: string) => /^[a-z][a-z0-9-]*\.(js|css)$/.test(name);
@@ -177,7 +182,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
177182 const calibrationDir = join(dataDir, "calibration");
178183 const settingsFile = join(dataDir, "settings.json");
179184 const streamTorque = options.streamTorque ?? 70;
180 const maxStep = options.maxStep ?? 400;
181185 const easeSpeed = options.easeSpeed ?? 300;
182186 const jamSampleMs = options.jamSampleMs ?? 100;
183187 const heartbeatMs = options.heartbeatMs ?? 1000;
@@ -202,6 +206,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
202206 };
203207 /** The stream's dead-man hold is in force: no frame came for holdAfterMs, and the arm stays where it was. */
204208 let held = false;
209 /** A stream closed after a release, still carrying the arm to its last target; a new stream takes over from it. */
210 let landing: { handOver(): void } | null = null;
205211
206212 const limits = new Map<number, { min: number; max: number }>();
207213 for (const id of ids) {
@@ -341,7 +347,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
341347 machine.note("state", "emergency stop", { by });
342348 }
343349 const was = driver;
344 if (was === "stream") for (const client of wss.clients) stopCause.set(client, "STOP"), end(client, ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks
350 if (was === "stream") for (const client of streamsNow()) stopCause.set(client, "STOP"), end(client, ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks
345351 else if (was === "record" && recording) {
346352 const { done } = recording;
347353 recording = null; // nothing is saved: the recording ends where it was cut
@@ -885,7 +891,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
885891 gate.stopped(body.epoch);
886892 if (body.hold !== false && gate.hold(actor(body))) metrics.event("hold", { by: actor(body) });
887893 const was = driver;
888 if (was === "stream") for (const client of wss.clients) stopCause.set(client, body.hold === false ? "turn-end" : "STOP"), end(client, 1000, "stopped from the viewer"); // its close handler holds the arm
894 if (was === "stream") for (const client of streamsNow()) stopCause.set(client, body.hold === false ? "turn-end" : "STOP"), end(client, 1000, "stopped from the viewer"); // its close handler holds the arm, a landing included
889895 else if (was === "record") {
890896 const summary = await finishRecording();
891897 return json(res, 200, { stopped: was, ...summary });
@@ -1015,6 +1021,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10151021 halts.get(ws)?.();
10161022 ws.close(code, why);
10171023 };
1024 /** Every stream with a say over the arm: the open ones, and one closed after a release that is still landing. */
1025 const streamsNow = () => new Set([...wss.clients, ...halts.keys()]);
10181026 /** Each sender's clock, by address: one device keeps one clock, so a burst it flushes on a new stream is still known as old. */
10191027 const clocks = new Map<string, Clock>();
10201028 const clock = (req: IncomingMessage | undefined) => {
@@ -1033,6 +1041,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10331041 ws.close(HELD_CLOSE, holding(gate.held));
10341042 return;
10351043 }
1044 if (driver === "stream") landing?.handOver(); // a new press takes over from the last one's landing
10361045 if (driver) {
10371046 ws.close(1013, `${describe(driver)} is driving the arm`);
10381047 return;
@@ -1044,6 +1053,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10441053 const admission = gate.open(clock(req), false);
10451054 /** Set the moment the stream ends: whatever is queued or under way gives up, and nothing more reaches the bus. */
10461055 let closed = false;
1056 /** The last command was a release: "go there and stop". A clean close then lands on it instead of holding short. */
1057 let released = false;
1058 /** A new stream took the arm over while this one landed: it owns the driver and the shared state now. */
1059 let handedOver = false;
10471060 streamStats = stats;
10481061 const tracker = (tracking = precision.begin());
10491062 /** Snap-backs are counted once per episode: a joint held at its limit, a zone refusal, a run of stale frames. */
@@ -1063,8 +1076,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10631076 const period = 1000 / settings.rateHz;
10641077 const governor = new Governor(bounds, speedLimits);
10651078 let last: Map<number, number> | null = null;
1066 /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */
1067 const drops = new Map<number, number>();
10681079 /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */
10691080 let heldDepth = 0;
10701081 /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */
@@ -1196,18 +1207,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
11961207 windows.delete(id);
11971208 }
11981209 const previous = last?.get(id);
1199 if (previous === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped
1200 else if (Math.abs(target - previous) > maxStep) {
1201 stats.dropped++;
1202 machine.note("clamp", "too far in one frame", { id, asked: target, from: previous, maxStep });
1203 const n = (drops.get(id) ?? 0) + 1;
1204 drops.set(id, n);
1205 if (n === 1) tracker.snapBack("maxStep-drop", Date.now(), id, target - previous);
1206 if (n < RESUME_AFTER_DROPS) continue; // no hand moves that far in one frame
1207 // The leader has stayed far away (after a block or a jam): ease over to it rather than leave the joint stranded.
1208 drops.delete(id);
1209 ease = true;
1210 } else drops.delete(id);
1210 // A joint this stream has not driven yet is eased over, never jumped. However far a driven joint's target jumps
1211 // (a flick, a tap on the rail), it is followed: the governor holds the motion to the speed and acceleration
1212 // limits, so a jump is a fast move, never a lurch, and a one-frame glitch is undone by the next frame.
1213 if (previous === undefined) ease = true;
12111214 targets.push([id, target]);
12121215 }
12131216 if (!targets.length) return;
@@ -1311,8 +1314,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13111314 overLimit.add(ws);
13121315 return end(ws, FLOOD_CLOSE, "too many frames: the stream was closed");
13131316 }
1314 if (pace === "drop") return void stats.gated++;
13151317 const verdict = admission.admit(String(data));
1318 if (pace === "drop" && !(verdict.ok && verdict.release)) return void stats.gated++; // a release is never dropped for pace
13161319 if (!verdict.ok) {
13171320 if (verdict.drop !== "malformed") {
13181321 stats.gated++;
@@ -1323,8 +1326,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13231326 }
13241327 staling = false;
13251328 const frame = verdict.joints;
1329 released = verdict.release === true;
13261330 clearTimeout(timer);
1327 timer = setTimeout(() => void hold(), holdAfterMs);
1331 // After a release the silence is meant: the governor finishes, and no dead-man parks the arm short of it.
1332 if (!released) timer = setTimeout(() => void hold(), holdAfterMs);
13281333 if (!arming) {
13291334 arming = true;
13301335 void arm();
@@ -1342,35 +1347,96 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13421347 clearInterval(ticker);
13431348 };
13441349 halts.set(ws, halt);
1350
1351 /**
1352 * After a release and a clean close: the governor carries on to the last targets. True once every joint this
1353 * stream drove is within LAND_NEAR ticks of its target; false when it is not there by the governor's own estimate
1354 * at the current limits plus LAND_GRACE_MS, or when the stream is halted (STOP, E-stop, a new press).
1355 */
1356 const land = async (): Promise<boolean> => {
1357 const giveUp = Date.now() + LAND_MAX_MS;
1358 let deadline = Infinity;
1359 while (!closed && Date.now() < Math.min(deadline, giveUp)) {
1360 if (!pending && !easing) {
1361 await chain; // the release is in the governor
1362 if (closed) break;
1363 if (deadline === Infinity) deadline = Date.now() + governor.eta() * 1000 + LAND_GRACE_MS;
1364 if (!governor.moving) {
1365 const wanted = last;
1366 if (!wanted) return true; // nothing was sent anywhere: the arm is where its goal is
1367 let there = false;
1368 await run(async () => {
1369 const positions = await bus.syncRead([...wanted.keys()], STS3215.Present_Position);
1370 for (const [id, p] of positions) tracker.sample(id, p, Date.now());
1371 there = [...wanted].every(([id, t]) => positions.has(id) && Math.abs(positions.get(id)! - t) <= LAND_NEAR);
1372 });
1373 if (there) return true;
1374 }
1375 }
1376 await new Promise((r) => setTimeout(r, period));
1377 }
1378 return false;
1379 };
1380
13451381 ws.on("close", (code) => {
1346 halt();
1347 halts.delete(ws);
13481382 admission.close();
13491383 clearInterval(heartbeat);
1350 const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE && !overLimit.has(ws);
1351 const cause: Cause | null = stopCause.get(ws) ?? (lost && armed ? "release-park" : null);
1352 void run(async () => {
1353 try {
1354 if (armed) {
1355 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1356 const [id, short] = shortfall(positions);
1357 if (cause === "release-park") tracker.snapBack(cause, Date.now(), null, null);
1358 else if (cause && Math.abs(short) > ARRIVE_TICKS) tracker.snapBack(cause, Date.now(), id, short);
1359 if (!held) tracker.release(Date.now());
1360 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
1361 await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, id === 6 ? settings.gripperTorquePercent * 10 : 1000] as const));
1362 }
1363 } finally {
1364 blocked = null;
1365 held = false;
1366 jams.clear();
1367 streamStats = null;
1384 clearTimeout(timer);
1385 const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE && code !== HELD_CLOSE && !overLimit.has(ws);
1386 // Only a clean close after a release lands; a lost link, a STOP or E-stop (already halted), a stream the relay
1387 // ended (HELD_CLOSE: a turn over, a STOP) or one over the limits stops where the arm is.
1388 const lands = armed && released && !closed && CLEAN_CLOSE.has(code) && !overLimit.has(ws);
1389 let finished: Promise<boolean> = Promise.resolve(false);
1390 if (lands) {
1391 machine.note("state", "landing", { on: last ? perServo([...last]) : {} });
1392 landing = {
1393 handOver() {
1394 handedOver = true;
1395 landing = null;
1396 halt();
1397 release(); // the new stream claims the arm at once; this one's cleanup leaves the shared state to it
1398 },
1399 };
1400 const mine = landing;
1401 finished = land().finally(() => {
1402 if (landing === mine) landing = null;
1403 });
1404 }
1405 void finished.then((landed) => {
1406 halt();
1407 halts.delete(ws);
1408 if (lands) machine.note("state", landed ? "landed" : handedOver ? "handed over" : "landed short", {});
1409 if (handedOver) {
13681410 if (tracking === tracker) tracking = null;
13691411 precision.end(tracker);
1370 metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats, ...(lost ? { lost: code } : {}) });
1371 release(); // whatever the bus did, the arm is free for the next driver
1412 return void metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats });
13721413 }
1373 if (lost && armed) void park(`the stream was lost (${code})`);
1414 // A STOP or turn end names its cause; a lost link parks. A landing reached its targets, so it is no snap-back.
1415 const cause: Cause | null = stopCause.get(ws) ?? (lost && armed ? "release-park" : null);
1416 void run(async () => {
1417 try {
1418 if (armed) {
1419 const positions = await bus.syncRead(ids, STS3215.Present_Position);
1420 const [id, short] = shortfall(positions);
1421 if (cause === "release-park") tracker.snapBack(cause, Date.now(), null, null);
1422 else if (cause && Math.abs(short) > ARRIVE_TICKS) tracker.snapBack(cause, Date.now(), id, short);
1423 if (!held) tracker.release(Date.now());
1424 // Landed, the goals are the targets: nothing is written over them. Otherwise the arm holds where it is.
1425 if (!landed) await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
1426 await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, id === 6 ? settings.gripperTorquePercent * 10 : 1000] as const));
1427 }
1428 } finally {
1429 blocked = null;
1430 held = false;
1431 jams.clear();
1432 streamStats = null;
1433 if (tracking === tracker) tracking = null;
1434 precision.end(tracker);
1435 metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats, ...(lost ? { lost: code } : {}) });
1436 release(); // whatever the bus did, the arm is free for the next driver
1437 }
1438 if (lost && armed) void park(`the stream was lost (${code})`);
1439 });
13741440 });
13751441 });
13761442 });
Software/so101/src/ui/frames.js +33 −3
@@ -1,7 +1,10 @@
11 // @ts-check
22 /**
33 * Frames on their way to /follow: `send` coalesces them into one per slot, the newest target winning; `flush` sends
4 * now (a finger let go). Each is stamped with `seq` and `t`, when its newest target was given (src/command-gate.ts).
4 * now. Each is stamped with `seq` and `t`, when its newest target was given (src/command-gate.ts). `release` is the
5 * finger letting go: at once, never coalesced, `{type: "release", seq, t, joints}` with every target of the gesture,
6 * so the arm server goes there and stops. `keep` repeats the targets while a press holds still, so the dead-man
7 * (which parks the arm when frames stop) only fires when the page itself goes quiet.
58 * @typedef {Record<number, number>} Frame
69 */
710 /** Tells the page something happened, for the health meter (src/ui/health-meter.js) to count. */
@@ -25,6 +28,10 @@ export class Frames {
2528 this.pending = null;
2629 /** @type {unknown} */
2730 this.timer = null;
31 /** Every joint's newest target written since the last release. @type {Frame} */
32 this.sent = {};
33 /** When the last frame was written. */
34 this.wrote = -Infinity;
2835 }
2936
3037 /** @param {Frame} frame */
@@ -42,17 +49,40 @@ export class Frames {
4249 /** Now, not at the next slot, with anything pending under it. @param {Frame} frame */
4350 flush(frame) {
4451 const data = { ...(this.pending ?? {}), ...frame };
45 this.drop();
52 this.unslot();
4653 this.stamped(data, this.now());
4754 }
4855
4956 /** @param {Frame} data @param {number} t */
5057 stamped(data, t) {
58 Object.assign(this.sent, data);
59 this.wrote = this.now();
5160 this.write(JSON.stringify({ ...data, seq: ++this.seq, t }));
5261 }
5362
54 /** Forget what is pending: the stream ended. */
63 /** The finger let go: the gesture's last targets go out now as a release, anything pending folded in. @param {Frame} [frame] */
64 release(frame = {}) {
65 const joints = { ...this.sent, ...(this.pending ?? {}), ...frame };
66 this.drop();
67 if (!Object.keys(joints).length) return;
68 this.wrote = this.now();
69 this.write(JSON.stringify({ type: "release", seq: ++this.seq, t: this.wrote, joints }));
70 }
71
72 /** A press holding still: the same targets again, freshly stamped, once `every` ms have passed with nothing written. */
73 keep(every = 250) {
74 if (this.timer !== null || !Object.keys(this.sent).length || this.now() - this.wrote < every) return;
75 this.stamped({ ...this.sent }, this.now());
76 }
77
78 /** Forget what is pending and what the gesture sent: the stream ended, or STOP. */
5579 drop() {
80 this.unslot();
81 this.sent = {};
82 }
83
84 /** Cancel the slot waiting, and what it would have sent. */
85 unslot() {
5686 if (this.timer !== null) this.cancel(this.timer);
5787 this.timer = null; this.pending = null;
5888 }
Software/so101/src/ui/frames.test.ts +50
@@ -87,4 +87,54 @@ describe("Frames", () => {
8787 slot();
8888 expect(stamps).toEqual([{ seq: 1, t: 1010 }, { seq: 2, t: 1030 }, { seq: 3, t: 1035 }]);
8989 });
90
91 test("a release goes out now, never coalesced: every target of the gesture and what was pending, as {type: release}", () => {
92 const { frames, slot, waiting } = rig();
93 const sent: string[] = [];
94 const raw = new Frames((d) => sent.push(d), { later: () => 1, cancel: () => {}, now: () => 5000 });
95 raw.flush({ 1: 2000, 2: 2100 });
96 raw.send({ 1: 2050 });
97 raw.release();
98 expect(sent.map((d) => JSON.parse(d))).toEqual([
99 { 1: 2000, 2: 2100, seq: 1, t: 5000 },
100 { type: "release", seq: 2, t: 5000, joints: { 1: 2050, 2: 2100 } },
101 ]);
102 frames.send({ 3: 1900 });
103 frames.release();
104 expect(waiting()).toBe(false);
105 slot();
106 });
107
108 test("after a release or a STOP's drop the gesture is over: the next release carries nothing old, and an empty one is not sent", () => {
109 const sent: string[] = [];
110 const frames = new Frames((d) => sent.push(d), { later: () => 1, cancel: () => {}, now: () => 1 });
111 frames.flush({ 1: 2000 });
112 frames.drop();
113 frames.release();
114 expect(sent.length).toBe(1);
115 frames.flush({ 2: 2200 });
116 frames.release();
117 frames.release();
118 expect(JSON.parse(sent.at(-1)!)).toMatchObject({ type: "release", joints: { 2: 2200 } });
119 expect(sent.length).toBe(3);
120 });
121
122 test("keep repeats the targets, freshly stamped, only once 250 ms pass with nothing written and nothing waiting", () => {
123 const { frames, wire, stamps, slot, tick } = rig();
124 frames.keep();
125 expect(wire).toEqual([]); // nothing to keep yet
126 frames.send({ 1: 2000, 2: 2100 });
127 slot();
128 tick(100);
129 frames.keep();
130 expect(wire.length).toBe(1);
131 tick(150);
132 frames.keep();
133 expect(wire.at(-1)).toEqual({ 1: 2000, 2: 2100 });
134 expect(stamps.at(-1)).toEqual({ seq: 2, t: 1250 });
135 frames.send({ 1: 2010 });
136 tick(300);
137 frames.keep(); // a slot is waiting: it will carry the target
138 expect(wire.length).toBe(2);
139 });
90140 });
Software/so101/src/ui/intent.js +59 −11
@@ -1,31 +1,56 @@
11 /**
22 * What the person asked for, kept apart from where the arm is. A joint dragged by a slider or the 3D model has an
3 * intent (a target in ticks): while it is held, and after release until the arm arrives or `hold` ms pass, the page
4 * shows the intent and never lets a pose update overwrite it. Plain ESM, served at /ui/ and imported by the tests.
3 * intent (a target in ticks): while it is held, and after release until the arm arrives or the time it should take
4 * has passed (`arrivalMs`), the page shows the intent and never lets a pose update overwrite it. A target the arm did
5 * not reach in that time is a miss, for the page to say why. Plain ESM, served at /ui/ and imported by the tests.
56 */
67
8 /** Slack on top of the predicted travel time before an unreached target counts as missed, ms. */
9 export const ARRIVAL_SLACK_MS = 300;
10
11 /**
12 * How long a joint takes to travel `distance` ticks from rest at the arm server's governor limits (src/safety.ts:
13 * `speed` ticks/s top, `accel` ticks/s²), plus ARRIVAL_SLACK_MS.
14 * @param {number} distance @param {number} speed @param {number} accel
15 */
16 export function arrivalMs(distance, speed, accel) {
17 const d = Math.abs(distance);
18 if (!(d > 0) || !(speed > 0) || !(accel > 0)) return ARRIVAL_SLACK_MS;
19 const seconds = d <= (speed * speed) / accel ? 2 * Math.sqrt(d / accel) : d / speed + speed / accel;
20 return Math.round(seconds * 1000) + ARRIVAL_SLACK_MS;
21 }
22
723 export class Intent {
8 /** @param {{ hold?: number, near?: number }} [options] hold: ms an unreached target is kept after release; near: ticks that count as arrived */
24 /** @param {{ hold?: number, near?: number }} [options] hold: ms an unreached target is kept after a release that names no time; near: ticks that count as arrived */
925 constructor(options = {}) {
1026 this.hold = options.hold ?? 2000;
1127 this.near = options.near ?? 12;
12 /** @type {Map<number, { ticks: number, down: boolean, since: number }>} */
28 /** @type {Map<number, { ticks: number, down: boolean, since: number, hold: number, from: number | null }>} */
1329 this.targets = new Map();
30 /** Released targets the arm did not reach in time, until `misses` takes them. @type {Array<{ id: number, ticks: number, actual: number | null }>} */
31 this.missed = [];
1432 }
1533
1634 /** The pointer is on the joint and wants `ticks`. @param {number} id @param {number} ticks @param {number} now */
1735 set(id, ticks, now) {
18 if (Number.isFinite(ticks)) this.targets.set(id, { ticks, down: true, since: now });
36 if (Number.isFinite(ticks)) this.targets.set(id, { ticks, down: true, since: now, hold: this.hold, from: null });
1937 }
2038
21 /** The pointer lifted: the target stays, timed from now. @param {number} id @param {number} now */
22 release(id, now) {
39 /**
40 * The pointer lifted: the target stays, timed from now, for `hold` ms (the predicted arrival, `arrivalMs`).
41 * `from` is where the arm was then: an arm still closing in on the target when the time is up is given longer.
42 * @param {number} id @param {number} now @param {number} [hold] @param {number | null} [from]
43 */
44 release(id, now, hold = this.hold, from = null) {
2345 const t = this.targets.get(id);
24 if (t) { t.down = false; t.since = now; }
46 if (t) { t.down = false; t.since = now; t.hold = hold; t.from = Number.isFinite(from) ? from : null; }
2547 }
2648
27 /** Drop everything (stopped, turn over). */
28 clear() { this.targets.clear(); }
49 /** Drop everything (stopped, turn over): nothing is a miss then. */
50 clear() { this.targets.clear(); this.missed = []; }
51
52 /** The targets missed since the last call. */
53 misses() { return this.missed.splice(0); }
2954
3055 /**
3156 * The target to show for a joint, or null when the real pose is the thing to show. Expires released targets the
@@ -35,8 +60,19 @@ export class Intent {
3560 target(id, actual, now) {
3661 const t = this.targets.get(id);
3762 if (!t) return null;
38 if (!t.down && (now - t.since >= this.hold || (Number.isFinite(actual) && Math.abs(/** @type {number} */ (actual) - t.ticks) <= this.near))) {
63 if (t.down) return t.ticks;
64 const there = Number.isFinite(actual) && Math.abs(/** @type {number} */ (actual) - t.ticks) <= this.near;
65 if (!there && now - t.since >= t.hold && t.from !== null && Number.isFinite(actual)) {
66 const gained = Math.abs(t.from - t.ticks) - Math.abs(/** @type {number} */ (actual) - t.ticks);
67 if (gained > this.near) {
68 // Still on its way (a slow ease, a lagging link): the hold follows the motion a little longer.
69 t.since = now; t.hold = ARRIVAL_SLACK_MS; t.from = /** @type {number} */ (actual);
70 return t.ticks;
71 }
72 }
73 if (there || now - t.since >= t.hold) {
3974 this.targets.delete(id);
75 if (!there) this.missed.push({ id, ticks: t.ticks, actual: Number.isFinite(actual) ? /** @type {number} */ (actual) : null });
4076 return null;
4177 }
4278 return t.ticks;
@@ -46,6 +82,18 @@ export class Intent {
4682 held(id) { return this.targets.get(id)?.down === true; }
4783 }
4884
85 /**
86 * In words, why a released target was not reached, after "held: ". `blocked` is the zone's refusal already in words,
87 * `jammed` the joints the arm server found pushing without moving, `capPercent` the owner's speed cap.
88 * @param {{ joint: string, ticks: number, actual: number | null, id: number, blocked?: string | null, jammed?: readonly number[], capPercent?: number }} m
89 */
90 export function heldWhy(m) {
91 if (m.blocked) return m.blocked;
92 if (m.jammed?.includes(m.id)) return `the ${m.joint} is pushing against something and stopped at ${m.actual ?? "?"}`;
93 if (typeof m.capPercent === "number" && m.capPercent < 100) return `the ${m.joint} is held to the owner's ${m.capPercent}% speed cap and stopped at ${m.actual ?? "?"}, short of ${m.ticks}`;
94 return `the ${m.joint} stopped at ${m.actual ?? "?"}, short of ${m.ticks}`;
95 }
96
4997 /**
5098 * In words, why the controls are off; null when they are on.
5199 * @param {{ queued?: boolean, turn: boolean, estop?: unknown, held?: unknown, reconnecting?: unknown }} s
Software/so101/src/ui/intent.test.ts +43 −1
@@ -1,5 +1,5 @@
11 import { describe, expect, test } from "bun:test";
2 import { Intent, whyNot } from "./intent.js";
2 import { ARRIVAL_SLACK_MS, arrivalMs, heldWhy, Intent, whyNot } from "./intent.js";
33
44 describe("Intent", () => {
55 test("a fake 30 Hz pose stream never moves a held slider", () => {
@@ -27,6 +27,48 @@ describe("Intent", () => {
2727 expect(i.held(3)).toBe(true);
2828 });
2929 test("an unknown joint shows the real pose", () => expect(new Intent().target(9, 1, 0)).toBeNull());
30
31 test("a release held for the predicted arrival: kept until then, a miss after it when the arm stopped short", () => {
32 const i = new Intent();
33 i.set(1, 3000, 0); i.release(1, 0, 700, 2600);
34 expect(i.target(1, 2900, 699)).toBe(3000);
35 expect(i.misses()).toEqual([]);
36 expect(i.target(1, 2900, 700)).toBe(3000); // gained 300 ticks since release: still on its way, held a little longer
37 expect(i.target(1, 2905, 700 + ARRIVAL_SLACK_MS)).toBeNull(); // then it stopped: missed, and said
38 expect(i.misses()).toEqual([{ id: 1, ticks: 3000, actual: 2905 }]);
39 expect(i.misses()).toEqual([]);
40 });
41
42 test("a target reached is not a miss, and clear forgets misses", () => {
43 const i = new Intent();
44 i.set(1, 3000, 0); i.release(1, 0, 500);
45 expect(i.target(1, 2995, 100)).toBeNull();
46 i.set(2, 3000, 0); i.release(2, 0, 100);
47 i.target(2, 2000, 200);
48 i.clear();
49 expect(i.misses()).toEqual([]);
50 });
51 });
52
53 describe("arrivalMs", () => {
54 test("the Governor's trapezoid plus slack: 40° at 90°/s and 360°/s² takes 0.69 s", () => {
55 const perDeg = 4096 / 360;
56 expect(arrivalMs(40 * perDeg, 90 * perDeg, 360 * perDeg)).toBe(694 + ARRIVAL_SLACK_MS);
57 expect(arrivalMs(-40 * perDeg, 90 * perDeg, 360 * perDeg)).toBe(694 + ARRIVAL_SLACK_MS);
58 expect(arrivalMs(10 * perDeg, 90 * perDeg, 360 * perDeg)).toBe(333 + ARRIVAL_SLACK_MS); // never reaches top speed
59 expect(arrivalMs(0, 1, 1)).toBe(ARRIVAL_SLACK_MS);
60 expect(arrivalMs(Number.NaN, 1, 1)).toBe(ARRIVAL_SLACK_MS);
61 });
62 });
63
64 describe("heldWhy", () => {
65 const m = { id: 6, joint: "gripper", ticks: 2500, actual: 2300 };
66 test("says what stood in the way: the zone first, then a jam, then the speed cap, else where it stopped", () => {
67 expect(heldWhy({ ...m, blocked: "gripper is 4 mm below the table (plus the 10 mm margin)" })).toBe("gripper is 4 mm below the table (plus the 10 mm margin)");
68 expect(heldWhy({ ...m, jammed: [6] })).toBe("the gripper is pushing against something and stopped at 2300");
69 expect(heldWhy({ ...m, capPercent: 40 })).toBe("the gripper is held to the owner's 40% speed cap and stopped at 2300, short of 2500");
70 expect(heldWhy({ ...m, capPercent: 100 })).toBe("the gripper stopped at 2300, short of 2500");
71 });
3072 });
3173
3274 describe("whyNot", () => {
Software/so101/src/viewer.html +47 −20
@@ -797,22 +797,28 @@
797797 }
798798 $("v3").onclick = () => pick("pane3");
799799
800 /** What stood in the way during the last gesture, for saying why a target was not reached. */
801 const inTheWay = { blocked: null, jammed: [] };
800802 // ---------------------------------------------------------------- the stream: the page as a leader
801 // Opened while a slider, a jog button or a model joint is held; frames go at most every 33 ms; closed shortly after letting go.
803 // Opened while a slider, a jog button or a model joint is held; frames go at most every 33 ms, and again every 250 ms
804 // while the press holds still (so the dead-man never parks a held target). Letting go sends a release, the last
805 // targets, at once: the arm server goes there and stops, and the socket closes shortly after.
802806 // Stamped targets (src/ui/frames.js): while the socket opens only the newest waits, and goes only if still fresh.
803807 const stream = {
804 ws: null, closeTimer: null, halted: false, waiting: null,
808 ws: null, closeTimer: null, halted: false, keeper: null,
809 // While a socket opens, only its newest frame waits on it (`ws.waiting`): a frame goes only if still fresh, a release always.
805810 frames: new Frames((data) => {
806811 const ws = stream.ws;
807812 if (ws?.readyState === 1) ws.send(data);
808 else if (ws?.readyState === 0) stream.waiting = { data, at: Date.now() };
813 else if (ws?.readyState === 0) ws.waiting = { data, at: Date.now(), release: data.startsWith('{"type":"release"') };
809814 }),
810815 /** STOP: drop what is pending, close, send nothing until a release or a resume. */
811816 halt() {
812817 clearTimeout(this.closeTimer);
818 clearInterval(this.keeper); this.keeper = null;
813819 this.frames.drop();
814 this.waiting = null;
815820 const ws = this.ws;
821 if (ws) ws.waiting = null;
816822 this.ws = null;
817823 this.halted = true;
818824 ws?.close();
@@ -820,14 +826,16 @@
820826 open() {
821827 if (!mayDrive()) return; // the server would refuse it: only the person whose turn it is streams
822828 clearTimeout(this.closeTimer);
823 if (this.ws && this.ws.readyState <= 1) return;
824829 if (this.halted) return; // the server ended this hold (Stop, or the arm is busy): nothing more until the next press
830 this.keeper ??= setInterval(() => this.ws?.readyState === 1 && this.frames.keep(), 250);
831 if (this.ws && this.ws.readyState <= 1) return;
832 inTheWay.blocked = null; inTheWay.jammed = [];
825833 const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`);
826 this.waiting = null;
827834 ws.onopen = () => {
828 const w = this.waiting;
829 this.waiting = null;
830 if (this.ws === ws && w && Date.now() - w.at <= 200) ws.send(w.data);
835 const w = ws.waiting;
836 ws.waiting = null;
837 if ((this.ws === ws || ws.leaving) && w && (w.release || Date.now() - w.at <= 200)) ws.send(w.data);
838 if (ws.leaving) setTimeout(() => ws.close(), 250); // a flick let go before the socket opened: its release went, now close
831839 };
832840 ws.onclose = (e) => {
833841 if (this.ws !== ws) return;
@@ -841,13 +849,17 @@
841849 this.open();
842850 if (this.ws) this.frames.send(frame);
843851 },
844 /** Now, not at the next slot, with anything pending: a finger lifted and this is where its joints stop. */
845 flush(frame) {
846 if (this.halted) return;
847 this.open();
848 if (this.ws) this.frames.flush(frame);
852 /** The finger lifted: the gesture's last targets go now as a release (nothing pending waits a slot), then the socket closes. */
853 end() {
854 clearInterval(this.keeper); this.keeper = null;
855 if (!this.halted && this.ws) this.frames.release(); else this.frames.drop();
856 clearTimeout(this.closeTimer);
857 this.closeTimer = setTimeout(() => {
858 const ws = this.ws; this.ws = null; this.halted = false;
859 if (ws?.readyState === 0 && ws.waiting?.release) ws.leaving = true; // still opening: close once the release is out
860 else ws?.close();
861 }, 250);
849862 },
850 end() { clearTimeout(this.closeTimer); this.closeTimer = setTimeout(() => { const ws = this.ws; this.ws = null; this.halted = false; ws?.close(); }, 250); },
851863 };
852864
853865 // ---------------------------------------------------------------- the arm
@@ -1199,7 +1211,7 @@
11991211 viewer.recenter();
12001212 });
12011213 viewer.addEventListener("manipulate-start", (e) => { dragging = e.detail; stream.open(); });
1202 viewer.addEventListener("manipulate-end", () => { const id = IDS[dragging]; if (id) intent.release(id, performance.now()); dragging = null; stream.end(); });
1214 viewer.addEventListener("manipulate-end", () => { const id = IDS[dragging]; if (id) letGo(id); dragging = null; stream.end(); });
12031215 viewer.addEventListener("angle-change", (e) => {
12041216 if (dragging !== e.detail || !viewer.robot) return;
12051217 const id = IDS[e.detail]; if (!id || !limits[id]) return;
@@ -1421,8 +1433,21 @@
14211433 const SHORT = { 1: "Base", 2: "Shoulder", 3: "Elbow", 4: "Wrist", 5: "Roll", 6: "Grip" };
14221434 let sliding = null;
14231435 // Lazy: the first view's budget is spent.
1424 let intent = { set() {}, release() {}, clear() {}, target: () => null }, whyNot = () => null, marks = () => {};
1425 const intentModule = import("/ui/__VERSION__/intent.js").then((m) => { intent = new m.Intent(); whyNot = m.whyNot; marks = m.marks; }, () => {});
1436 let intent = { set() {}, release() {}, clear() {}, target: () => null, misses: () => [] }, whyNot = () => null, marks = () => {}, arrivalMs = () => 2000, heldWhy = () => "";
1437 const intentModule = import("/ui/__VERSION__/intent.js").then((m) => { intent = new m.Intent(); whyNot = m.whyNot; marks = m.marks; arrivalMs = m.arrivalMs; heldWhy = m.heldWhy; }, () => {});
1438 /** Let go of a joint: its target is held for as long as the governor needs to get there (src/safety.ts), plus a little. */
1439 function letGo(id) {
1440 const at = status?.joints.find((j) => j.id === id)?.position, want = intent.targets?.get(id)?.ticks;
1441 const perDeg = 4096 / 360, far = typeof at === "number" && typeof want === "number" ? want - at : 0;
1442 intent.release(id, performance.now(), arrivalMs(far, (settings?.maxSpeedDegS ?? 90) * perDeg, (settings?.maxAccelDegS2 ?? 360) * perDeg), typeof at === "number" ? at : null);
1443 }
1444 /** A released target the arm did not reach: say why, never just snap back. */
1445 function sayMisses(d) {
1446 const m = intent.misses()[0];
1447 if (!m) return;
1448 msg(`held: ${heldWhy({ ...m, joint: d.joints.find((j) => j.id === m.id)?.joint ?? NAMES[m.id] ?? `joint ${m.id}`, blocked: inTheWay.blocked, jammed: [...inTheWay.jammed, ...(d.jammed ?? [])], capPercent: lobby.state?.maxSpeedPercent })}`, true);
1449 inTheWay.blocked = null; inTheWay.jammed = [];
1450 }
14261451 function render(d) {
14271452 for (const j of d.joints) {
14281453 let el = built.get(j.id);
@@ -1437,7 +1462,7 @@
14371462 range.setAttribute("aria-label", j.joint ?? `joint ${j.id}`);
14381463 range.addEventListener("pointerdown", () => { sliding = j.id; stream.open(); });
14391464 range.addEventListener("input", () => { if (sliding === j.id) { intent.set(j.id, Number(range.value), performance.now()); stream.send({ [j.id]: Number(range.value) }); el.pos.textContent = range.value; } });
1440 const done = () => { if (sliding === j.id) { sliding = null; intent.release(j.id, performance.now()); stream.end(); } };
1465 const done = () => { if (sliding === j.id) { sliding = null; letGo(j.id); stream.end(); } };
14411466 range.addEventListener("pointerup", done); range.addEventListener("pointercancel", done); range.addEventListener("change", done);
14421467 for (const b of div.querySelectorAll(".jog button")) jog(b, j.id, () => Number(b.dataset.d));
14431468 joints.append(div);
@@ -1517,7 +1542,9 @@
15171542 $("record").textContent = d.driving === "record" ? "■ Stop recording" : "● Record";
15181543 for (const id of ["hold", "releaseBtn", "play"]) $(id).disabled = d.driving !== null;
15191544 if (tab === "controls") render(d);
1520 banner(d); applyModel(d);
1545 if (d.blocked) inTheWay.blocked = sayViolation(d.blocked);
1546 for (const id of d.jammed ?? []) if (!inTheWay.jammed.includes(id)) inTheWay.jammed.push(id);
1547 banner(d); applyModel(d); sayMisses(d);
15211548 $("readout").innerHTML = `<span>lowest part <b>${Number.isFinite(d.lowestMm) ? d.lowestMm : "—"} mm</b> above the table</span><span>zone <b>${d.zone === "enforce" || d.zone === "warn" || d.zone === "off" ? d.zone : "—"}</b></span>`;
15221549 }
15231550 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs