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 #479
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/437-letting-go-means-go-there-a-release-land → main 2 commits fork
12 files
changed +245
−56
Software/so101/bench/precision.test.ts +2 −3
| @@ -11,12 +11,11 @@ test("a short script on the fake bus gives a scorecard with every headline numbe | ||
| 11 | 11 | { at: 0, joints: { 1: 2048 } }, |
| 12 | 12 | { at: 400, joints: { 1: 2348 } }, |
| 13 | 13 | { at: 900, joints: { 1: 2000 } }, // 348 ticks back: within maxStep |
| 14 | { at: 1000, joints: { 1: 2900 } }, // 900 ticks in one frame: dropped | |
| 14 | { at: 1000, joints: { 1: 2900 } }, // 900 ticks in one frame: followed, not dropped | |
| 15 | 15 | ]); |
| 16 | 16 | expect(card.trackingP95Deg).not.toBeNull(); |
| 17 | 17 | expect(card.arriveP95Ms).toBeGreaterThan(0); |
| 18 | 18 | expect(card.latencyP95Ms).toBeGreaterThan(0); |
| 19 | expect(card.snapBacks.byCause["maxStep-drop"]).toBe(1); | |
| 20 | expect(card.snapBacksPer100Releases).toBe(100); | |
| 19 | expect(card.snapBacks.byCause["maxStep-drop"] ?? 0).toBe(0); | |
| 21 | 20 | expect(render(card)).toContain("p95 tracking error"); |
| 22 | 21 | }, 10_000); |
Software/so101/src/command-gate.ts +9
| @@ -53,6 +53,15 @@ export function parseCommand(text: string): Command | null { | ||
| 53 | 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 }; |
| 54 | 54 | } |
| 55 | 55 | |
| 56 | /** Whether the frame says it is the last of a gesture: the target the arm is to land on before the stream closes. */ | |
| 57 | export function isRelease(text: string): boolean { | |
| 58 | try { | |
| 59 | return (JSON.parse(text) as { type?: unknown }).type === "release"; | |
| 60 | } catch { | |
| 61 | return false; | |
| 62 | } | |
| 63 | } | |
| 64 | ||
| 56 | 65 | /** One sender's clock, seen from here: when a frame it stamped would have arrived had nothing held it up. */ |
| 57 | 66 | export class Clock { |
| 58 | 67 | private offset: number | null = null; |
Software/so101/src/precision-history.test.ts +1 −1
| @@ -116,7 +116,7 @@ describe("GET /precision on the arm server", () => { | ||
| 116 | 116 | expect(row).toMatchObject({ commit: "feed123", sessions: 1 }); |
| 117 | 117 | const causes = after.window.snapBacks.byCause; |
| 118 | 118 | expect(causes.cap).toBe(1); |
| 119 | expect(causes["maxStep-drop"]).toBe(1); | |
| 119 | expect(causes["maxStep-drop"] ?? 0).toBe(0); // a jump is followed, not dropped | |
| 120 | 120 | expect(causes.jam).toBe(1); |
| 121 | 121 | expect(causes.STOP).toBe(1); |
| 122 | 122 | expect(after.window.releases).toBe(1); |
Software/so101/src/relay.ts +5 −4
| @@ -85,7 +85,7 @@ import { proxyFor, whyNot } from "./egress.ts"; | ||
| 85 | 85 | import { CalibrationError } from "./leader-map.js"; |
| 86 | 86 | import type { Lobby } from "./lobby.ts"; |
| 87 | 87 | import { capBody, FrameCap } from "./speed-cap.ts"; |
| 88 | import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts"; | |
| 88 | import { Clock, CommandGate, HELD_CLOSE, isRelease } from "./command-gate.ts"; | |
| 89 | 89 | import { FLOOD_CLOSE, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts"; |
| 90 | 90 | |
| 91 | 91 | export interface Remote { |
| @@ -868,7 +868,7 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 868 | 868 | const values = (text: string) => perServo(parseFrame(text) ?? []); |
| 869 | 869 | const rate = new FrameRate(); |
| 870 | 870 | /** A frame as it goes to the arm server: carrying the STOP epoch it was sent under. */ |
| 871 | const stamped = (frame: string) => JSON.stringify({ ...(JSON.parse(frame) as object), epoch }); | |
| 871 | const stamped = (frame: string, release: boolean) => JSON.stringify({ ...(JSON.parse(frame) as object), ...(release ? { type: "release" } : {}), epoch }); | |
| 872 | 872 | /** Set when this relay ends the stream for breaking the limits: the arm then holds, as for a stream closed on purpose. */ |
| 873 | 873 | let overLimit = false; |
| 874 | 874 | browser.on("message", (raw) => { |
| @@ -897,12 +897,13 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 897 | 897 | if (arm.readyState !== WebSocket.OPEN) return; |
| 898 | 898 | if (real && mode() !== "live") return void machine.note("clamp", "not passed on", { why: RECONNECTING }); |
| 899 | 899 | const data = JSON.stringify(Object.fromEntries(verdict.joints)); |
| 900 | const release = isRelease(String(raw)); | |
| 900 | 901 | const percent = lobby?.maxSpeedPercent ?? 100; |
| 901 | 902 | const text = String(data); |
| 902 | 903 | if (percent >= 100) { |
| 903 | 904 | if (lobby) cap.cap(text, percent); // keeps each joint's last target, so a cap set mid-stream starts from there |
| 904 | 905 | machine.note("sent", "follow frame", { values: values(text) }); |
| 905 | arm.send(stamped(data)); | |
| 906 | arm.send(stamped(data, release)); | |
| 906 | 907 | return; |
| 907 | 908 | } |
| 908 | 909 | if (!seeded) seed(); |
| @@ -910,7 +911,7 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 910 | 911 | if (capped !== text) machine.note("clamp", "speed cap", { percent, asked: values(text), sent: capped === null ? {} : values(capped) }); |
| 911 | 912 | if (capped === null) return; |
| 912 | 913 | machine.note("sent", "follow frame", { values: values(capped) }); |
| 913 | arm.send(stamped(capped)); | |
| 914 | arm.send(stamped(capped, release)); | |
| 914 | 915 | }); |
| 915 | 916 | arm.on("close", (code, why) => browser.close(sendable(code) ? code : 1011, why.length ? why.toString() : "the arm server went away")); |
| 916 | 917 | // A browser that closed the stream on purpose leaves the arm holding; one that vanished leaves it parking. |
Software/so101/src/release.test.ts added +107
| @@ -0,0 +1,107 @@ | ||
| 1 | import { afterEach, beforeEach, 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 { FeetechBus } from "./bus.ts"; | |
| 7 | import { FakeBus } from "./fake-bus.ts"; | |
| 8 | import { startServer, type RunningServer } from "./server.ts"; | |
| 9 | import { Frames } from "./ui/frames.js"; | |
| 10 | ||
| 11 | // Letting go means go there: the real Governor on a fake bus whose servos follow goals at 2000 ticks/s (faster than the 1024 the Governor allows). | |
| 12 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 13 | const START = 2048; | |
| 14 | const DEG40 = Math.round((40 * 4096) / 360); | |
| 15 | let fake: FakeBus; | |
| 16 | let server: RunningServer; | |
| 17 | ||
| 18 | beforeEach(async () => { | |
| 19 | fake = new FakeBus([1], { speedTicksS: 2000 }); | |
| 20 | fake.servos.get(1)!.set([0x00, 0x00, 0xff, 0x0f], 9); // limits 0-4095 | |
| 21 | server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1], posesFile: join(mkdtempSync(join(tmpdir(), "so101-release-")), "poses.json"), modelDir: "/nowhere" }); | |
| 22 | }); | |
| 23 | afterEach(() => Promise.race([server.close(), sleep(500)])); | |
| 24 | ||
| 25 | const open = async () => { | |
| 26 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); | |
| 27 | await new Promise((r) => ws.once("open", r)); | |
| 28 | return ws; | |
| 29 | }; | |
| 30 | const goals = (from = 0) => fake.writes.slice(from).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); | |
| 31 | const present = async () => ((await (await fetch(`http://127.0.0.1:${server.port}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints[0]!.position; | |
| 32 | /** The drag: 40° in 200 ms, in 20 frames. */ | |
| 33 | const drag = async (ws: WebSocket, to: number) => { | |
| 34 | for (let i = 1; i <= 20; i++) { | |
| 35 | ws.send(JSON.stringify({ 1: START + Math.round(((to - START) * i) / 20) })); | |
| 36 | await sleep(10); | |
| 37 | } | |
| 38 | }; | |
| 39 | ||
| 40 | describe("a release lands on the target", () => { | |
| 41 | test("a 40° drag released at once: the arm arrives, no goal is parked short, the goals only go toward the target", async () => { | |
| 42 | const ws = await open(); | |
| 43 | await sleep(100); // armed | |
| 44 | const target = START + DEG40; | |
| 45 | await drag(ws, target); | |
| 46 | const mark = fake.writes.length; | |
| 47 | ws.send(JSON.stringify({ 1: target, type: "release" })); | |
| 48 | ws.close(1000); | |
| 49 | await sleep(1500); | |
| 50 | const g = goals(mark); | |
| 51 | expect(Math.abs((await present()) - target)).toBeLessThanOrEqual(12); | |
| 52 | expect(Math.abs(g.at(-1)! - target)).toBeLessThanOrEqual(12); // the hold writes present, which is there | |
| 53 | for (let i = 1; i < g.length; i++) expect(g[i]!).toBeGreaterThanOrEqual(g[i - 1]! - 1); // never back toward where it was | |
| 54 | expect(g.filter((x) => x < target - 12).length).toBeGreaterThan(0); // it was still on its way when it let go | |
| 55 | }); | |
| 56 | ||
| 57 | test("a 90° flick in 100 ms is followed to the target, not dropped", async () => { | |
| 58 | const ws = await open(); | |
| 59 | await sleep(100); | |
| 60 | const target = START + 1024; | |
| 61 | ws.send(JSON.stringify({ 1: target - 512 })); | |
| 62 | await sleep(50); | |
| 63 | ws.send(JSON.stringify({ 1: target, type: "release" })); | |
| 64 | ws.close(1000); | |
| 65 | await sleep(2000); | |
| 66 | expect(Math.abs((await present()) - target)).toBeLessThanOrEqual(12); | |
| 67 | }); | |
| 68 | ||
| 69 | test("a lost link during the move parks at present", async () => { | |
| 70 | const ws = await open(); | |
| 71 | await sleep(100); | |
| 72 | const target = START + DEG40; | |
| 73 | await drag(ws, target); | |
| 74 | ws.terminate(); | |
| 75 | await sleep(800); | |
| 76 | expect(Math.abs((await present()) - target)).toBeGreaterThan(50); | |
| 77 | expect(goals().at(-1)).toBe((await present())); | |
| 78 | }); | |
| 79 | ||
| 80 | test("STOP during the move holds at once", async () => { | |
| 81 | const ws = await open(); | |
| 82 | await sleep(100); | |
| 83 | const target = START + DEG40; | |
| 84 | await drag(ws, target); | |
| 85 | ws.send(JSON.stringify({ 1: target, type: "release" })); | |
| 86 | ws.close(1000); | |
| 87 | await sleep(60); | |
| 88 | await fetch(`http://127.0.0.1:${server.port}/stop`, { method: "POST", body: JSON.stringify({ by: "test" }) }); | |
| 89 | await sleep(800); | |
| 90 | expect(Math.abs((await present()) - target)).toBeGreaterThan(50); | |
| 91 | expect(goals().at(-1)).toBe((await present())); | |
| 92 | }); | |
| 93 | ||
| 94 | test("a press held still for 3 s keeps the target: the page's beat keeps the dead-man quiet", async () => { | |
| 95 | const ws = await open(); | |
| 96 | const frames = new Frames((data) => ws.send(data)); | |
| 97 | const target = START + 300; | |
| 98 | frames.send({ 1: target }); | |
| 99 | await sleep(3000); | |
| 100 | const g = goals(); | |
| 101 | expect(g.at(-1)).toBe(target); | |
| 102 | expect(Math.abs((await present()) - target)).toBeLessThanOrEqual(12); | |
| 103 | frames.drop(); | |
| 104 | ws.close(1000); | |
| 105 | await sleep(100); | |
| 106 | }, 10_000); | |
| 107 | }); |
Software/so101/src/server.test.ts +4 −5
| @@ -887,7 +887,7 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 887 | 887 | await moved({ "1": mid(1), "6": mid(6) }); |
| 888 | 888 | }); |
| 889 | 889 | |
| 890 | test("a joint left far behind after a block is eased over once the leader has stayed away long enough", async () => { | |
| 890 | test("a joint left far behind after a block is followed to the leader along the governor's profile", async () => { | |
| 891 | 891 | const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`); |
| 892 | 892 | await new Promise((r) => ws.once("open", r)); |
| 893 | 893 | // Frames keep coming every 10 ms, as a leader's do, so the 60 ms watchdog of this server never turns this into a plain hold. |
| @@ -898,13 +898,12 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 898 | 898 | } |
| 899 | 899 | }; |
| 900 | 900 | await send(mid(2) + 200, 10); // safe, eases |
| 901 | await send(ticks(2, 80), 4); // dives at the table: dropped as a jump, or refused by the zone | |
| 902 | 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 | |
| 903 | await sleep(150); | |
| 901 | await send(ticks(2, 80), 4); // dives at the table: refused by the zone | |
| 902 | await send(mid(2) - 500, 140); // safe, but 700 ticks from where the joint was held: the governor's 90°/s takes it there | |
| 904 | 903 | const s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; blocked: unknown; stream: { dropped: number } }; |
| 905 | 904 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) - 500); |
| 906 | 905 | expect(s.blocked).toBeNull(); |
| 907 | expect(s.stream.dropped).toBeGreaterThanOrEqual(19); | |
| 906 | expect(s.stream.dropped).toBe(0); // far is followed, never dropped | |
| 908 | 907 | const closed = new Promise((r) => ws.once("close", r)); |
| 909 | 908 | ws.close(); |
| 910 | 909 | await closed; |
Software/so101/src/server.ts +34 −25
| @@ -72,7 +72,7 @@ import { fileURLToPath } from "node:url"; | ||
| 72 | 72 | import { ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts"; |
| 73 | 73 | import { WebSocketServer, type WebSocket } from "ws"; |
| 74 | 74 | import { browserModule } from "./browser-modules.ts"; |
| 75 | import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts"; | |
| 75 | import { Clock, CommandGate, HELD_CLOSE, isRelease } from "./command-gate.ts"; | |
| 76 | 76 | import { FLOOD_CLOSE, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts"; |
| 77 | 77 | import type { FeetechBus } from "./bus.ts"; |
| 78 | 78 | import { ArmModel, gripperTicksToRadians, ticksToRadians, type Vec3, type Violation } from "./kinematics.ts"; |
| @@ -101,8 +101,6 @@ export interface ServerOptions { | ||
| 101 | 101 | printablesDir?: string; |
| 102 | 102 | /** Torque cap while a stream drives the arm, percent. */ |
| 103 | 103 | streamTorque?: number; |
| 104 | /** Largest per-frame change accepted on the stream, ticks. */ | |
| 105 | maxStep?: number; | |
| 106 | 104 | /** With no frame for this long, the arm is told to hold where it is; overrides the holdAfterMs setting. */ |
| 107 | 105 | watchdogMs?: number; |
| 108 | 106 | /** How often a stream's client is pinged; one that misses a ping is a lost connection, and the arm parks. */ |
| @@ -181,8 +179,6 @@ const PRESENT = [STS3215.Present_Position, STS3215.Present_Load, STS3215.Present | ||
| 181 | 179 | const TORQUE_EVERY_MS = 500; |
| 182 | 180 | const JAM_LOAD = 45; |
| 183 | 181 | const JAM_WINDOW = 6; |
| 184 | /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */ | |
| 185 | const RESUME_AFTER_DROPS = 20; | |
| 186 | 182 | const VENDORED = new Set(["three", "urdf-loader"]); |
| 187 | 183 | /** A module the page loads from src/ui: plain JavaScript, never its tests. */ |
| 188 | 184 | const isUiModule = (name: string) => /^[a-z][a-z0-9-]*\.(js|css)$/.test(name); |
| @@ -197,7 +193,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 197 | 193 | const settingsFile = join(dataDir, "settings.json"); |
| 198 | 194 | const factoryFile = join(dataDir, "servo-factory.json"); |
| 199 | 195 | const streamTorque = options.streamTorque ?? 70; |
| 200 | const maxStep = options.maxStep ?? 400; | |
| 201 | 196 | const easeSpeed = options.easeSpeed ?? 300; |
| 202 | 197 | const jamSampleMs = options.jamSampleMs ?? 100; |
| 203 | 198 | const heartbeatMs = options.heartbeatMs ?? 1000; |
| @@ -375,7 +370,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 375 | 370 | machine.note("state", "emergency stop", { by }); |
| 376 | 371 | } |
| 377 | 372 | const was = driver; |
| 378 | 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 | |
| 373 | if (was === "stream") for (const client of [...halts.keys()]) stopCause.set(client, "STOP"), end(client, ESTOP_CLOSE, "emergency stop"); // its close handler holds, never parks | |
| 379 | 374 | else if (was === "record" && recording) { |
| 380 | 375 | const { done } = recording; |
| 381 | 376 | recording = null; // nothing is saved: the recording ends where it was cut |
| @@ -1015,7 +1010,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1015 | 1010 | gate.stopped(body.epoch); |
| 1016 | 1011 | if (body.hold !== false && gate.hold(actor(body))) metrics.event("hold", { by: actor(body) }); |
| 1017 | 1012 | const was = driver; |
| 1018 | 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 | |
| 1013 | if (was === "stream") for (const client of [...halts.keys()]) stopCause.set(client, body.hold === false ? "turn-end" : "STOP"), end(client, 1000, "stopped from the viewer"); // its close handler holds the arm | |
| 1019 | 1014 | else if (was === "record") { |
| 1020 | 1015 | const summary = await finishRecording(); |
| 1021 | 1016 | return json(res, 200, { stopped: was, ...summary }); |
| @@ -1174,6 +1169,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1174 | 1169 | const admission = gate.open(clock(req), false); |
| 1175 | 1170 | /** Set the moment the stream ends: whatever is queued or under way gives up, and nothing more reaches the bus. */ |
| 1176 | 1171 | let closed = false; |
| 1172 | /** The page said its last target is in: a clean close then lets the arm arrive before it holds. */ | |
| 1173 | let releasing = false; | |
| 1177 | 1174 | streamStats = stats; |
| 1178 | 1175 | const tracker = (tracking = precision.begin()); |
| 1179 | 1176 | /** Snap-backs are counted once per episode: a joint held at its limit, a zone refusal, a run of stale frames. */ |
| @@ -1193,8 +1190,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1193 | 1190 | const period = 1000 / settings.rateHz; |
| 1194 | 1191 | const governor = new Governor(bounds, speedLimits); |
| 1195 | 1192 | let last: Map<number, number> | null = null; |
| 1196 | /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */ | |
| 1197 | const drops = new Map<number, number>(); | |
| 1198 | 1193 | /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */ |
| 1199 | 1194 | let heldDepth = 0; |
| 1200 | 1195 | /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */ |
| @@ -1325,19 +1320,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1325 | 1320 | jams.delete(id); |
| 1326 | 1321 | windows.delete(id); |
| 1327 | 1322 | } |
| 1328 | const previous = last?.get(id); | |
| 1329 | if (previous === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped | |
| 1330 | else if (Math.abs(target - previous) > maxStep) { | |
| 1331 | stats.dropped++; | |
| 1332 | machine.note("clamp", "too far in one frame", { id, asked: target, from: previous, maxStep }); | |
| 1333 | const n = (drops.get(id) ?? 0) + 1; | |
| 1334 | drops.set(id, n); | |
| 1335 | if (n === 1) tracker.snapBack("maxStep-drop", Date.now(), id, target - previous); | |
| 1336 | if (n < RESUME_AFTER_DROPS) continue; // no hand moves that far in one frame | |
| 1337 | // The leader has stayed far away (after a block or a jam): ease over to it rather than leave the joint stranded. | |
| 1338 | drops.delete(id); | |
| 1339 | ease = true; | |
| 1340 | } else drops.delete(id); | |
| 1323 | // A flick is followed along the governor's profile, never dropped for being far: the limits are what keep it safe. | |
| 1324 | if (last?.get(id) === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped | |
| 1341 | 1325 | targets.push([id, target]); |
| 1342 | 1326 | } |
| 1343 | 1327 | if (!targets.length) return; |
| @@ -1453,6 +1437,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1453 | 1437 | } |
| 1454 | 1438 | staling = false; |
| 1455 | 1439 | const frame = verdict.joints; |
| 1440 | if (isRelease(String(data))) releasing = true; | |
| 1456 | 1441 | clearTimeout(timer); |
| 1457 | 1442 | timer = setTimeout(() => void hold(), holdAfterMs); |
| 1458 | 1443 | if (!arming) { |
| @@ -1473,10 +1458,34 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1473 | 1458 | }; |
| 1474 | 1459 | halts.set(ws, halt); |
| 1475 | 1460 | ws.on("close", (code) => { |
| 1461 | clearInterval(heartbeat); | |
| 1462 | // A clean close after a release lets the governor finish to the target; only then does the arm hold. A STOP | |
| 1463 | // (closed already), a lost link or an over-limit stream parks at present at once. | |
| 1464 | const settle = releasing && armed && !closed && CLEAN_CLOSE.has(code) && !overLimit.has(ws); | |
| 1465 | if (settle) clearTimeout(timer); | |
| 1466 | else halt(); | |
| 1467 | void (settle ? arrive() : Promise.resolve()).then(() => finish(code)); | |
| 1468 | }); | |
| 1469 | /** Wait for the governor to land every joint on its target: until it has, nothing is queued, or the time the rest of the way takes at the limits plus 500 ms. */ | |
| 1470 | const arrive = async () => { | |
| 1471 | const busy = () => pending || easing || ticking; | |
| 1472 | const wait = async (until: number, more: () => unknown) => { | |
| 1473 | while (!closed && Date.now() < until && more()) await new Promise((r) => setTimeout(r, period)); | |
| 1474 | }; | |
| 1475 | await wait(Date.now() + 5000, busy); // a frame waiting on the ease is followed, never dropped | |
| 1476 | await chain; // and the one just taken has reached the governor | |
| 1477 | await wait(Date.now() + 5000, busy); | |
| 1478 | const { speed, accel } = speedLimits(); | |
| 1479 | let deadline = Date.now() + 500 + 1000 * (speed / accel); | |
| 1480 | try { | |
| 1481 | deadline += (1000 * Math.abs(shortfall(await bus.syncRead(ids, STS3215.Present_Position))[1])) / speed; | |
| 1482 | } catch {} | |
| 1483 | await wait(deadline, () => busy() || governor.moving); | |
| 1476 | 1484 | halt(); |
| 1485 | }; | |
| 1486 | const finish = (code: number) => { | |
| 1477 | 1487 | halts.delete(ws); |
| 1478 | 1488 | admission.close(); |
| 1479 | clearInterval(heartbeat); | |
| 1480 | 1489 | const lost = !CLEAN_CLOSE.has(code) && code !== ESTOP_CLOSE && !overLimit.has(ws); |
| 1481 | 1490 | const cause: Cause | null = stopCause.get(ws) ?? (lost && armed ? "release-park" : null); |
| 1482 | 1491 | void run(async () => { |
| @@ -1502,7 +1511,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1502 | 1511 | } |
| 1503 | 1512 | if (lost && armed) void park(`the stream was lost (${code})`); |
| 1504 | 1513 | }); |
| 1505 | }); | |
| 1514 | }; | |
| 1506 | 1515 | }); |
| 1507 | 1516 | |
| 1508 | 1517 | // ------------------------------------------------------------------ the watch |
Software/so101/src/ui/frames.js +22 −3
| @@ -1,7 +1,8 @@ | ||
| 1 | 1 | // @ts-check |
| 2 | 2 | /** |
| 3 | 3 | * 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 (a finger let go); while a gesture goes on, the newest target is sent again every `keep` ms, so a pointer held still | |
| 5 | * is not taken for a silent one. Each is stamped with `seq` and `t`, when its newest target was given (src/command-gate.ts). | |
| 5 | 6 | * @typedef {Record<number, number>} Frame |
| 6 | 7 | */ |
| 7 | 8 | /** Tells the page something happened, for the health meter (src/ui/health-meter.js) to count. */ |
| @@ -10,7 +11,7 @@ const tell = (/** @type {string} */ what) => /** @type {any} */ (globalThis).dis | ||
| 10 | 11 | export class Frames { |
| 11 | 12 | /** |
| 12 | 13 | * @param {(data: string) => void} write |
| 13 | * @param {{ wait?: number, later?: (fn: () => void, ms: number) => unknown, cancel?: (handle: unknown) => void, now?: () => number }} [options] | |
| 14 | * @param {{ wait?: number, later?: (fn: () => void, ms: number) => unknown, cancel?: (handle: unknown) => void, now?: () => number, keep?: number }} [options] | |
| 14 | 15 | */ |
| 15 | 16 | constructor(write, options = {}) { |
| 16 | 17 | this.write = write; |
| @@ -18,6 +19,11 @@ export class Frames { | ||
| 18 | 19 | this.later = options.later ?? ((fn, ms) => setTimeout(fn, ms)); |
| 19 | 20 | this.cancel = options.cancel ?? ((h) => clearTimeout(/** @type {any} */ (h))); |
| 20 | 21 | this.now = options.now ?? (() => Date.now()); |
| 22 | this.keep = options.keep ?? 300; | |
| 23 | /** Every joint's newest target in this gesture, resent by the beat. @type {Frame} */ | |
| 24 | this.latest = {}; | |
| 25 | /** @type {unknown} */ | |
| 26 | this.beat = null; | |
| 21 | 27 | /** The last seq written. */ |
| 22 | 28 | this.seq = 0; |
| 23 | 29 | this.at = 0; |
| @@ -30,7 +36,9 @@ export class Frames { | ||
| 30 | 36 | /** @param {Frame} frame */ |
| 31 | 37 | send(frame) { |
| 32 | 38 | this.pending = { ...(this.pending ?? {}), ...frame }; |
| 39 | this.latest = { ...this.latest, ...frame }; | |
| 33 | 40 | this.at = this.now(); |
| 41 | this.beating(); | |
| 34 | 42 | if (this.timer !== null) return tell("cmd-drop"); // folded into the slot already waiting: the older target never goes out |
| 35 | 43 | this.timer = this.later(() => { |
| 36 | 44 | this.timer = null; |
| @@ -46,6 +54,16 @@ export class Frames { | ||
| 46 | 54 | this.stamped(data, this.now()); |
| 47 | 55 | } |
| 48 | 56 | |
| 57 | /** Keep the beat going while a gesture does: the target again when nothing newer went out for `keep` ms. */ | |
| 58 | beating() { | |
| 59 | if (this.beat !== null) return; | |
| 60 | this.beat = this.later(() => { | |
| 61 | this.beat = null; | |
| 62 | if (this.timer === null && this.now() - this.at >= this.keep) this.stamped({ ...this.latest }, (this.at = this.now())); | |
| 63 | this.beating(); | |
| 64 | }, this.keep); | |
| 65 | } | |
| 66 | ||
| 49 | 67 | /** @param {Frame} data @param {number} t */ |
| 50 | 68 | stamped(data, t) { |
| 51 | 69 | this.write(JSON.stringify({ ...data, seq: ++this.seq, t })); |
| @@ -54,6 +72,7 @@ export class Frames { | ||
| 54 | 72 | /** Forget what is pending: the stream ended. */ |
| 55 | 73 | drop() { |
| 56 | 74 | if (this.timer !== null) this.cancel(this.timer); |
| 57 | this.timer = null; this.pending = null; | |
| 75 | if (this.beat !== null) this.cancel(this.beat); | |
| 76 | this.timer = null; this.pending = null; this.beat = null; this.latest = {}; | |
| 58 | 77 | } |
| 59 | 78 | } |
Software/so101/src/ui/frames.test.ts +20 −2
| @@ -6,6 +6,7 @@ function rig() { | ||
| 6 | 6 | const wire: Array<Record<string, number>> = []; |
| 7 | 7 | const stamps: Array<{ seq: number; t: number }> = []; |
| 8 | 8 | let due: (() => void) | null = null; |
| 9 | let beatDue: (() => void) | null = null; | |
| 9 | 10 | let clock = 1000; |
| 10 | 11 | const frames = new Frames( |
| 11 | 12 | (data) => { |
| @@ -13,10 +14,11 @@ function rig() { | ||
| 13 | 14 | stamps.push({ seq, t }); |
| 14 | 15 | wire.push(joints); |
| 15 | 16 | }, |
| 16 | { later: (fn) => ((due = fn), 1), cancel: () => (due = null), now: () => clock }, | |
| 17 | { later: (fn, ms) => (ms === 33 ? ((due = fn), 1) : ((beatDue = fn), 2)), cancel: (h) => (h === 1 ? (due = null) : (beatDue = null)), now: () => clock }, | |
| 17 | 18 | ); |
| 18 | 19 | const slot = () => { const fn = due; due = null; fn?.(); }; |
| 19 | return { frames, wire, stamps, slot, waiting: () => due !== null, tick: (ms: number) => (clock += ms) }; | |
| 20 | const beat = () => { const fn = beatDue; beatDue = null; fn?.(); }; | |
| 21 | return { frames, wire, stamps, slot, beat, waiting: () => due !== null, tick: (ms: number) => (clock += ms) }; | |
| 20 | 22 | } |
| 21 | 23 | |
| 22 | 24 | describe("Frames", () => { |
| @@ -66,6 +68,22 @@ describe("Frames", () => { | ||
| 66 | 68 | expect(wire).toEqual([{ 6: 2500 }]); |
| 67 | 69 | }); |
| 68 | 70 | |
| 71 | test("a pointer held still keeps sending its target, and a flush ends the beat", () => { | |
| 72 | const { frames, wire, slot, beat, tick } = rig(); | |
| 73 | frames.send({ 1: 2000 }); | |
| 74 | slot(); | |
| 75 | tick(300); | |
| 76 | beat(); | |
| 77 | tick(300); | |
| 78 | beat(); | |
| 79 | expect(wire).toEqual([{ 1: 2000 }, { 1: 2000 }, { 1: 2000 }]); | |
| 80 | frames.flush({ 1: 2100 }); | |
| 81 | const n = wire.length; | |
| 82 | tick(300); | |
| 83 | beat(); | |
| 84 | expect(wire.length).toBe(n); | |
| 85 | }); | |
| 86 | ||
| 69 | 87 | test("drop forgets the pending frame", () => { |
| 70 | 88 | const { frames, wire, slot } = rig(); |
| 71 | 89 | frames.send({ 1: 2000 }); |
Software/so101/src/ui/intent.js +16 −5
| @@ -9,7 +9,7 @@ export class Intent { | ||
| 9 | 9 | constructor(options = {}) { |
| 10 | 10 | this.hold = options.hold ?? 2000; |
| 11 | 11 | this.near = options.near ?? 12; |
| 12 | /** @type {Map<number, { ticks: number, down: boolean, since: number }>} */ | |
| 12 | /** @type {Map<number, { ticks: number, down: boolean, since: number, hold?: number }>} */ | |
| 13 | 13 | this.targets = new Map(); |
| 14 | 14 | /** Targets that were let go unreached (they waited out `hold`, or were cleared): the page's snap-backs, until `takeLost`. @type {Array<{ id: number, ticks: number }>} */ |
| 15 | 15 | this.lost = []; |
| @@ -20,10 +20,10 @@ export class Intent { | ||
| 20 | 20 | if (Number.isFinite(ticks)) this.targets.set(id, { ticks, down: true, since: now }); |
| 21 | 21 | } |
| 22 | 22 | |
| 23 | /** The pointer lifted: the target stays, timed from now. @param {number} id @param {number} now */ | |
| 24 | release(id, now) { | |
| 23 | /** The pointer lifted: the target stays, timed from now, for `hold` ms when the motion says how long it takes. @param {number} id @param {number} now @param {number} [hold] */ | |
| 24 | release(id, now, hold) { | |
| 25 | 25 | const t = this.targets.get(id); |
| 26 | if (t) { t.down = false; t.since = now; } | |
| 26 | if (t) { t.down = false; t.since = now; t.hold = hold; } | |
| 27 | 27 | } |
| 28 | 28 | |
| 29 | 29 | /** Drop everything (stopped, turn over); a released target the arm had not reached is lost. */ |
| @@ -50,7 +50,7 @@ export class Intent { | ||
| 50 | 50 | const t = this.targets.get(id); |
| 51 | 51 | if (!t) return null; |
| 52 | 52 | const arrived = Number.isFinite(actual) && Math.abs(/** @type {number} */ (actual) - t.ticks) <= this.near; |
| 53 | if (!t.down && (now - t.since >= this.hold || arrived)) { | |
| 53 | if (!t.down && (now - t.since >= (t.hold ?? this.hold) || arrived)) { | |
| 54 | 54 | this.targets.delete(id); |
| 55 | 55 | if (!arrived) this.(id, t.ticks); |
| 56 | 56 | return null; |
| @@ -62,6 +62,17 @@ export class Intent { | ||
| 62 | 62 | held(id) { return this.targets.get(id)?.down === true; } |
| 63 | 63 | } |
| 64 | 64 | |
| 65 | /** | |
| 66 | * How long a released target is kept: the time the governor needs to cover `ticks` (accelerate, cruise, brake) plus 300 ms. | |
| 67 | * @param {number} ticks @param {number} speedDegS @param {number} accelDegS2 | |
| 68 | */ | |
| 69 | export function settleMs(ticks, speedDegS, accelDegS2) { | |
| 70 | const deg = (Math.abs(ticks) * 360) / 4096; | |
| 71 | const ramp = speedDegS ** 2 / accelDegS2; // degrees spent getting to speed and back down | |
| 72 | const s = deg >= ramp ? deg / speedDegS + speedDegS / accelDegS2 : 2 * Math.sqrt(deg / accelDegS2); | |
| 73 | return Math.round(s * 1000) + 300; | |
| 74 | } | |
| 75 | ||
| 65 | 76 | /** |
| 66 | 77 | * In words, why the controls are off; null when they are on. |
| 67 | 78 | * @param {{ queued?: boolean, turn: boolean, estop?: unknown, held?: unknown, reconnecting?: unknown }} s |
Software/so101/src/ui/intent.test.ts +11 −1
| @@ -1,5 +1,5 @@ | ||
| 1 | 1 | import { describe, expect, test } from "bun:test"; |
| 2 | import { Intent, whyNot } from "./intent.js"; | |
| 2 | import { Intent, settleMs, whyNot } from "./intent.js"; | |
| 3 | 3 | |
| 4 | 4 | describe("Intent", () => { |
| 5 | 5 | test("a fake 30 Hz pose stream never moves a held slider", () => { |
| @@ -56,3 +56,13 @@ describe("marks", () => { | ||
| 56 | 56 | expect(row.mark.hidden).toBe(true); |
| 57 | 57 | }); |
| 58 | 58 | }); |
| 59 | ||
| 60 | describe("a hold that follows the motion", () => { | |
| 61 | test("a release keeps the target for the time the governor needs, plus 300 ms, not a fixed 2000", () => { | |
| 62 | const i = new Intent(); | |
| 63 | i.set(1, 3000, 0); i.release(1, 0, settleMs(455, 90, 360)); | |
| 64 | expect(settleMs(455, 90, 360)).toBe(994); // 40° at 90°/s and 360°/s²: 694 ms of motion, plus 300 | |
| 65 | expect(i.target(1, 2000, 900)).toBe(3000); | |
| 66 | expect(i.target(1, 2000, 1000)).toBeNull(); | |
| 67 | }); | |
| 68 | }); |
Software/so101/src/viewer.html +14 −7
| @@ -850,6 +850,11 @@ | ||
| 850 | 850 | this.open(); |
| 851 | 851 | if (this.ws) this.frames.flush(frame); |
| 852 | 852 | }, |
| 853 | /** Letting go: the last target goes out now, marked as the release, and the arm lands on it before it holds. */ | |
| 854 | release(frame) { | |
| 855 | if (frame) this.flush({ ...frame, type: "release" }); | |
| 856 | this.end(); | |
| 857 | }, | |
| 853 | 858 | end() { clearTimeout(this.closeTimer); this.closeTimer = setTimeout(() => { const ws = this.ws; this.ws = null; this.halted = false; ws?.close(); }, 250); }, |
| 854 | 859 | }; |
| 855 | 860 | |
| @@ -861,6 +866,8 @@ | ||
| 861 | 866 | // minimum is the jaw's zero, open at the maximum is the URDF's upper limit. Trims are added in degrees. |
| 862 | 867 | const trim = (id) => (((settings?.jointOffsetsDeg ?? {})[NAMES[id]] ?? 0) * Math.PI) / 180; |
| 863 | 868 | const gripperOpen = () => modelInfo?.joints?.find((j) => j.name === "gripper")?.upper ?? 1.745; |
| 869 | /** How long the page keeps a released target: as long as the arm needs to get there at the limits, plus a beat. */ | |
| 870 | const holdFor = (id, want) => { const at = status?.joints.find((x) => x.id === id)?.position; return at == null || want == null ? undefined : settleMs(want - at, settings?.maxSpeedDegS ?? 90, settings?.maxAccelDegS2 ?? 360); }; | |
| 864 | 871 | const toRad = (id, ticks) => (id === 6 ? ((ticks - limits[id].min) / (limits[id].max - limits[id].min)) * gripperOpen() : ((ticks - mid(id)) * 2 * Math.PI) / 4095) + trim(id); |
| 865 | 872 | const toTicks = (id, rad) => Math.round(id === 6 ? limits[id].min + ((rad - trim(id)) / gripperOpen()) * (limits[id].max - limits[id].min) : ((rad - trim(id)) * 4095) / (2 * Math.PI) + mid(id)); |
| 866 | 873 | const clampTicks = (id, t) => Math.max(limits[id].min + 30, Math.min(limits[id].max - 30, Math.round(t))); |
| @@ -937,7 +944,7 @@ | ||
| 937 | 944 | }); |
| 938 | 945 | |
| 939 | 946 | /** Hold to repeat: `tick` runs every 50 ms while the finger (or the mouse) is down, `begin` once first; false from `begin` refuses. */ |
| 940 | function holdRepeat(b, begin, tick) { | |
| 947 | function holdRepeat(b, begin, tick, final) { | |
| 941 | 948 | let timer = null; |
| 942 | 949 | const start = (e) => { |
| 943 | 950 | e.preventDefault(); |
| @@ -947,7 +954,7 @@ | ||
| 947 | 954 | stream.open(); |
| 948 | 955 | timer = setInterval(tick, 50); |
| 949 | 956 | }; |
| 950 | const end = () => { if (timer) { clearInterval(timer); timer = null; stream.end(); } }; | |
| 957 | const end = () => { if (timer) { clearInterval(timer); timer = null; stream.release(final?.()); } }; | |
| 951 | 958 | for (const ev of ["pointerup", "pointercancel", "lostpointercapture", "pointerleave", "blur"]) b.addEventListener(ev, end); |
| 952 | 959 | addEventListener("blur", end); |
| 953 | 960 | b.addEventListener("pointerdown", start); |
| @@ -960,7 +967,7 @@ | ||
| 960 | 967 | const joint = status?.joints.find((x) => x.id === id); |
| 961 | 968 | if (!joint || joint.position == null || !limits[id]) { msg("no position for that joint", true); return false; } |
| 962 | 969 | target = joint.position; |
| 963 | }, () => { target = clampTicks(id, target + (direction() * Number($("rate").value)) / 20); stream.send({ [id]: target }); }); | |
| 970 | }, () => { target = clampTicks(id, target + (direction() * Number($("rate").value)) / 20); stream.send({ [id]: target }); }, () => (target === null ? null : { [id]: target })); | |
| 964 | 971 | } |
| 965 | 972 | |
| 966 | 973 | // ---------------------------------------------------------------- recordings |
| @@ -1203,7 +1210,7 @@ | ||
| 1203 | 1210 | viewer.recenter(); |
| 1204 | 1211 | }); |
| 1205 | 1212 | viewer.addEventListener("manipulate-start", (e) => { dragging = e.detail; stream.open(); }); |
| 1206 | viewer.addEventListener("manipulate-end", () => { const id = IDS[dragging]; if (id) intent.release(id, performance.now()); dragging = null; stream.end(); }); | |
| 1213 | viewer.addEventListener("manipulate-end", () => { const id = IDS[dragging]; const want = id ? intent.target(id, null, performance.now()) : null; if (id) intent.release(id, performance.now(), holdFor(id, want)); dragging = null; stream.release(want === null ? null : { [id]: want }); }); | |
| 1207 | 1214 | viewer.addEventListener("angle-change", (e) => { |
| 1208 | 1215 | if (dragging !== e.detail || !viewer.robot) return; |
| 1209 | 1216 | const id = IDS[e.detail]; if (!id || !limits[id]) return; |
| @@ -1467,8 +1474,8 @@ | ||
| 1467 | 1474 | const SHORT = { 1: "Base", 2: "Shoulder", 3: "Elbow", 4: "Wrist", 5: "Roll", 6: "Grip" }; |
| 1468 | 1475 | let sliding = null; |
| 1469 | 1476 | // Lazy: the first view's budget is spent. |
| 1470 | let intent = { set() {}, release() {}, clear() {}, target: () => null }, whyNot = () => null, marks = () => {}; | |
| 1471 | const intentModule = import("/ui/__VERSION__/intent.js").then((m) => { intent = new m.Intent(); whyNot = m.whyNot; marks = m.marks; }, () => {}); | |
| 1477 | let intent = { set() {}, release() {}, clear() {}, target: () => null }, whyNot = () => null, marks = () => {}, settleMs = () => undefined; | |
| 1478 | const intentModule = import("/ui/__VERSION__/intent.js").then((m) => { intent = new m.Intent(); whyNot = m.whyNot; marks = m.marks; settleMs = m.settleMs; }, () => {}); | |
| 1472 | 1479 | function render(d) { |
| 1473 | 1480 | for (const j of d.joints) { |
| 1474 | 1481 | let el = built.get(j.id); |
| @@ -1483,7 +1490,7 @@ | ||
| 1483 | 1490 | range.setAttribute("aria-label", j.joint ?? `joint ${j.id}`); |
| 1484 | 1491 | range.addEventListener("pointerdown", () => { sliding = j.id; stream.open(); }); |
| 1485 | 1492 | range.addEventListener("input", () => { if (sliding === j.id) { command(j.id, Number(range.value)); stream.send({ [j.id]: Number(range.value) }); el.pos.textContent = range.value; } }); |
| 1486 | const done = () => { if (sliding === j.id) { sliding = null; intent.release(j.id, performance.now()); stream.end(); } }; | |
| 1493 | const done = () => { if (sliding === j.id) { sliding = null; intent.release(j.id, performance.now(), holdFor(j.id, Number(range.value))); stream.release({ [j.id]: Number(range.value) }); } }; | |
| 1487 | 1494 | range.addEventListener("pointerup", done); range.addEventListener("pointercancel", done); range.addEventListener("change", done); |
| 1488 | 1495 | for (const b of div.querySelectorAll(".jog button")) jog(b, j.id, () => Number(b.dataset.d)); |
| 1489 | 1496 | joints.append(div); |
new above
offline