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

Merged opened this

Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.

Merged

hunt/motion → main 2 commits

  • Merged. hunt/motion was merged into main as 13634a0.

Review

Sign in to review this change.
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", () => {
4040 expect(s.admit(frame(8, 1500 - MAX_AGE_MS + 1), at).ok).toBe(true); // just young enough
4141 });
4242
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
4355 test("held: everything is dropped, stamped or not; a resume lets through only what was sent after it", () => {
4456 const gate = new CommandGate();
4557 const clock = new Clock();
Software/so101/src/command-gate.ts +1 −1
@@ -216,7 +216,7 @@ export class GateStream {
216216 this.lastSeq = c.seq;
217217 const late = this.clock.observe(c.t, now);
218218 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
220220 if (this.clock.here(c.t) < this.gate.since) return { ok: false, drop: "before-resume" };
221221 return { ok: true, joints: c.joints };
222222 }
Software/so101/src/fake-bus.ts +61 −1
@@ -19,6 +19,12 @@ export interface FakeOptions {
1919 dropAfter?: Map<number, number>;
2020 /** Goals are followed at this many ticks a second, read by read, instead of reached at once (bench/precision.ts). */
2121 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 };
2228 }
2329
2430 /** 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 {
3541 /** With a speed: each servo's goal and when its position was last brought up to date. */
3642 readonly #goals = new Map<number, { goal: number; p: number }>();
3743 #movedAt = performance.now();
44 /** With dynamics: each servo's position and velocity, ticks and ticks/s. */
45 readonly #motion = new Map<number, { p: number; v: number }>();
3846
3947 constructor(ids: number[], options: FakeOptions = {}) {
4048 this.#options = options;
@@ -123,7 +131,22 @@ export class FakeBus implements Transport {
123131 }
124132 }
125133 }
126 for (const [delay, frame] of replies) setTimeout(() => this.#listener?.(frame), delay);
134 const latency = this.#options.dynamics ? (this.#options.dynamics.latencyMs ?? 1) : 0;
135 for (const [delay, frame] of replies) setTimeout(() => this.#listener?.(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.#travel();
141 }
142
143 /** A servo's true position right now, ticks, fractional with dynamics. */
144 position(id: number): number {
145 this.#travel();
146 const m = this.#motion.get(id);
147 if (m) return m.p;
148 const memory = this.servos.get(id)!;
149 return memory[56]! | (memory[57]! << 8);
127150 }
128151
129152 drain(callback: (error: Error | null | undefined) => void): void {
@@ -138,6 +161,10 @@ export class FakeBus implements Transport {
138161 #follow(id: number, address: number): void {
139162 if (address !== STS3215.Goal_Position.address) return;
140163 const memory = this.servos.get(id)!;
164 if (this.#options.dynamics) {
165 if (!this.#motion.has(id)) this.#motion.set(id, { p: memory[56]! | (memory[57]! << 8), v: 0 });
166 return;
167 }
141168 if (this.#options.stuck?.has(id)) {
142169 memory[60] = 900 & 0xff;
143170 memory[61] = 900 >> 8;
@@ -151,6 +178,12 @@ export class FakeBus implements Transport {
151178 /** With a speed, every moving servo goes the distance the time since the last packet allows toward its goal. */
152179 #travel(): void {
153180 const now = performance.now();
181 if (this.#options.dynamics) {
182 const elapsed = Math.min(now - this.#movedAt, 10_000);
183 this.#movedAt = now;
184 for (let t = 0; t < elapsed; t += 1) this.#step(Math.min(1, elapsed - t) / 1000);
185 return;
186 }
154187 const reach = ((now - this.#movedAt) / 1000) * (this.#options.speedTicksS ?? 0);
155188 this.#movedAt = now;
156189 if (!reach) return;
@@ -164,6 +197,33 @@ export class FakeBus implements Transport {
164197 }
165198 }
166199
200 #step(dt: number): void {
201 for (const [id, m] of this.#motion) {
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
167227 #rekey(from: number, to: number): void {
168228 const memory = this.servos.get(from)!;
169229 this.servos.delete(from);
Software/so101/src/lobby.test.ts +11 −4
@@ -790,7 +790,7 @@ describe("lobby", () => {
790790 expect((await torque(port, ALICE)).status).toBe(200);
791791 });
792792
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 () => {
794794 const port = await start();
795795 const alice = await join(port, ALICE);
796796 alice.send({ type: "join" });
@@ -798,12 +798,12 @@ describe("lobby", () => {
798798 const sim = `http://127.0.0.1:${simulation.port}`;
799799 const pan = async () => ((await (await fetch(`${sim}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 1)!.position;
800800 const driving = async () => ((await (await fetch(`${sim}/status`)).json()) as { driving: string | null }).driving;
801 const stream = async () => {
801 const stream = async (wait = 80) => {
802802 const ws = new WebSocket(`ws://127.0.0.1:${port}/follow`, { headers: ALICE });
803803 sockets.push(ws);
804804 await new Promise((r) => ws.once("open", r));
805805 ws.send(stamper()({ "1": 2300 }));
806 await sleep(80);
806 await sleep(wait);
807807 return ws;
808808 };
809809 const settle = async () => {
@@ -811,7 +811,14 @@ describe("lobby", () => {
811811 };
812812 (await stream()).close();
813813 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
815822 (await stream()).terminate();
816823 await sleep(50);
817824 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
103103 expect(typeof clamp.detail.armSeq).toBe("number");
104104 expect(entryLine(clamp, clamp.t).text).toBe("⚠ joint limit id=1 asked=9999 sent=3457 source=stream (arm #" + clamp.detail.armSeq + ")");
105105 // 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");
107107 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");
110110 expect(p.log.find((e) => e.kind === "state" && e.what === "driver")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) });
111111 follow.close();
112112 p.ws.close();
Software/so101/src/server.test.ts +69 −29
@@ -58,24 +58,36 @@ describe("so101 serve", () => {
5858 await sleep(40);
5959 });
6060
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 () => {
6262 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
6363 await new Promise((r) => ws.once("open", r));
6464 ws.send(JSON.stringify({ "1": 2100, "2": 100, "6": 9999 })); // 2: below min → clamped; 6: above max → clamped
6565 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
6767 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
6871 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]);
7278 expect(goals).toContainEqual([2, 843]);
7379 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
7687 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);
7788 expect(cap(1)).toBe(1000);
7889 expect(cap(6)).toBe(350);
90 await post("/move", { targets: { "1": 2048, "2": 2048, "6": 2048 } }); // back where the next tests expect it
7991 });
8092
8193 test("a silent stream makes the arm hold", async () => {
@@ -85,7 +97,9 @@ describe("so101 serve", () => {
8597 ws.send(JSON.stringify({ "1": 2200 }));
8698 await sleep(150); // past the 60 ms watchdog
8799 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);
89103 expect(holds.length).toBeGreaterThanOrEqual(2);
90104 ws.close();
91105 await sleep(30);
@@ -485,7 +499,11 @@ describe("a bus that fails mid-session", () => {
485499 ws.close();
486500 await sleep(80);
487501 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 }
489507 expect(status.driving).toBeNull();
490508 expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(200);
491509 });
@@ -877,9 +895,12 @@ describe("the safe zone, the jam watch, and the model routes", () => {
877895 let s = (await (await fetch(`${zurl}/status`)).json()) as { blocked: { what: string } | null; joints: Array<{ id: number; position: number }> };
878896 expect(s.blocked?.what).toBe("table");
879897 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 }
883904 expect(s.blocked).toBeNull();
884905 expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) + 100);
885906 const closed = new Promise((r) => ws.once("close", r));
@@ -888,20 +909,30 @@ describe("the safe zone, the jam watch, and the model routes", () => {
888909 await idle();
889910 });
890911
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 () => {
892913 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
893914 await new Promise((r) => ws.once("open", r));
894915 ws.send(JSON.stringify({ "6": mid(6) + 100 }));
895916 await sleep(120);
896917 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 }
899922 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);
905936 expect(s.stream.dropped).toBe(0);
906937 const closed = new Promise((r) => ws.once("close", r));
907938 ws.close();
@@ -944,9 +975,12 @@ describe("the safe zone, the jam watch, and the model routes", () => {
944975 await sleep(120);
945976 let s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }> };
946977 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 }
950984 expect(s.joints.find((j) => j.id === 2)!.position).toBe(ticks(2, 50));
951985 const closed = new Promise((r) => ws.once("close", r));
952986 ws.close();
@@ -1134,6 +1168,7 @@ describe("the safety envelope", () => {
11341168 const ws = await open();
11351169 ws.send(JSON.stringify({ "1": 100 }));
11361170 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
11371172 ws.close();
11381173 await idle();
11391174 expect(goals(1, before)).toContain(1925); // 2152.5 - 20°
@@ -1172,14 +1207,15 @@ describe("the safety envelope", () => {
11721207 });
11731208
11741209 test("silence makes the arm hold where it is, even halfway through a governed move", async () => {
1210 const start = await position(1);
11751211 const ws = await open();
1176 ws.send(JSON.stringify({ "1": 2000 }));
1212 ws.send(JSON.stringify({ "1": start - 600 }));
11771213 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
11791215 await sleep(400);
11801216 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);
11831219 const count = goals(1).length;
11841220 await sleep(200);
11851221 expect(goals(1).length).toBe(count); // nothing more is sent
@@ -1194,9 +1230,13 @@ describe("the safety envelope", () => {
11941230 let ws = await open();
11951231 ws.send(JSON.stringify({ "1": 2050 }));
11961232 await sleep(60);
1197 ws.close(); // on purpose
1233 ws.close(); // on purpose: held where the governor had got to
11981234 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);
12001240 ws = await open();
12011241 ws.send(JSON.stringify({ "1": 2100 }));
12021242 await sleep(60);
Software/so101/src/server.ts +13 −45
@@ -111,8 +111,6 @@ export interface ServerOptions {
111111 commit?: string;
112112 /** Starting values for the safety settings, before what is stored (src/safety.ts). */
113113 safety?: Partial<SafetySettings>;
114 /** Speed used to catch up with the first frame of a stream, ticks/s. */
115 easeSpeed?: number;
116114 /** How often the stream's jam watch samples position and load, ms. */
117115 jamSampleMs?: number;
118116 /** 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 = {}):
195193 const settingsFile = join(dataDir, "settings.json");
196194 const factoryFile = join(dataDir, "servo-factory.json");
197195 const streamTorque = options.streamTorque ?? 70;
198 const easeSpeed = options.easeSpeed ?? 300;
199196 const jamSampleMs = options.jamSampleMs ?? 100;
200197 const heartbeatMs = options.heartbeatMs ?? 1000;
201198 const estopFile = join(dataDir, "estop.json");
@@ -1238,11 +1235,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
12381235 await servoLimits(ready);
12391236 await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const));
12401237 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
12421239 heldDepth = evaluate(positions)?.violation?.depth ?? 0;
12431240 armed = true;
12441241 });
1245 let easing = false;
12461242
12471243 // The jam watch: a joint that pushes hard without moving toward its target is parked where it is.
12481244 const windows = new Map<number, Array<{ p: number; l: number }>>();
@@ -1288,7 +1284,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
12881284 const now = performance.now();
12891285 const dt = Math.min(now - lastTick, 2 * period) / 1000; // a late tick never turns into a jump
12901286 lastTick = now;
1291 if (!armed || easing || ticking || !governor.moving) return;
1287 if (!armed || ticking || !governor.moving) return;
12921288 ticking = true;
12931289 void run(async () => {
12941290 try {
@@ -1315,7 +1311,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13151311 if (closed) return; // queued before a stop: dropped, never replayed
13161312 const b = bounds();
13171313 const targets: Array<readonly [number, number]> = [];
1318 let ease = last === null; // after a hold, everything eases
13191314 for (const [id, value] of frame) {
13201315 const target = bound(b, id, value);
13211316 if (target === null) continue;
@@ -1333,7 +1328,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13331328 windows.delete(id);
13341329 }
13351330 // 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
13371331 targets.push([id, target]);
13381332 }
13391333 if (!targets.length) return;
@@ -1369,38 +1363,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13691363 refusing = false;
13701364 heldDepth = zone?.violation?.depth ?? 0;
13711365 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
14041367 for (const [id, t] of targets) {
14051368 governor.aim(id, t);
14061369 tracker.target(id, t, Date.now());
@@ -1415,9 +1378,6 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
14151378 const take = () => {
14161379 pacer = undefined;
14171380 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));
14211381 const frame = pending;
14221382 pending = null;
14231383 lastTaken = Date.now();
@@ -1483,11 +1443,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
14831443 });
14841444 /** 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. */
14851445 const arrive = async () => {
1486 const busy = () => pending || easing || ticking;
1446 const busy = () => pending || ticking;
14871447 const wait = async (until: number, more: () => unknown) => {
14881448 while (!closed && Date.now() < until && more()) await new Promise((r) => setTimeout(r, period));
14891449 };
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
14911451 await chain; // and the one just taken has reached the governor
14921452 await wait(Date.now() + 5000, busy);
14931453 const { speed, accel } = speedLimits();
@@ -1496,6 +1456,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
14961456 deadline += (1000 * Math.abs(shortfall(await bus.syncRead(ids, STS3215.Present_Position))[1])) / speed;
14971457 } catch {}
14981458 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 }
14991467 halt();
15001468 };
15011469 const finish = (code: number) => {
Software/so101/src/slider-follow.test.ts +18 −5
@@ -65,17 +65,30 @@ async function drive(): Promise<number> {
6565 return relay.port;
6666 }
6767
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 () => {
6969 const port = await drive();
7070 const stream = await open(port, "/follow", ALICE);
7171 const before = fake.writes.length;
7272 const sentAt = Date.now();
7373 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);
7784 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);
7992 });
8093
8194 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", () => {
338338 /** Goal_Position (42) and Goal_Velocity (46) writes since `from`. */
339339 const goals = (from: number) => fake.writes.slice(from).filter((w) => w.address === 42 || w.address === 46);
340340
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 () => {
342342 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
344344 ws.send(JSON.stringify({ 1: 2100, 2: 2600, seq: 1, t: Date.now() }));
345345 await sleep(150);
346346 const from = fake.writes.length;
347347 expect(await (await post("/stop", { by: "ann" })).json()).toEqual({ stopped: "stream" });
348348 await sleep(150);
349349 const after = goals(from);
350 expect(after.filter((w) => w.address === 46)).toEqual([]); // the ease did not finish by speeding up again
351350 const holds = after.filter((w) => w.address === 42).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]);
352351 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
354353 expect(((await (await fetch(`${base}/status`)).json()) as { held: { by: string } }).held.by).toBe("ann");
355354 });
356355
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
357380 test("while held, a stream is refused and nothing it sends moves the arm; after resume, only a new command does", async () => {
358381 const from = fake.writes.length;
359382 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
@@ -375,7 +398,9 @@ describe("STOP on the arm server itself", () => {
375398 await sleep(100);
376399 const moved = goals(from).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
377400 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
379404 s.close();
380405 await sleep(50);
381406 });
@@ -398,7 +423,8 @@ describe("STOP on the arm server itself", () => {
398423 s.send(JSON.stringify({ 1: 2150, seq: 2, t: Date.now(), epoch: N }));
399424 await sleep(100);
400425 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
402428 expect(moved).not.toContain(2300);
403429 s.close();
404430 await sleep(50);

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs