Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Motion: a release lands on target, no slow first-frame ease, a late final frame is never dropped (flick: 42 ticks short and never arriving → 10, arriving in 1.2 s) #503
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
hunt/motion → main 2 commits
11 files
changed +656
−94
Software/so101/bench/motion.test.ts added +47
| @@ -0,0 +1,47 @@ | ||
| 1 | import { describe, expect, test } from "bun:test"; | |
| 2 | import { runGesture } from "./motion.ts"; | |
| 3 | ||
| 4 | const drag = (from: number, to: number, ms: number, startAt = 0) => { | |
| 5 | const n = Math.max(1, Math.round(ms / 16.7)); | |
| 6 | return Array.from({ length: n + 1 }, (_, i) => [Math.round(startAt + (ms * i) / n), Math.round(from + ((to - from) * i) / n)] as const); | |
| 7 | }; | |
| 8 | ||
| 9 | describe("the arm follows the hand, end to end (page -> relay -> arm server -> servos that move)", () => { | |
| 10 | test("letting go mid-motion: the arm finishes the move to where the hand let go, not where it was 250 ms later", async () => { | |
| 11 | // 600 ticks in 300 ms is faster than the governor's 90 deg/s, so the arm is still on its way when the page closes the stream. | |
| 12 | const row = await runGesture({ name: "release", joint: 1, from: 2048, path: drag(2048, 2648, 300) }); | |
| 13 | expect(Math.abs(row.finalErr)).toBeLessThanOrEqual(12); | |
| 14 | expect(row.arriveMs).not.toBeNull(); | |
| 15 | expect(row.overshoot).toBeLessThanOrEqual(12); | |
| 16 | expect(row.modelSnapBacks).toBe(0); | |
| 17 | }, 15_000); | |
| 18 | ||
| 19 | test("STOP mid-motion is strict: nothing moves after it until resume and a new command", async () => { | |
| 20 | const row = await runGesture({ name: "stop", joint: 1, from: 2048, path: drag(2048, 3048, 600), stopAt: 300 }); | |
| 21 | expect(row.movedAfterStop).toBeLessThanOrEqual(25); // the servo's own braking, no more goals | |
| 22 | expect(row.movedAfterResume).toBe(true); | |
| 23 | }, 15_000); | |
| 24 | }); | |
| 25 | ||
| 26 | describe("fast flicks are followed along the governor's profile, never dropped", () => { | |
| 27 | test("a 90 degree flick in 100 ms: no frame dropped for jumping far, and the arm gets there", async () => { | |
| 28 | // A hand starts slowly (the first frame is eased over) and then flicks 1000 ticks in 100 ms: 400+ ticks a frame. | |
| 29 | const row = await runGesture({ name: "flick", joint: 1, from: 2048, path: [...drag(2048, 2058, 150), ...drag(2058, 3072, 100, 200)] }); | |
| 30 | expect(row.droppedMaxStep).toBe(0); | |
| 31 | expect(Math.abs(row.finalErr)).toBeLessThanOrEqual(12); | |
| 32 | expect(row.serverSnapBacks).not.toContain("maxStep-drop"); | |
| 33 | }, 15_000); | |
| 34 | }); | |
| 35 | ||
| 36 | describe("no slow ease in the middle of a gesture", () => { | |
| 37 | test("a hard flick from rest and a drag, pause, drag are followed at the governor's speed from the first frame", async () => { | |
| 38 | // The first frame used to take a 300 ticks/s ease (and later frames waited for it): 1000 ticks then took 3+ s and the | |
| 39 | // page had closed the stream by then. At 90 deg/s (1024 ticks/s) with the governor's acceleration it is under 1.5 s. | |
| 40 | const flick = await runGesture({ name: "flick from rest", joint: 1, from: 2048, path: drag(2048, 3048, 100) }); | |
| 41 | expect(Math.abs(flick.finalErr)).toBeLessThanOrEqual(12); | |
| 42 | expect(flick.arriveMs!).toBeLessThan(1600); | |
| 43 | const resumed = await runGesture({ name: "pause", joint: 1, from: 2048, path: [...drag(2048, 2348, 400), ...drag(2348, 2948, 400, 1900)] }); | |
| 44 | expect(Math.abs(resumed.finalErr)).toBeLessThanOrEqual(12); | |
| 45 | expect(resumed.arriveMs!).toBeLessThan(1600); | |
| 46 | }, 20_000); | |
| 47 | }); |
Software/so101/bench/motion.ts added +389
| @@ -0,0 +1,389 @@ | ||
| 1 | /** | |
| 2 | * The end-to-end motion bench: a page-like client -> the relay (as the hosted app runs it, a local lobby in front) -> | |
| 3 | * the arm server -> a fake bus whose servos MOVE (a trapezoid at the Goal_Velocity / Acceleration registers, 1 ms of | |
| 4 | * bus latency; src/fake-bus.ts `dynamics`). It scripts gestures and scores how the arm follows the hand. | |
| 5 | * bun bench/motion.ts the scorecard, as text | |
| 6 | * bun bench/motion.ts --json the rows | |
| 7 | * | |
| 8 | * The page side is the real one where it can be: ui/frames.js (the 33 ms coalescer, seq and t stamps) and ui/intent.js | |
| 9 | * (what the person asked for, kept apart from where the arm is); the pointer/slider handlers mirror viewer.html: every | |
| 10 | * move is `send`, a release is `intent.release` and, 250 ms later, the stream closes. | |
| 11 | * | |
| 12 | * Per gesture: | |
| 13 | * latency first frame sent -> the servo has moved 3 ticks | |
| 14 | * arrive the last frame sent (the release) -> the servo is, and stays, within 12 ticks of the final target | |
| 15 | * final position error long after everything is over (ticks), and the overshoot past the final target | |
| 16 | * dropped frames the arm server gave up on, by reason (maxStep, gate = stale / replayed / ...) | |
| 17 | * snap-backs the page's intent let go unreached (the model jumping back), and the server's own, by cause | |
| 18 | */ | |
| 19 | import { mkdtempSync } from "node:fs"; | |
| 20 | import { tmpdir } from "node:os"; | |
| 21 | import { join } from "node:path"; | |
| 22 | import WebSocket from "ws"; | |
| 23 | import { Accounts } from "../src/accounts.ts"; | |
| 24 | import { FeetechBus } from "../src/bus.ts"; | |
| 25 | import { FakeBus } from "../src/fake-bus.ts"; | |
| 26 | import { Lobby } from "../src/lobby.ts"; | |
| 27 | import { Moderator } from "../src/moderator.ts"; | |
| 28 | import { startRelay } from "../src/relay.ts"; | |
| 29 | import { startServer } from "../src/server.ts"; | |
| 30 | import { Frames } from "../src/ui/frames.js"; | |
| 31 | import { Intent } from "../src/ui/intent.js"; | |
| 32 | ||
| 33 | const RANGES: Record<number, [number, number]> = { | |
| 34 | 1: [818, 3487], | |
| 35 | 2: [813, 3202], | |
| 36 | 3: [900, 3100], | |
| 37 | 4: [700, 3300], | |
| 38 | 5: [600, 3400], | |
| 39 | 6: [1995, 3525], | |
| 40 | }; | |
| 41 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 42 | const ARRIVE = 12; | |
| 43 | ||
| 44 | /** The hand: what the pointer does on one joint, as [ms from the start, ticks] and the moment it lets go. */ | |
| 45 | export interface Gesture { | |
| 46 | name: string; | |
| 47 | joint: number; | |
| 48 | /** Where the arm starts, ticks. */ | |
| 49 | from: number; | |
| 50 | /** Pointer positions; the last is the release. A gap in `at` is a pause. */ | |
| 51 | path: Array<readonly [number, number]>; | |
| 52 | /** STOP is pressed this many ms in; then the page keeps sending as a bug-tolerant client would. */ | |
| 53 | stopAt?: number; | |
| 54 | /** Pressing the arm back to life: resume, then one new command, this many ms after STOP. */ | |
| 55 | resumeAfter?: number; | |
| 56 | } | |
| 57 | ||
| 58 | /** A straight drag of `to - from` ticks over `ms`, pointer events at 60 Hz. */ | |
| 59 | function drag( | |
| 60 | from: number, | |
| 61 | to: number, | |
| 62 | ms: number, | |
| 63 | startAt = 0, | |
| 64 | ): Array<readonly [number, number]> { | |
| 65 | const n = Math.max(1, Math.round(ms / 16.7)); | |
| 66 | return Array.from( | |
| 67 | { length: n + 1 }, | |
| 68 | (_, i) => | |
| 69 | [ | |
| 70 | Math.round(startAt + (ms * i) / n), | |
| 71 | Math.round(from + ((to - from) * i) / n), | |
| 72 | ] as const, | |
| 73 | ); | |
| 74 | } | |
| 75 | ||
| 76 | export const GESTURES: Gesture[] = [ | |
| 77 | { | |
| 78 | name: "slow drag 250t/1.5s", | |
| 79 | joint: 1, | |
| 80 | from: 2048, | |
| 81 | path: drag(2048, 2298, 1500), | |
| 82 | }, | |
| 83 | { | |
| 84 | name: "fast 90deg flick/100ms", | |
| 85 | joint: 1, | |
| 86 | from: 2048, | |
| 87 | path: drag(2048, 3072, 100), | |
| 88 | }, | |
| 89 | { | |
| 90 | name: "drag, pause 1.5s, drag", | |
| 91 | joint: 1, | |
| 92 | from: 2048, | |
| 93 | path: [...drag(2048, 2348, 400), ...drag(2348, 2648, 400, 1900)], | |
| 94 | }, | |
| 95 | { | |
| 96 | name: "release mid-motion", | |
| 97 | joint: 1, | |
| 98 | from: 2048, | |
| 99 | path: drag(2048, 2848, 300), | |
| 100 | }, | |
| 101 | { | |
| 102 | name: "STOP mid-motion", | |
| 103 | joint: 1, | |
| 104 | from: 2048, | |
| 105 | path: drag(2048, 3048, 600), | |
| 106 | stopAt: 300, | |
| 107 | }, | |
| 108 | ]; | |
| 109 | ||
| 110 | export interface Row { | |
| 111 | gesture: string; | |
| 112 | latencyMs: number | null; | |
| 113 | arriveMs: number | null; | |
| 114 | /** From the release to the arm being within 12 ticks for good, or null if it never was. */ | |
| 115 | finalErr: number; | |
| 116 | overshoot: number; | |
| 117 | droppedMaxStep: number; | |
| 118 | droppedGate: number; | |
| 119 | modelSnapBacks: number; | |
| 120 | serverSnapBacks: string; | |
| 121 | /** STOP only: how far the servo went after STOP was answered, ticks; and whether a new command after resume moved it. */ | |
| 122 | movedAfterStop?: number; | |
| 123 | movedAfterResume?: boolean; | |
| 124 | } | |
| 125 | ||
| 126 | interface Rig { | |
| 127 | base: string; | |
| 128 | relayPort: number; | |
| 129 | fake: FakeBus; | |
| 130 | close: () => Promise<void>; | |
| 131 | } | |
| 132 | ||
| 133 | async function rig(): Promise<Rig> { | |
| 134 | const fake = new FakeBus([1, 2, 3, 4, 5, 6], { dynamics: { latencyMs: 1 } }); | |
| 135 | for (const [id, [min, max]] of Object.entries(RANGES)) | |
| 136 | fake.servos | |
| 137 | .get(Number(id))! | |
| 138 | .set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9); | |
| 139 | const dir = mkdtempSync(join(tmpdir(), "so101-motion-")); | |
| 140 | const simulation = await startServer(new FeetechBus(fake), { | |
| 141 | host: "127.0.0.1", | |
| 142 | port: 0, | |
| 143 | posesFile: join(dir, "poses.json"), | |
| 144 | modelDir: "/nowhere", | |
| 145 | commit: "bench", | |
| 146 | }); | |
| 147 | const lobby = new Lobby({ | |
| 148 | local: true, | |
| 149 | chatFile: join(dir, "chat.json"), | |
| 150 | adminFile: join(dir, "admin.json"), | |
| 151 | countsFile: join(dir, "visitors.json"), | |
| 152 | turnsFile: join(dir, "turns.json"), | |
| 153 | wordsFile: join(dir, "words.json"), | |
| 154 | reportsFile: join(dir, "reports.json"), | |
| 155 | accounts: new Accounts({ file: join(dir, "accounts.sqlite") }), | |
| 156 | moderator: new Moderator({ file: join(dir, "crew.json"), model: null }), | |
| 157 | }); | |
| 158 | const relay = await startRelay({ | |
| 159 | host: "127.0.0.1", | |
| 160 | port: 0, | |
| 161 | simulation: `http://127.0.0.1:${simulation.port}`, | |
| 162 | remote: null, | |
| 163 | problem: null, | |
| 164 | lobby, | |
| 165 | summaryEveryMs: 60_000, | |
| 166 | holdFile: join(dir, "hold.json"), | |
| 167 | baselineFile: join(dir, "servo-factory.json"), | |
| 168 | }); | |
| 169 | return { | |
| 170 | base: `http://127.0.0.1:${simulation.port}`, | |
| 171 | relayPort: relay.port, | |
| 172 | fake, | |
| 173 | close: async () => { | |
| 174 | await Promise.race([relay.close(), sleep(500)]); | |
| 175 | await Promise.race([simulation.close(), sleep(500)]); | |
| 176 | }, | |
| 177 | }; | |
| 178 | } | |
| 179 | ||
| 180 | const post = (url: string, body: unknown = {}) => | |
| 181 | fetch(url, { method: "POST", body: JSON.stringify(body) }).then((r) => | |
| 182 | r.json().catch(() => ({})), | |
| 183 | ) as Promise<Record<string, unknown>>; | |
| 184 | ||
| 185 | /** Runs one gesture from a fresh rig. */ | |
| 186 | export async function runGesture(g: Gesture): Promise<Row> { | |
| 187 | const r = await rig(); | |
| 188 | try { | |
| 189 | const joint = g.joint; | |
| 190 | const arm = `http://127.0.0.1:${r.relayPort}`; | |
| 191 | // Put the servo at `from`, torque on, before the page ever connects. | |
| 192 | const m = r.fake.servos.get(joint)!; | |
| 193 | m[56] = g.from & 0xff; | |
| 194 | m[57] = g.from >> 8; | |
| 195 | r.fake.position(joint); | |
| 196 | const final = g.path[g.path.length - 1]![1]; | |
| 197 | ||
| 198 | const trace: Array<readonly [number, number]> = []; | |
| 199 | const t0 = performance.now(); | |
| 200 | const sampler = setInterval( | |
| 201 | () => trace.push([performance.now() - t0, r.fake.position(joint)]), | |
| 202 | 2, | |
| 203 | ); | |
| 204 | ||
| 205 | const ws = new WebSocket(`ws://127.0.0.1:${r.relayPort}/follow`); | |
| 206 | await new Promise( | |
| 207 | (ok, bad) => (ws.once("open", ok), ws.once("error", bad)), | |
| 208 | ); | |
| 209 | const frames = new Frames( | |
| 210 | (data: string) => ws.readyState === WebSocket.OPEN && ws.send(data), | |
| 211 | ); | |
| 212 | const intent = new Intent(); | |
| 213 | let lost = 0; | |
| 214 | let stopped = false; | |
| 215 | // The page's feedback: the arm's position as /live batches it, every 200 ms. | |
| 216 | let seen: number = g.from; | |
| 217 | const feedback = setInterval(() => (seen = r.fake.position(joint)), 200); | |
| 218 | const watchIntent = setInterval(() => { | |
| 219 | intent.target(joint, seen, performance.now()); | |
| 220 | lost += intent.takeLost().length; | |
| 221 | }, 50); | |
| 222 | let streamStats = { dropped: 0, gated: 0 }; | |
| 223 | const poll = setInterval(async () => { | |
| 224 | const s = (await (await fetch(`${r.base}/status`)) | |
| 225 | .json() | |
| 226 | .catch(() => null)) as { | |
| 227 | stream?: { dropped: number; gated: number }; | |
| 228 | } | null; | |
| 229 | if (s?.stream) | |
| 230 | streamStats = { dropped: s.stream.dropped, gated: s.stream.gated }; | |
| 231 | }, 50); | |
| 232 | ||
| 233 | const start = performance.now(); | |
| 234 | let sentFirst = 0; | |
| 235 | let releasedAt = 0; | |
| 236 | let stopAnswered = 0; | |
| 237 | let stopPos = 0; | |
| 238 | let resumedOk: boolean | undefined; | |
| 239 | for (const [at, ticks] of g.path) { | |
| 240 | const wait = at - (performance.now() - start); | |
| 241 | if (wait > 0) await sleep(wait); | |
| 242 | if ( | |
| 243 | g.stopAt !== undefined && | |
| 244 | !stopped && | |
| 245 | performance.now() - start >= g.stopAt | |
| 246 | ) { | |
| 247 | stopped = true; | |
| 248 | frames.drop(); | |
| 249 | await post(`${arm}/stop`, {}); | |
| 250 | stopAnswered = performance.now(); | |
| 251 | stopPos = r.fake.position(joint); | |
| 252 | } | |
| 253 | if (stopped) { | |
| 254 | frames.send({ [joint]: ticks }); // a page that kept sending: nothing may move the arm | |
| 255 | continue; | |
| 256 | } | |
| 257 | if (!sentFirst) sentFirst = performance.now() - t0; | |
| 258 | intent.set(joint, ticks, performance.now()); | |
| 259 | frames.send({ [joint]: ticks }); | |
| 260 | } | |
| 261 | releasedAt = performance.now() - t0; | |
| 262 | if (!stopped) { | |
| 263 | intent.release(joint, performance.now()); | |
| 264 | frames.flush({ [joint]: final, type: "release" } as never); // viewer.html stream.release(): the last target, marked | |
| 265 | await sleep(250); | |
| 266 | ws.close(); // and end(): 250 ms later | |
| 267 | } else { | |
| 268 | await sleep(1200); | |
| 269 | const afterStop = r.fake.position(joint); | |
| 270 | // Resume, then one new command on a new stream: now it may move. | |
| 271 | await post(`${arm}/resume`, {}); | |
| 272 | const ws2 = new WebSocket(`ws://127.0.0.1:${r.relayPort}/follow`); | |
| 273 | await new Promise( | |
| 274 | (ok, bad) => (ws2.once("open", ok), ws2.once("error", bad)), | |
| 275 | ); | |
| 276 | const f2 = new Frames((d: string) => ws2.send(d)); | |
| 277 | await sleep(30); | |
| 278 | f2.flush({ [joint]: g.from }); | |
| 279 | await sleep(2500); | |
| 280 | resumedOk = Math.abs(r.fake.position(joint) - g.from) < 30; | |
| 281 | ws2.close(); | |
| 282 | stopPos = afterStop - stopPos; // signed travel since STOP was answered | |
| 283 | } | |
| 284 | await sleep(stopped ? 200 : 3300); | |
| 285 | clearInterval(sampler); | |
| 286 | clearInterval(feedback); | |
| 287 | clearInterval(watchIntent); | |
| 288 | clearInterval(poll); | |
| 289 | lost += intent.takeLost().length; | |
| 290 | const card = (await (await fetch(`${r.base}/precision`)).json()) as { | |
| 291 | window: { snapBacks: { byCause: Record<string, number> } }; | |
| 292 | }; | |
| 293 | const dir = Math.sign(final - g.from) || 1; | |
| 294 | ||
| 295 | let latencyMs: number | null = null; | |
| 296 | for (const [t, p] of trace) | |
| 297 | if (t >= sentFirst && Math.abs(p - g.from) >= 3) { | |
| 298 | latencyMs = Math.round(t - sentFirst); | |
| 299 | break; | |
| 300 | } | |
| 301 | // Arrival: the last moment the servo was outside the band, after the release. | |
| 302 | let arriveMs: number | null = null; | |
| 303 | if (!stopped) { | |
| 304 | let lastOut = -1; | |
| 305 | for (let i = 0; i < trace.length; i++) | |
| 306 | if (Math.abs(trace[i]![1] - final) > ARRIVE) lastOut = i; | |
| 307 | if (lastOut < trace.length - 1) | |
| 308 | arriveMs = Math.max( | |
| 309 | 0, | |
| 310 | Math.round((lastOut < 0 ? 0 : trace[lastOut + 1]![0]) - releasedAt), | |
| 311 | ); | |
| 312 | } | |
| 313 | const endPos = trace[trace.length - 1]![1]; | |
| 314 | const overshoot = Math.max(0, ...trace.map(([, p]) => (p - final) * dir)); | |
| 315 | return { | |
| 316 | gesture: g.name, | |
| 317 | latencyMs, | |
| 318 | arriveMs, | |
| 319 | finalErr: stopped ? NaN : Math.round(endPos - final), | |
| 320 | overshoot: Math.round(overshoot), | |
| 321 | droppedMaxStep: streamStats.dropped, | |
| 322 | droppedGate: streamStats.gated, | |
| 323 | modelSnapBacks: lost, | |
| 324 | serverSnapBacks: | |
| 325 | Object.entries(card.window.snapBacks.byCause) | |
| 326 | .filter(([, n]) => n) | |
| 327 | .map(([c, n]) => `${c}:${n}`) | |
| 328 | .join(",") || "-", | |
| 329 | ...(stopped | |
| 330 | ? { | |
| 331 | movedAfterStop: Math.round(Math.abs(stopPos)), | |
| 332 | movedAfterResume: resumedOk, | |
| 333 | } | |
| 334 | : {}), | |
| 335 | }; | |
| 336 | } finally { | |
| 337 | await r.close(); | |
| 338 | } | |
| 339 | } | |
| 340 | ||
| 341 | export async function bench(gestures: Gesture[] = GESTURES): Promise<Row[]> { | |
| 342 | const rows: Row[] = []; | |
| 343 | for (const g of gestures) rows.push(await runGesture(g)); | |
| 344 | return rows; | |
| 345 | } | |
| 346 | ||
| 347 | export function render(rows: Row[]): string { | |
| 348 | const cell = (v: unknown, w: number) => String(v ?? "-").padEnd(w); | |
| 349 | const head = [ | |
| 350 | "gesture".padEnd(26), | |
| 351 | "lat ms".padEnd(7), | |
| 352 | "arrive ms".padEnd(10), | |
| 353 | "final err".padEnd(10), | |
| 354 | "overshoot".padEnd(10), | |
| 355 | "drop step".padEnd(10), | |
| 356 | "drop gate".padEnd(10), | |
| 357 | "model snap".padEnd(11), | |
| 358 | "server snaps", | |
| 359 | ].join(""); | |
| 360 | return [ | |
| 361 | head, | |
| 362 | ...rows.map((x) => | |
| 363 | [ | |
| 364 | cell(x.gesture, 26), | |
| 365 | cell(x.latencyMs, 7), | |
| 366 | cell(x.arriveMs, 10), | |
| 367 | cell(Number.isNaN(x.finalErr) ? "-" : x.finalErr, 10), | |
| 368 | cell(x.overshoot, 10), | |
| 369 | cell(x.droppedMaxStep, 10), | |
| 370 | cell(x.droppedGate, 10), | |
| 371 | cell(x.modelSnapBacks, 11), | |
| 372 | x.serverSnapBacks + | |
| 373 | (x.movedAfterStop !== undefined | |
| 374 | ? ` [after STOP moved ${x.movedAfterStop} ticks; after resume+cmd moved: ${x.movedAfterResume}]` | |
| 375 | : ""), | |
| 376 | ].join(""), | |
| 377 | ), | |
| 378 | ].join("\n"); | |
| 379 | } | |
| 380 | ||
| 381 | if (import.meta.main) { | |
| 382 | const rows = await bench(); | |
| 383 | console.log( | |
| 384 | process.argv.includes("--json") | |
| 385 | ? JSON.stringify(rows, null, 1) | |
| 386 | : render(rows), | |
| 387 | ); | |
| 388 | process.exit(0); | |
| 389 | } |
Software/so101/src/command-gate.test.ts +12
| @@ -40,6 +40,18 @@ describe("CommandGate", () => { | ||
| 40 | 40 | expect(s.admit(frame(8, 1500 - MAX_AGE_MS + 1), at).ok).toBe(true); // just young enough |
| 41 | 41 | }); |
| 42 | 42 | |
| 43 | test("a late release frame is the final target and is never dropped as stale (but still not when held or replayed)", () => { | |
| 44 | const gate = new CommandGate(); | |
| 45 | const s = gate.open(new Clock(), true); | |
| 46 | for (let i = 1; i <= 5; i++) expect(s.admit(frame(i, 1000 + i * 30), 1000 + i * 30 + SKEW + 12).ok).toBe(true); | |
| 47 | const at = 1500 + SKEW + 12; | |
| 48 | const release = (seq: number, t: number) => JSON.stringify({ 1: 3000, seq, t, type: "release" }); | |
| 49 | expect(s.admit(release(6, 1180), at)).toMatchObject({ ok: true }); // 330 ms late | |
| 50 | expect(s.admit(release(6, 1180), at)).toEqual({ ok: false, drop: "replayed" }); | |
| 51 | gate.hold("ann"); | |
| 52 | expect(s.admit(release(7, 1200), at)).toEqual({ ok: false, drop: "held" }); | |
| 53 | }); | |
| 54 | ||
| 43 | 55 | test("held: everything is dropped, stamped or not; a resume lets through only what was sent after it", () => { |
| 44 | 56 | const gate = new CommandGate(); |
| 45 | 57 | const clock = new Clock(); |
Software/so101/src/command-gate.ts +1 −1
| @@ -216,7 +216,7 @@ export class GateStream { | ||
| 216 | 216 | this.lastSeq = c.seq; |
| 217 | 217 | const late = this.clock.observe(c.t, now); |
| 218 | 218 | if (this.gate.held) return { ok: false, drop: "held" }; |
| 219 | if (late > MAX_AGE_MS) return { ok: false, drop: "stale" }; | |
| 219 | if (late > MAX_AGE_MS && !isRelease(text)) return { ok: false, drop: "stale" }; // the release is the final target: late, it still counts | |
| 220 | 220 | if (this.clock.here(c.t) < this.gate.since) return { ok: false, drop: "before-resume" }; |
| 221 | 221 | return { ok: true, joints: c.joints }; |
| 222 | 222 | } |
Software/so101/src/fake-bus.ts +61 −1
| @@ -19,6 +19,12 @@ export interface FakeOptions { | ||
| 19 | 19 | dropAfter?: Map<number, number>; |
| 20 | 20 | /** Goals are followed at this many ticks a second, read by read, instead of reached at once (bench/precision.ts). */ |
| 21 | 21 | speedTicksS?: number; |
| 22 | /** | |
| 23 | * Servos that move like the real thing: each follows its Goal_Position along a trapezoid, at no more than its | |
| 24 | * Goal_Velocity register (0 = the servo's own ~3400 ticks/s) and its Acceleration register (x100 ticks/s^2, 0 = the | |
| 25 | * servo's own maximum), integrated in 1 ms steps; every reply is held back `latencyMs` (default 1), like the bus. | |
| 26 | */ | |
| 27 | dynamics?: { latencyMs?: number }; | |
| 22 | 28 | } |
| 23 | 29 | |
| 24 | 30 | /** Enough of an STS3215 to answer the bus: a memory map per id, replies by instruction. */ |
| @@ -35,6 +41,8 @@ export class FakeBus implements Transport { | ||
| 35 | 41 | /** With a speed: each servo's goal and when its position was last brought up to date. */ |
| 36 | 42 | readonly = new Map<number, { goal: number; p: number }>(); |
| 37 | 43 | = performance.now(); |
| 44 | /** With dynamics: each servo's position and velocity, ticks and ticks/s. */ | |
| 45 | readonly = new Map<number, { p: number; v: number }>(); | |
| 38 | 46 | |
| 39 | 47 | constructor(ids: number[], options: FakeOptions = {}) { |
| 40 | 48 | this. = options; |
| @@ -123,7 +131,22 @@ export class FakeBus implements Transport { | ||
| 123 | 131 | } |
| 124 | 132 | } |
| 125 | 133 | } |
| 126 | for (const [delay, frame] of replies) setTimeout(() => this.?.(frame), delay); | |
| 134 | const latency = this..dynamics ? (this..dynamics.latencyMs ?? 1) : 0; | |
| 135 | for (const [delay, frame] of replies) setTimeout(() => this.?.(frame), delay + latency); | |
| 136 | } | |
| 137 | ||
| 138 | /** Bring the servos up to date with the clock (dynamics and speedTicksS); a bench reads positions after this. */ | |
| 139 | advance(): void { | |
| 140 | this.(); | |
| 141 | } | |
| 142 | ||
| 143 | /** A servo's true position right now, ticks, fractional with dynamics. */ | |
| 144 | position(id: number): number { | |
| 145 | this.(); | |
| 146 | const m = this..get(id); | |
| 147 | if (m) return m.p; | |
| 148 | const memory = this.servos.get(id)!; | |
| 149 | return memory[56]! | (memory[57]! << 8); | |
| 127 | 150 | } |
| 128 | 151 | |
| 129 | 152 | drain(callback: (error: Error | null | undefined) => void): void { |
| @@ -138,6 +161,10 @@ export class FakeBus implements Transport { | ||
| 138 | 161 | (id: number, address: number): void { |
| 139 | 162 | if (address !== STS3215.Goal_Position.address) return; |
| 140 | 163 | const memory = this.servos.get(id)!; |
| 164 | if (this..dynamics) { | |
| 165 | if (!this..has(id)) this..set(id, { p: memory[56]! | (memory[57]! << 8), v: 0 }); | |
| 166 | return; | |
| 167 | } | |
| 141 | 168 | if (this..stuck?.has(id)) { |
| 142 | 169 | memory[60] = 900 & 0xff; |
| 143 | 170 | memory[61] = 900 >> 8; |
| @@ -151,6 +178,12 @@ export class FakeBus implements Transport { | ||
| 151 | 178 | /** With a speed, every moving servo goes the distance the time since the last packet allows toward its goal. */ |
| 152 | 179 | (): void { |
| 153 | 180 | const now = performance.now(); |
| 181 | if (this..dynamics) { | |
| 182 | const elapsed = Math.min(now - this., 10_000); | |
| 183 | this. = now; | |
| 184 | for (let t = 0; t < elapsed; t += 1) this.(Math.min(1, elapsed - t) / 1000); | |
| 185 | return; | |
| 186 | } | |
| 154 | 187 | const reach = ((now - this.) / 1000) * (this..speedTicksS ?? 0); |
| 155 | 188 | this. = now; |
| 156 | 189 | if (!reach) return; |
| @@ -164,6 +197,33 @@ export class FakeBus implements Transport { | ||
| 164 | 197 | } |
| 165 | 198 | } |
| 166 | 199 | |
| 200 | (dt: number): void { | |
| 201 | for (const [id, m] of this.) { | |
| 202 | const memory = this.servos.get(id); | |
| 203 | if (!memory) continue; | |
| 204 | const goal = memory[42]! | (memory[43]! << 8); | |
| 205 | const reg = memory[46]! | (memory[47]! << 8); | |
| 206 | const vmax = reg === 0 ? 3400 : reg; | |
| 207 | const accel = memory[41]! === 0 ? 25_400 : memory[41]! * 100; | |
| 208 | const error = goal - m.p; | |
| 209 | if (error === 0 && m.v === 0) continue; | |
| 210 | const brake = accel * (Math.sqrt((dt * dt) / 4 + (2 * Math.abs(error)) / accel) - dt / 2); | |
| 211 | const wanted = Math.sign(error) * Math.min(vmax, brake); | |
| 212 | m.v = Math.max(m.v - accel * dt, Math.min(m.v + accel * dt, wanted)); | |
| 213 | const next = m.p + m.v * dt; | |
| 214 | if ((goal - next) * Math.sign(error || 1) <= 0 || Math.abs(goal - next) < 0.05) { | |
| 215 | m.p = goal; | |
| 216 | m.v = 0; | |
| 217 | } else m.p = next; | |
| 218 | const ticks = Math.round(m.p); | |
| 219 | memory[56] = ticks & 0xff; | |
| 220 | memory[57] = ticks >> 8; | |
| 221 | const v = Math.min(Math.abs(Math.round(m.v)), 0x7fff) | (m.v < 0 ? 0x8000 : 0); | |
| 222 | memory[58] = v & 0xff; | |
| 223 | memory[59] = v >> 8; | |
| 224 | } | |
| 225 | } | |
| 226 | ||
| 167 | 227 | (from: number, to: number): void { |
| 168 | 228 | const memory = this.servos.get(from)!; |
| 169 | 229 | this.servos.delete(from); |
Software/so101/src/lobby.test.ts +11 −4
| @@ -790,7 +790,7 @@ describe("lobby", () => { | ||
| 790 | 790 | expect((await torque(port, ALICE)).status).toBe(200); |
| 791 | 791 | }); |
| 792 | 792 | |
| 793 | test("a driver's browser that drops mid-stream parks the arm; one that closes the stream leaves it holding", async () => { | |
| 793 | test("a driver's browser that drops mid-stream parks the arm; one that closes the stream leaves it holding; one that lets go finishes the move", async () => { | |
| 794 | 794 | const port = await start(); |
| 795 | 795 | const alice = await join(port, ALICE); |
| 796 | 796 | alice.send({ type: "join" }); |
| @@ -798,12 +798,12 @@ describe("lobby", () => { | ||
| 798 | 798 | const sim = `http://127.0.0.1:${simulation.port}`; |
| 799 | 799 | const pan = async () => ((await (await fetch(`${sim}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 1)!.position; |
| 800 | 800 | const driving = async () => ((await (await fetch(`${sim}/status`)).json()) as { driving: string | null }).driving; |
| 801 | const stream = async () => { | |
| 801 | const stream = async (wait = 80) => { | |
| 802 | 802 | const ws = new WebSocket(`ws://127.0.0.1:${port}/follow`, { headers: ALICE }); |
| 803 | 803 | sockets.push(ws); |
| 804 | 804 | await new Promise((r) => ws.once("open", r)); |
| 805 | 805 | ws.send(stamper()({ "1": 2300 })); |
| 806 | await sleep(80); | |
| 806 | await sleep(wait); | |
| 807 | 807 | return ws; |
| 808 | 808 | }; |
| 809 | 809 | const settle = async () => { |
| @@ -811,7 +811,14 @@ describe("lobby", () => { | ||
| 811 | 811 | }; |
| 812 | 812 | (await stream()).close(); |
| 813 | 813 | await settle(); |
| 814 | expect(await pan()).toBe(2300); // closed on purpose: held | |
| 814 | const held = await pan(); | |
| 815 | expect(held).toBeGreaterThan(2048); | |
| 816 | expect(held).toBeLessThan(2300); // closed on purpose: held where the governor had got to | |
| 817 | await sleep(100); | |
| 818 | expect(await pan()).toBe(held); | |
| 819 | { const w = await stream(20); w.send(JSON.stringify({ 1: 2300, seq: 2, t: Date.now(), type: "release" })); w.close(); } // still on its way (the arm server's dead-man is 60 ms here) | |
| 820 | await settle(); | |
| 821 | expect(await pan()).toBe(2300); // let go of: finished to the last target, then stopped | |
| 815 | 822 | (await stream()).terminate(); |
| 816 | 823 | await sleep(50); |
| 817 | 824 | await settle(); |
Software/so101/src/machine-feed.test.ts +3 −3
| @@ -103,10 +103,10 @@ test("a clamp shows up in the log the moment it is applied, with the command tha | ||
| 103 | 103 | expect(typeof clamp.detail.armSeq).toBe("number"); |
| 104 | 104 | expect(entryLine(clamp, clamp.t).text).toBe("⚠ joint limit id=1 asked=9999 sent=3457 source=stream (arm #" + clamp.detail.armSeq + ")"); |
| 105 | 105 | // What reached the servo is the bounded goal, never the 9999 asked for, and the panel's target says so. |
| 106 | const goal = await p.until(() => p.log.find((e) => e.kind === "sent" && e.what === "Goal_Position" && (e.detail.values as Record<string, number>)["1"] === 3457), "the bounded goal on the bus"); | |
| 106 | const goal = await p.until(() => p.log.find((e) => e.kind === "sent" && e.what === "Goal_Position" && (e.detail.values as Record<string, number>)["1"] !== undefined && (e.detail.values as Record<string, number>)["1"]! > 2200), "a step toward the bounded goal on the bus"); | |
| 107 | 107 | expect(goal.seq).toBeGreaterThan(clamp.seq); |
| 108 | expect(p.log.some((e) => e.what === "Goal_Position" && (e.detail.values as Record<string, number>)["1"] === 9999)).toBe(false); | |
| 109 | await p.until(() => p.messages.find((m) => m.type === "status" && jointRows(m.status as never)[0]?.target === "3457 (303.8°)"), "the target in the joints"); | |
| 108 | expect(p.log.some((e) => e.what === "Goal_Position" && ((e.detail.values as Record<string, number>)["1"] ?? 0) > 3457)).toBe(false); | |
| 109 | await p.until(() => p.messages.find((m) => m.type === "status" && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) > 2200 && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) <= 3457), "the goal in the joints, stepping toward 3457"); | |
| 110 | 110 | expect(p.log.find((e) => e.kind === "state" && e.what === "driver")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) }); |
| 111 | 111 | follow.close(); |
| 112 | 112 | p.ws.close(); |
Software/so101/src/server.test.ts +69 −29
| @@ -58,24 +58,36 @@ describe("so101 serve", () => { | ||
| 58 | 58 | await sleep(40); |
| 59 | 59 | }); |
| 60 | 60 | |
| 61 | test("a stream drives the arm inside the limits and drops impossible jumps", async () => { | |
| 61 | test("a stream drives the arm inside the limits; a far jump is followed along the governor's profile, not dropped", async () => { | |
| 62 | 62 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); |
| 63 | 63 | await new Promise((r) => ws.once("open", r)); |
| 64 | 64 | ws.send(JSON.stringify({ "1": 2100, "2": 100, "6": 9999 })); // 2: below min → clamped; 6: above max → clamped |
| 65 | 65 | await sleep(30); |
| 66 | ws.send(JSON.stringify({ "1": 3000 })); // 900 ticks in one frame: dropped | |
| 66 | ws.send(JSON.stringify({ "1": 3000 })); // 900 ticks in one frame: followed, not dropped | |
| 67 | 67 | await sleep(30); |
| 68 | const stats = ((await (await fetch(`${base}/status`)).json()) as { stream: { dropped: number } }).stream; | |
| 69 | expect(stats.dropped).toBe(0); | |
| 70 | ws.send(JSON.stringify({ "1": 3000, type: "release" })); // the driver lets go: the last target, marked | |
| 68 | 71 | ws.close(); |
| 69 | await sleep(30); | |
| 70 | const goals = fake.writes.filter((w) => w.address === 42).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]); | |
| 71 | expect(goals).toContainEqual([1, 2100]); | |
| 72 | // The driver let go: the arm finishes the move to where they let go (the fake follows goals at once), then stops. | |
| 73 | const goalsOf = () => fake.writes.filter((w) => w.address === 42).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]); | |
| 74 | const arrived = () => [[1, 3000], [2, 843], [6, 3495]].every(([i, t]) => goalsOf().some(([id, g]) => id === i && g === t)); | |
| 75 | for (let i = 0; i < 250 && !arrived(); i++) await sleep(20); | |
| 76 | const goals = goalsOf(); | |
| 77 | expect(goals).toContainEqual([1, 3000]); | |
| 72 | 78 | expect(goals).toContainEqual([2, 843]); |
| 73 | 79 | expect(goals).toContainEqual([6, 3495]); |
| 74 | expect(goals).not.toContainEqual([1, 3000]); | |
| 75 | // closing the stream holds at the present position and restores the torque cap; the gripper keeps its force limit | |
| 80 | const ones = goals.filter(([id]) => id === 1).map(([, g]) => g!); | |
| 81 | expect(Math.max(...ones.slice(0, 4)) - 2048).toBeLessThan(200); // stepped along the profile, not jumped to 3000 | |
| 82 | for (let i = 0; i < 100; i++) { | |
| 83 | if (((await (await fetch(`${base}/status`)).json()) as { driving: string | null }).driving === null) break; | |
| 84 | await sleep(20); | |
| 85 | } | |
| 86 | // the arm is free again, the torque cap restored; the gripper keeps its force limit | |
| 76 | 87 | const cap = (id: number) => fake.writes.filter((w) => w.address === 48 && w.id === id).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)).at(-1); |
| 77 | 88 | expect(cap(1)).toBe(1000); |
| 78 | 89 | expect(cap(6)).toBe(350); |
| 90 | await post("/move", { targets: { "1": 2048, "2": 2048, "6": 2048 } }); // back where the next tests expect it | |
| 79 | 91 | }); |
| 80 | 92 | |
| 81 | 93 | test("a silent stream makes the arm hold", async () => { |
| @@ -85,7 +97,9 @@ describe("so101 serve", () => { | ||
| 85 | 97 | ws.send(JSON.stringify({ "1": 2200 })); |
| 86 | 98 | await sleep(150); // past the 60 ms watchdog |
| 87 | 99 | const holds = fake.writes.slice(before).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); |
| 88 | expect(holds.at(-1)).toBe(2200); // present position (the fake follows goals), written again by the watchdog | |
| 100 | const position = ((await (await fetch(`${base}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 1)!.position; | |
| 101 | expect(holds.at(-1)).toBe(position); // present position (the fake follows goals), written by the watchdog: short of 2200, where the governor had got to | |
| 102 | expect(position).toBeLessThan(2200); | |
| 89 | 103 | expect(holds.length).toBeGreaterThanOrEqual(2); |
| 90 | 104 | ws.close(); |
| 91 | 105 | await sleep(30); |
| @@ -485,7 +499,11 @@ describe("a bus that fails mid-session", () => { | ||
| 485 | 499 | ws.close(); |
| 486 | 500 | await sleep(80); |
| 487 | 501 | broken = false; |
| 488 | const status = (await (await fetch(`${url}/status`)).json()) as { driving: string | null }; | |
| 502 | let status = (await (await fetch(`${url}/status`)).json()) as { driving: string | null }; | |
| 503 | for (let i = 0; i < 150 && status.driving !== null; i++) { | |
| 504 | await sleep(20); // the released stream may still be easing the arm to where the driver let go; the dead bus cut that short or not | |
| 505 | status = (await (await fetch(`${url}/status`)).json()) as { driving: string | null }; | |
| 506 | } | |
| 489 | 507 | expect(status.driving).toBeNull(); |
| 490 | 508 | expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(200); |
| 491 | 509 | }); |
| @@ -877,9 +895,12 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 877 | 895 | let s = (await (await fetch(`${zurl}/status`)).json()) as { blocked: { what: string } | null; joints: Array<{ id: number; position: number }> }; |
| 878 | 896 | expect(s.blocked?.what).toBe("table"); |
| 879 | 897 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2)); // held |
| 880 | ws.send(JSON.stringify({ "2": mid(2) + 100 })); // the leader comes back up | |
| 881 | await sleep(60); | |
| 882 | s = (await (await fetch(`${zurl}/status`)).json()) as typeof s; | |
| 898 | for (let i = 0; i < 60; i++) { // the leader comes back up, and keeps sending: the governor takes it there at its own speed | |
| 899 | ws.send(JSON.stringify({ "2": mid(2) + 100 })); | |
| 900 | await sleep(20); | |
| 901 | s = (await (await fetch(`${zurl}/status`)).json()) as typeof s; | |
| 902 | if (s.joints.find((j) => j.id === 2)!.position === mid(2) + 100) break; | |
| 903 | } | |
| 883 | 904 | expect(s.blocked).toBeNull(); |
| 884 | 905 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) + 100); |
| 885 | 906 | const closed = new Promise((r) => ws.once("close", r)); |
| @@ -888,20 +909,30 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 888 | 909 | await idle(); |
| 889 | 910 | }); |
| 890 | 911 | |
| 891 | test("a joint the stream has not driven yet is eased over, never jumped", async () => { | |
| 912 | test("a joint the stream has not driven yet is taken along the governor's profile, never jumped and never waited for", async () => { | |
| 892 | 913 | const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`); |
| 893 | 914 | await new Promise((r) => ws.once("open", r)); |
| 894 | 915 | ws.send(JSON.stringify({ "6": mid(6) + 100 })); |
| 895 | 916 | await sleep(120); |
| 896 | 917 | const before = fake6.writes.length; |
| 897 | ws.send(JSON.stringify({ "6": mid(6) + 100, "1": mid(1) + 900 })); // the pan joins the stream 900 ticks away | |
| 898 | await sleep(150); | |
| 918 | for (let i = 0; i < 8; i++) { // the pan joins the stream 900 ticks away; the leader keeps sending (the watchdog here is 60 ms) | |
| 919 | ws.send(JSON.stringify({ "6": mid(6) + 100, "1": mid(1) + 900 })); | |
| 920 | await sleep(20); | |
| 921 | } | |
| 899 | 922 | const writes = fake6.writes.slice(before).filter((w) => w.id === 1); |
| 900 | const velocity = writes.find((w) => w.address === 46); | |
| 901 | expect(velocity!.bytes[0]! | (velocity!.bytes[1]! << 8)).toBe(300); // eased at easeSpeed, not Goal_Velocity 0 (max) | |
| 902 | const s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; stream: { frames: number; dropped: number } }; | |
| 903 | expect(s.joints.find((j) => j.id === 1)!.position).toBe(mid(1) + 900); | |
| 904 | expect(s.stream.frames).toBe(2); | |
| 923 | expect(writes.find((w) => w.address === 46 && (w.bytes[0]! | (w.bytes[1]! << 8)) === 300)).toBeUndefined(); // no slow ease speed | |
| 924 | const goal = (w: { bytes: number[] }) => w.bytes[0]! | (w.bytes[1]! << 8); | |
| 925 | const first = writes.find((w) => w.address === 42); | |
| 926 | expect(Math.abs(goal(first!) - mid(1))).toBeLessThan(150); // a step of the profile, not the jump | |
| 927 | let position = 0; | |
| 928 | for (let i = 0; i < 150 && position !== mid(1) + 900; i++) { | |
| 929 | ws.send(JSON.stringify({ "6": mid(6) + 100, "1": mid(1) + 900 })); | |
| 930 | await sleep(20); | |
| 931 | position = ((await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 1)!.position; | |
| 932 | } | |
| 933 | expect(position).toBe(mid(1) + 900); // and it gets there at the governor's speed | |
| 934 | const s = (await (await fetch(`${zurl}/status`)).json()) as { stream: { frames: number; dropped: number } }; | |
| 935 | expect(s.stream.frames).toBeGreaterThan(1); | |
| 905 | 936 | expect(s.stream.dropped).toBe(0); |
| 906 | 937 | const closed = new Promise((r) => ws.once("close", r)); |
| 907 | 938 | ws.close(); |
| @@ -944,9 +975,12 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 944 | 975 | await sleep(120); |
| 945 | 976 | let s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }> }; |
| 946 | 977 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(ticks(2, 60)); |
| 947 | ws.send(JSON.stringify({ "2": ticks(2, 50) })); // backing out: let through | |
| 948 | await sleep(120); | |
| 949 | s = (await (await fetch(`${zurl}/status`)).json()) as typeof s; | |
| 978 | for (let i = 0; i < 40; i++) { // backing out: let through, and the leader keeps sending while the governor takes the arm there | |
| 979 | ws.send(JSON.stringify({ "2": ticks(2, 50) })); | |
| 980 | await sleep(20); | |
| 981 | s = (await (await fetch(`${zurl}/status`)).json()) as typeof s; | |
| 982 | if (s.joints.find((j) => j.id === 2)!.position === ticks(2, 50)) break; | |
| 983 | } | |
| 950 | 984 | expect(s.joints.find((j) => j.id === 2)!.position).toBe(ticks(2, 50)); |
| 951 | 985 | const closed = new Promise((r) => ws.once("close", r)); |
| 952 | 986 | ws.close(); |
| @@ -1134,6 +1168,7 @@ describe("the safety envelope", () => { | ||
| 1134 | 1168 | const ws = await open(); |
| 1135 | 1169 | ws.send(JSON.stringify({ "1": 100 })); |
| 1136 | 1170 | await sleep(60); |
| 1171 | ws.send(JSON.stringify({ "1": 100, type: "release" })); // the driver lets go: the arm finishes the move to the (limited) target | |
| 1137 | 1172 | ws.close(); |
| 1138 | 1173 | await idle(); |
| 1139 | 1174 | expect(goals(1, before)).toContain(1925); // 2152.5 - 20° |
| @@ -1172,14 +1207,15 @@ describe("the safety envelope", () => { | ||
| 1172 | 1207 | }); |
| 1173 | 1208 | |
| 1174 | 1209 | test("silence makes the arm hold where it is, even halfway through a governed move", async () => { |
| 1210 | const start = await position(1); | |
| 1175 | 1211 | const ws = await open(); |
| 1176 | ws.send(JSON.stringify({ "1": 2000 })); | |
| 1212 | ws.send(JSON.stringify({ "1": start - 600 })); | |
| 1177 | 1213 | await sleep(60); |
| 1178 | ws.send(JSON.stringify({ "1": 2390 })); // then nothing: the dead-man holds after 150 ms | |
| 1214 | ws.send(JSON.stringify({ "1": start - 700 })); // then nothing: the dead-man holds after 150 ms, the governor still far from there | |
| 1179 | 1215 | await sleep(400); |
| 1180 | 1216 | const held = await position(1); |
| 1181 | expect(held).toBeGreaterThan(2000); | |
| 1182 | expect(held).toBeLessThan(2390); | |
| 1217 | expect(held).toBeLessThan(start - 20); | |
| 1218 | expect(held).toBeGreaterThan(start - 600); | |
| 1183 | 1219 | const count = goals(1).length; |
| 1184 | 1220 | await sleep(200); |
| 1185 | 1221 | expect(goals(1).length).toBe(count); // nothing more is sent |
| @@ -1194,9 +1230,13 @@ describe("the safety envelope", () => { | ||
| 1194 | 1230 | let ws = await open(); |
| 1195 | 1231 | ws.send(JSON.stringify({ "1": 2050 })); |
| 1196 | 1232 | await sleep(60); |
| 1197 | ws.close(); // on purpose | |
| 1233 | ws.close(); // on purpose: held where the governor had got to | |
| 1198 | 1234 | await idle(); |
| 1199 | expect(await position(1)).toBe(2050); | |
| 1235 | const held = await position(1); | |
| 1236 | expect(held).toBeGreaterThan(2000); | |
| 1237 | expect(held).toBeLessThan(2050); | |
| 1238 | await sleep(100); | |
| 1239 | expect(await position(1)).toBe(held); | |
| 1200 | 1240 | ws = await open(); |
| 1201 | 1241 | ws.send(JSON.stringify({ "1": 2100 })); |
| 1202 | 1242 | await sleep(60); |
Software/so101/src/server.ts +13 −45
| @@ -111,8 +111,6 @@ export interface ServerOptions { | ||
| 111 | 111 | commit?: string; |
| 112 | 112 | /** Starting values for the safety settings, before what is stored (src/safety.ts). */ |
| 113 | 113 | safety?: Partial<SafetySettings>; |
| 114 | /** Speed used to catch up with the first frame of a stream, ticks/s. */ | |
| 115 | easeSpeed?: number; | |
| 116 | 114 | /** How often the stream's jam watch samples position and load, ms. */ |
| 117 | 115 | jamSampleMs?: number; |
| 118 | 116 | /** How often the status is read for /watch, ms; it is sent when it changed (and at least every watchQuietMs). */ |
| @@ -195,7 +193,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 195 | 193 | const settingsFile = join(dataDir, "settings.json"); |
| 196 | 194 | const factoryFile = join(dataDir, "servo-factory.json"); |
| 197 | 195 | const streamTorque = options.streamTorque ?? 70; |
| 198 | const easeSpeed = options.easeSpeed ?? 300; | |
| 199 | 196 | const jamSampleMs = options.jamSampleMs ?? 100; |
| 200 | 197 | const heartbeatMs = options.heartbeatMs ?? 1000; |
| 201 | 198 | const estopFile = join(dataDir, "estop.json"); |
| @@ -1238,11 +1235,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1238 | 1235 | await servoLimits(ready); |
| 1239 | 1236 | await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const)); |
| 1240 | 1237 | governor.reset(positions); |
| 1241 | last = null; // the first frame eases the arm over, however far away the leader is | |
| 1238 | last = null; // the first frame's targets are taken from where the arm is, by the governor | |
| 1242 | 1239 | heldDepth = evaluate(positions)?.violation?.depth ?? 0; |
| 1243 | 1240 | armed = true; |
| 1244 | 1241 | }); |
| 1245 | let easing = false; | |
| 1246 | 1242 | |
| 1247 | 1243 | // The jam watch: a joint that pushes hard without moving toward its target is parked where it is. |
| 1248 | 1244 | const windows = new Map<number, Array<{ p: number; l: number }>>(); |
| @@ -1288,7 +1284,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1288 | 1284 | const now = performance.now(); |
| 1289 | 1285 | const dt = Math.min(now - lastTick, 2 * period) / 1000; // a late tick never turns into a jump |
| 1290 | 1286 | lastTick = now; |
| 1291 | if (!armed || easing || ticking || !governor.moving) return; | |
| 1287 | if (!armed || ticking || !governor.moving) return; | |
| 1292 | 1288 | ticking = true; |
| 1293 | 1289 | void run(async () => { |
| 1294 | 1290 | try { |
| @@ -1315,7 +1311,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1315 | 1311 | if (closed) return; // queued before a stop: dropped, never replayed |
| 1316 | 1312 | const b = bounds(); |
| 1317 | 1313 | const targets: Array<readonly [number, number]> = []; |
| 1318 | let ease = last === null; // after a hold, everything eases | |
| 1319 | 1314 | for (const [id, value] of frame) { |
| 1320 | 1315 | const target = bound(b, id, value); |
| 1321 | 1316 | if (target === null) continue; |
| @@ -1333,7 +1328,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1333 | 1328 | windows.delete(id); |
| 1334 | 1329 | } |
| 1335 | 1330 | // A flick is followed along the governor's profile, never dropped for being far: the limits are what keep it safe. |
| 1336 | if (last?.get(id) === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped | |
| 1337 | 1331 | targets.push([id, target]); |
| 1338 | 1332 | } |
| 1339 | 1333 | if (!targets.length) return; |
| @@ -1369,38 +1363,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1369 | 1363 | refusing = false; |
| 1370 | 1364 | heldDepth = zone?.violation?.depth ?? 0; |
| 1371 | 1365 | approach(zone); |
| 1372 | if (ease) { | |
| 1373 | // Catch up slowly with every joint in this frame, the servos' own speed held low, then hand over to the governor. | |
| 1374 | // A joint that jams on the way is parked and no longer waited for. | |
| 1375 | easing = true; | |
| 1376 | for (const [id, t] of targets) tracker.target(id, t, Date.now()); | |
| 1377 | try { | |
| 1378 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, jointMotion(speedLimits(), id).speed)] as const)); | |
| 1379 | await bus.syncWrite(STS3215.Goal_Position, targets); | |
| 1380 | const deadline = Date.now() + 4000; | |
| 1381 | const wanted = new Map(targets); | |
| 1382 | while (Date.now() < deadline && !closed) { | |
| 1383 | const positions = await bus.syncRead([...wanted.keys()], STS3215.Present_Position); | |
| 1384 | const loads = await bus.syncRead([...wanted.keys()], STS3215.Present_Load); | |
| 1385 | let worst = 0; | |
| 1386 | for (const [id, t] of wanted) { | |
| 1387 | const p = positions.get(id), l = loads.get(id); | |
| 1388 | if (p === undefined || l === undefined) continue; | |
| 1389 | if (await observe(id, p, l, t)) continue; // jammed: parked, not waited for | |
| 1390 | worst = Math.max(worst, Math.abs(p - t)); | |
| 1391 | } | |
| 1392 | if (worst <= 25) break; | |
| 1393 | await new Promise((r) => setTimeout(r, 50)); | |
| 1394 | } | |
| 1395 | if (!closed) await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, jointMotion(speedLimits(), id).speed] as const)); | |
| 1396 | } finally { | |
| 1397 | easing = false; | |
| 1398 | } | |
| 1399 | last = new Map([...(last ?? []), ...targets]); | |
| 1400 | for (const [id, t] of targets) governor.pin(id, jams.get(id)?.position ?? t); | |
| 1401 | for (const [id, jam] of jams) last.set(id, jam.position); | |
| 1402 | return; | |
| 1403 | } | |
| 1366 | last ??= new Map(); // the first frame of a stream, or the first after a hold, goes the same way as any other | |
| 1404 | 1367 | for (const [id, t] of targets) { |
| 1405 | 1368 | governor.aim(id, t); |
| 1406 | 1369 | tracker.target(id, t, Date.now()); |
| @@ -1415,9 +1378,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1415 | 1378 | const take = () => { |
| 1416 | 1379 | pacer = undefined; |
| 1417 | 1380 | if (!pending || closed) return; |
| 1418 | // While the arm catches up, the frame waits (later ones merge into it) and is taken once it has: never dropped, | |
| 1419 | // or a leader that sends one last frame and stops would leave the arm short of it. | |
| 1420 | if (easing) return void (pacer = setTimeout(take, period)); | |
| 1421 | 1381 | const frame = pending; |
| 1422 | 1382 | pending = null; |
| 1423 | 1383 | lastTaken = Date.now(); |
| @@ -1483,11 +1443,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1483 | 1443 | }); |
| 1484 | 1444 | /** 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. */ |
| 1485 | 1445 | const arrive = async () => { |
| 1486 | const busy = () => pending || easing || ticking; | |
| 1446 | const busy = () => pending || ticking; | |
| 1487 | 1447 | const wait = async (until: number, more: () => unknown) => { |
| 1488 | 1448 | while (!closed && Date.now() < until && more()) await new Promise((r) => setTimeout(r, period)); |
| 1489 | 1449 | }; |
| 1490 | await wait(Date.now() + 5000, busy); // a frame waiting on the ease is followed, never dropped | |
| 1450 | await wait(Date.now() + 5000, busy); // a frame waiting for its slot is followed, never dropped | |
| 1491 | 1451 | await chain; // and the one just taken has reached the governor |
| 1492 | 1452 | await wait(Date.now() + 5000, busy); |
| 1493 | 1453 | const { speed, accel } = speedLimits(); |
| @@ -1496,6 +1456,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1496 | 1456 | deadline += (1000 * Math.abs(shortfall(await bus.syncRead(ids, STS3215.Present_Position))[1])) / speed; |
| 1497 | 1457 | } catch {} |
| 1498 | 1458 | await wait(deadline, () => busy() || governor.moving); |
| 1459 | // The governor's goal leads the servo by the distance it still has to brake; holding at present now would stop it that far short. | |
| 1460 | for (const end = Date.now() + 600; !closed && Date.now() < end; await new Promise((r) => setTimeout(r, period))) { | |
| 1461 | try { | |
| 1462 | if (Math.abs(shortfall(await bus.syncRead(ids, STS3215.Present_Position))[1]) <= ARRIVE_TICKS) break; | |
| 1463 | } catch { | |
| 1464 | break; | |
| 1465 | } | |
| 1466 | } | |
| 1499 | 1467 | halt(); |
| 1500 | 1468 | }; |
| 1501 | 1469 | const finish = (code: number) => { |
Software/so101/src/slider-follow.test.ts +18 −5
| @@ -65,17 +65,30 @@ async function drive(): Promise<number> { | ||
| 65 | 65 | return relay.port; |
| 66 | 66 | } |
| 67 | 67 | |
| 68 | test("a driver's slider move reaches the fake bus as goal positions within one status tick", async () => { | |
| 68 | test("a driver's slider move reaches the fake bus as goal positions within one status tick, and the arm gets there along the profile", async () => { | |
| 69 | 69 | const port = await drive(); |
| 70 | 70 | const stream = await open(port, "/follow", ALICE); |
| 71 | 71 | const before = fake.writes.length; |
| 72 | 72 | const sentAt = Date.now(); |
| 73 | 73 | stream.send(stamp({ "1": 2200, "2": 1900, "6": 2500 })); |
| 74 | const want = { 1: 2200, 2: 1900, 6: 2500 }; | |
| 75 | const latest = () => Object.fromEntries(goals(before).map(([id, ticks]) => [id!, ticks!])); | |
| 76 | while (Date.now() - sentAt < TICK_MS && JSON.stringify(latest()) !== JSON.stringify(want)) await sleep(5); // taking the stream first parks the joints where they are | |
| 74 | const want: Record<number, number> = { 1: 2200, 2: 1900, 6: 2500 }; | |
| 75 | // Taking the stream first parks the joints where they are; then each goal steps toward its target, none past it. | |
| 76 | const stepping = () => { | |
| 77 | const all = goals(before); | |
| 78 | return Object.entries(want).every(([id, target]) => { | |
| 79 | const mine = all.filter(([i]) => i === Number(id)).map(([, g]) => g!); | |
| 80 | return mine.length > 1 && Math.sign(mine.at(-1)! - mine[0]!) === Math.sign(target - mine[0]!) && mine.every((g) => (g - target) * Math.sign(mine[0]! - target) >= 0); | |
| 81 | }); | |
| 82 | }; | |
| 83 | while (Date.now() - sentAt < TICK_MS && !stepping()) await sleep(5); | |
| 77 | 84 | expect(Date.now() - sentAt).toBeLessThanOrEqual(TICK_MS); |
| 78 | expect(latest()).toEqual(want); | |
| 85 | expect(stepping()).toBe(true); | |
| 86 | for (let i = 0; i < 100; i++) { | |
| 87 | const last = Object.fromEntries(goals(before).map(([id, ticks]) => [id!, ticks!])); | |
| 88 | if (JSON.stringify(last) === JSON.stringify(want)) break; | |
| 89 | await sleep(20); | |
| 90 | } | |
| 91 | expect(Object.fromEntries(goals(before).map(([id, ticks]) => [id!, ticks!]))).toEqual(want); | |
| 79 | 92 | }); |
| 80 | 93 | |
| 81 | 94 | test("a non-driver's frame is not applied", async () => { |
Software/so101/src/stop.test.ts +32 −6
| @@ -338,22 +338,45 @@ describe("STOP on the arm server itself", () => { | ||
| 338 | 338 | /** Goal_Position (42) and Goal_Velocity (46) writes since `from`. */ |
| 339 | 339 | const goals = (from: number) => fake.writes.slice(from).filter((w) => w.address === 42 || w.address === 46); |
| 340 | 340 | |
| 341 | test("STOP during the catch-up ease ends it at once: no more of the ease, and the arm holds where it is", async () => { | |
| 341 | test("STOP while the governor is easing the arm to a far target ends it at once: no more steps, and the arm holds where it is", async () => { | |
| 342 | 342 | const ws = await stream(); |
| 343 | // joint 2 is stuck: the ease toward 2600 would wait out its 4 s, then speed the servos back up | |
| 343 | // joint 2 is stuck: the governor keeps stepping its goal toward 2600 until STOP | |
| 344 | 344 | ws.send(JSON.stringify({ 1: 2100, 2: 2600, seq: 1, t: Date.now() })); |
| 345 | 345 | await sleep(150); |
| 346 | 346 | const from = fake.writes.length; |
| 347 | 347 | expect(await (await post("/stop", { by: "ann" })).json()).toEqual({ stopped: "stream" }); |
| 348 | 348 | await sleep(150); |
| 349 | 349 | const after = goals(from); |
| 350 | expect(after.filter((w) => w.address === 46)).toEqual([]); // the ease did not finish by speeding up again | |
| 351 | 350 | const holds = after.filter((w) => w.address === 42).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]); |
| 352 | 351 | expect(holds).toContainEqual([2, 2048]); // held where it is, not driven on to 2600 |
| 353 | expect(holds).not.toContainEqual([2, 2600]); | |
| 352 | expect(holds.filter(([id]) => id === 2).at(-1)![1]).toBe(2048); // the last word is the hold; a step already on the wire may land first, none after | |
| 354 | 353 | expect(((await (await fetch(`${base}/status`)).json()) as { held: { by: string } }).held.by).toBe("ann"); |
| 355 | 354 | }); |
| 356 | 355 | |
| 356 | test("STOP (and the end of a turn) while a released stream is still finishing its move holds the arm where it is: nothing more is sent", async () => { | |
| 357 | for (const hold of [true, false]) { | |
| 358 | await post("/resume", {}); | |
| 359 | expect((await post("/move", { targets: { 1: 2048 } })).status).toBe(200); | |
| 360 | const ws = await stream(); | |
| 361 | ws.send(JSON.stringify({ 1: 3400 })); | |
| 362 | await sleep(60); | |
| 363 | ws.send(JSON.stringify({ 1: 3400, type: "release" })); // the driver let go: the arm is finishing the long move to 3400 | |
| 364 | ws.close(); | |
| 365 | await sleep(80); | |
| 366 | const from = fake.writes.length; | |
| 367 | expect(await (await post("/stop", { by: "ann", hold })).json()).toEqual({ stopped: "stream" }); | |
| 368 | await sleep(250); | |
| 369 | const ones = () => fake.writes.slice(from).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); | |
| 370 | const seen = ones(); | |
| 371 | expect(seen.length).toBeGreaterThan(0); | |
| 372 | expect(seen.at(-1)).toBeLessThan(3400); // held where it had got to, not carried on | |
| 373 | await sleep(250); | |
| 374 | expect(ones()).toEqual(seen); // and nothing more is sent | |
| 375 | expect(((await (await fetch(`${base}/status`)).json()) as { driving: string | null }).driving).toBeNull(); | |
| 376 | } | |
| 377 | await post("/stop", { by: "ann" }); // held again, as the next test finds it | |
| 378 | }); | |
| 379 | ||
| 357 | 380 | test("while held, a stream is refused and nothing it sends moves the arm; after resume, only a new command does", async () => { |
| 358 | 381 | const from = fake.writes.length; |
| 359 | 382 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); |
| @@ -375,7 +398,9 @@ describe("STOP on the arm server itself", () => { | ||
| 375 | 398 | await sleep(100); |
| 376 | 399 | const moved = goals(from).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); |
| 377 | 400 | expect(moved.length).toBeGreaterThan(0); |
| 378 | expect(new Set(moved)).toEqual(new Set([here, 2150])); // armed where it was, then the new command | |
| 401 | expect(moved[0]).toBe(here); // armed where it was | |
| 402 | expect(moved.length).toBeGreaterThan(1); | |
| 403 | expect(moved.slice(1).every((g) => g > here && g <= 2150)).toBe(true); // then the new command, along the governor's profile | |
| 379 | 404 | s.close(); |
| 380 | 405 | await sleep(50); |
| 381 | 406 | }); |
| @@ -398,7 +423,8 @@ describe("STOP on the arm server itself", () => { | ||
| 398 | 423 | s.send(JSON.stringify({ 1: 2150, seq: 2, t: Date.now(), epoch: N })); |
| 399 | 424 | await sleep(100); |
| 400 | 425 | const moved = goals(from).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8)); |
| 401 | expect(moved).toContain(2150); | |
| 426 | expect(moved.length).toBeGreaterThan(1); | |
| 427 | expect(moved.every((g) => g <= 2150) && moved.at(-1)! > moved[0]!).toBe(true); // stepping toward 2150 along the governor's profile | |
| 402 | 428 | expect(moved).not.toContain(2300); |
| 403 | 429 | s.close(); |
| 404 | 430 | await sleep(50); |
new above
offline