Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
A jam is load without progress: hard acceleration toward a far target is not a jam (17 false jams, 2 of 3 snap-backs) #511
vesper/arm:vesper/509-a-jam-is-load-without-progress-hard-acce → main 1 commit fork
8 files
changed +268
−24
Software/so101/src/fake-bus.ts +12 −1
| @@ -23,6 +23,8 @@ export interface FakeOptions { | ||
| 23 | 23 | * Servos that move like the real thing: each follows its Goal_Position along a trapezoid, at no more than its |
| 24 | 24 | * Goal_Velocity register (0 = the servo's own ~3400 ticks/s) and its Acceleration register (x100 ticks/s^2, 0 = the |
| 25 | 25 | * servo's own maximum), integrated in 1 ms steps; every reply is held back `latencyMs` (default 1), like the bus. |
| 26 | * Present_Load is what the real arm shows on a hard move: 70 % while a servo speeds up or brakes, 15 % at a steady | |
| 27 | * speed, nothing at rest. A stuck id never moves and reports 90 %. | |
| 26 | 28 | */ |
| 27 | 29 | dynamics?: { latencyMs?: number }; |
| 28 | 30 | } |
| @@ -162,6 +164,7 @@ export class FakeBus implements Transport { | ||
| 162 | 164 | if (address !== STS3215.Goal_Position.address) return; |
| 163 | 165 | const memory = this.servos.get(id)!; |
| 164 | 166 | if (this..dynamics) { |
| 167 | if (this..stuck?.has(id)) return void memory.set([900 & 0xff, 900 >> 8], 60); | |
| 165 | 168 | if (!this..has(id)) this..set(id, { p: memory[56]! | (memory[57]! << 8), v: 0 }); |
| 166 | 169 | return; |
| 167 | 170 | } |
| @@ -206,7 +209,11 @@ export class FakeBus implements Transport { | ||
| 206 | 209 | const vmax = reg === 0 ? 3400 : reg; |
| 207 | 210 | const accel = memory[41]! === 0 ? 25_400 : memory[41]! * 100; |
| 208 | 211 | const error = goal - m.p; |
| 209 | if (error === 0 && m.v === 0) continue; | |
| 212 | if (error === 0 && m.v === 0) { | |
| 213 | memory[60] = memory[61] = 0; | |
| 214 | continue; | |
| 215 | } | |
| 216 | const before = m.v; | |
| 210 | 217 | const brake = accel * (Math.sqrt((dt * dt) / 4 + (2 * Math.abs(error)) / accel) - dt / 2); |
| 211 | 218 | const wanted = Math.sign(error) * Math.min(vmax, brake); |
| 212 | 219 | m.v = Math.max(m.v - accel * dt, Math.min(m.v + accel * dt, wanted)); |
| @@ -221,6 +228,10 @@ export class FakeBus implements Transport { | ||
| 221 | 228 | const v = Math.min(Math.abs(Math.round(m.v)), 0x7fff) | (m.v < 0 ? 0x8000 : 0); |
| 222 | 229 | memory[58] = v & 0xff; |
| 223 | 230 | memory[59] = v >> 8; |
| 231 | const push = m.v - before; // speeding up or braking: the motor works hard; at a steady speed it barely does | |
| 232 | const load = push !== 0 ? 700 | (push < 0 ? 0x400 : 0) : m.v !== 0 ? 150 | (m.v < 0 ? 0x400 : 0) : 0; | |
| 233 | memory[60] = load & 0xff; | |
| 234 | memory[61] = load >> 8; | |
| 224 | 235 | } |
| 225 | 236 | } |
| 226 | 237 |
Software/so101/src/jam-stream.test.ts added +71
| @@ -0,0 +1,71 @@ | ||
| 1 | import { afterEach, describe, expect, test } from "bun:test"; | |
| 2 | import { mkdtempSync } from "node:fs"; | |
| 3 | import { tmpdir } from "node:os"; | |
| 4 | import { join } from "node:path"; | |
| 5 | import WebSocket from "ws"; | |
| 6 | import { FeetechBus } from "./bus.ts"; | |
| 7 | import { FakeBus } from "./fake-bus.ts"; | |
| 8 | import { startServer, type RunningServer } from "./server.ts"; | |
| 9 | ||
| 10 | // The jam watch on the stream, against servos that move like the real ones (FakeBus dynamics: 70 % load while | |
| 11 | // speeding up or braking) at the limits the real arm runs: 180°/s and 1080°/s². | |
| 12 | const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms)); | |
| 13 | const START = 2048; | |
| 14 | let fake: FakeBus; | |
| 15 | let server: RunningServer; | |
| 16 | ||
| 17 | const boot = async (stuck: number[], jamSampleMs: number) => { | |
| 18 | fake = new FakeBus([1, 2], { dynamics: {}, stuck: new Set(stuck) }); | |
| 19 | server = await startServer(new FeetechBus(fake), { | |
| 20 | host: "127.0.0.1", | |
| 21 | port: 0, | |
| 22 | ids: [1, 2], | |
| 23 | posesFile: join(mkdtempSync(join(tmpdir(), "so101-jam-")), "poses.json"), | |
| 24 | modelDir: "/nowhere", | |
| 25 | jamSampleMs, | |
| 26 | safety: { maxSpeedDegS: 180, maxAccelDegS2: 1080 }, | |
| 27 | }); | |
| 28 | const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`); | |
| 29 | await new Promise((r) => ws.once("open", r)); | |
| 30 | await sleep(100); // armed | |
| 31 | return ws; | |
| 32 | }; | |
| 33 | const get = async <T>(path: string) => (await (await fetch(`http://127.0.0.1:${server.port}${path}`)).json()) as T; | |
| 34 | type Status = { jammed: number[]; jams: Array<{ id: number; joint: string; load: number; why: string }> }; | |
| 35 | afterEach(() => Promise.race([server.close(), sleep(500)])); | |
| 36 | ||
| 37 | describe("a jam is load without progress", () => { | |
| 38 | test("a hard 90° flick there and back, read every 5 ms, raises no jam", async () => { | |
| 39 | const ws = await boot([], 5); // readings a few ms apart, as a backed-up queue takes them | |
| 40 | const loads: number[] = []; | |
| 41 | const watch = setInterval(() => void get<{ joints: Array<{ load: number }> }>("/status").then((s) => loads.push(Math.abs(s.joints[0]!.load))), 20); | |
| 42 | for (const target of [START + 1024, START, START + 1024, START]) { | |
| 43 | // The flick, then the target held as the page holds it (a frame every 20 ms), so the dead-man stays quiet. | |
| 44 | for (let t = 0; t < 900; t += 20) ws.send(JSON.stringify({ 1: target, 2: target })), await sleep(20); // 90° takes about 670 ms | |
| 45 | } | |
| 46 | clearInterval(watch); | |
| 47 | expect(Math.max(...loads)).toBeGreaterThanOrEqual(45); // the load spiked over the jam threshold | |
| 48 | expect(Math.abs(fake.position(1) - START)).toBeLessThanOrEqual(12); // and the arm went all the way, every time | |
| 49 | const s = await get<Status>("/status"); | |
| 50 | expect(s.jammed).toEqual([]); | |
| 51 | expect((await get<{ counters: { jam: number } }>("/metrics")).counters.jam).toBe(0); | |
| 52 | ws.close(); | |
| 53 | }, 10_000); | |
| 54 | ||
| 55 | test("a joint held against a stop jams within 500 ms, and the page is told why", async () => { | |
| 56 | const ws = await boot([2], 100); | |
| 57 | const asked = performance.now(); | |
| 58 | const push = setInterval(() => ws.send(JSON.stringify({ 1: START + 300, 2: START + 300 })), 20); | |
| 59 | let s = await get<Status>("/status"); | |
| 60 | while (!s.jammed.length && performance.now() - asked < 1500) s = await get<Status>((await sleep(10), "/status")); | |
| 61 | const took = performance.now() - asked; | |
| 62 | clearInterval(push); | |
| 63 | expect(s.jammed).toEqual([2]); | |
| 64 | expect(took).toBeLessThan(500); | |
| 65 | expect(s.jams).toEqual([{ id: 2, joint: "shoulder_lift", position: START, target: START + 300, load: 90, why: expect.stringContaining("pushed at 90%+ load") }]); | |
| 66 | expect(s.jams[0]!.why).toContain("moved 0 of the 300 ticks"); | |
| 67 | expect(Math.abs(fake.position(1) - (START + 300))).toBeLessThanOrEqual(12); // the free joint kept going | |
| 68 | expect((await get<{ counters: { jam: number } }>("/metrics")).counters.jam).toBe(1); | |
| 69 | ws.close(); | |
| 70 | }); | |
| 71 | }); |
Software/so101/src/jam-watch.test.ts added +63
| @@ -0,0 +1,63 @@ | ||
| 1 | import { describe, expect, test } from "bun:test"; | |
| 2 | import { JAM_WINDOW_MS, JamWatch, judge, type JamReading } from "./jam-watch.ts"; | |
| 3 | ||
| 4 | /** Readings every `every` ms from t=0, positions and loads by index. */ | |
| 5 | const run = (n: number, every: number, p: (i: number) => number, load: number, target: number): JamReading[] => | |
| 6 | Array.from({ length: n }, (_, i) => ({ t: i * every, p: p(i), load, target })); | |
| 7 | const feed = (rs: JamReading[]) => { | |
| 8 | const w = new JamWatch(); | |
| 9 | return rs.map((r) => w.observe(1, r)); | |
| 10 | }; | |
| 11 | ||
| 12 | describe("a jam is load without progress, over real time", () => { | |
| 13 | test("a joint held against a stop at 90 % load jams once the window has passed", () => { | |
| 14 | const out = feed(run(6, 100, () => 2048, 90, 2048 + 300)); | |
| 15 | expect(out.slice(0, 3)).toEqual([null, null, null]); // 0-200 ms: not yet | |
| 16 | const jam = out[3]!; // 300 ms | |
| 17 | expect(jam).toMatchObject({ position: 2048, target: 2348, load: 90, ms: 300, progress: 0, direction: 1 }); | |
| 18 | expect(jam.why).toBe("pushed at 90%+ load for 300 ms and moved 0 of the 300 ticks to its target"); | |
| 19 | }); | |
| 20 | ||
| 21 | test("chattering by ±3 ticks against the stop is still no progress", () => { | |
| 22 | expect(feed(run(5, 100, (i) => 2048 + (i % 2 ? 3 : -3), -60, 1700)).at(-1)).toMatchObject({ direction: -1, load: 60 }); | |
| 23 | }); | |
| 24 | ||
| 25 | test("six readings bunched into 15 ms at the start of a hard move (the old count-based window's false jam) are not a jam", () => { | |
| 26 | // 1080°/s² = 12288 ticks/s²: from rest, 0.5·a·t² is under 2 ticks in the first 15 ms. | |
| 27 | const a = 12288; | |
| 28 | expect(feed(run(6, 3, (i) => 1581 - Math.round(0.5 * a * ((i * 3) / 1000) ** 2), 60.4, 1315)).every((j) => j === null)).toBe(true); | |
| 29 | }); | |
| 30 | ||
| 31 | test("a joint advancing toward a far target under heavy load is never a jam", () => { | |
| 32 | expect(feed(run(20, 50, (i) => 2978 + i * 20, -70, 3600)).every((j) => j === null)).toBe(true); | |
| 33 | }); | |
| 34 | ||
| 35 | test("a joint braking away from a target that just reversed is moving, so it is not a jam", () => { | |
| 36 | // The leader turned back: the target is behind, and the joint slows (high load) while still going the other way. | |
| 37 | const rs = run(8, 50, (i) => 2000 + 100 * i - 6 * i * i, 70, 1800); | |
| 38 | expect(feed(rs).every((j) => j === null)).toBe(true); | |
| 39 | }); | |
| 40 | ||
| 41 | test("load that dips below the threshold, or a target within 30 ticks, is not a jam", () => { | |
| 42 | expect(feed(run(6, 100, () => 2048, 30, 2400)).every((j) => j === null)).toBe(true); | |
| 43 | expect(feed(run(6, 100, () => 2048, 90, 2070)).every((j) => j === null)).toBe(true); | |
| 44 | const dip = run(6, 100, () => 2048, 90, 2400); | |
| 45 | dip[2]!.load = 20; | |
| 46 | expect(judge(dip)).toBeNull(); | |
| 47 | }); | |
| 48 | ||
| 49 | test("a gap as long as the window starts over: two readings a hold apart are no evidence", () => { | |
| 50 | const w = new JamWatch(); | |
| 51 | expect(w.observe(1, { t: 0, p: 2048, load: 90, target: 2400 })).toBeNull(); | |
| 52 | expect(w.observe(1, { t: JAM_WINDOW_MS + 500, p: 2048, load: 90, target: 2400 })).toBeNull(); | |
| 53 | expect(w.observe(1, { t: JAM_WINDOW_MS + 600, p: 2048, load: 90, target: 2400 })).toBeNull(); | |
| 54 | }); | |
| 55 | ||
| 56 | test("clear forgets a joint's readings, and joints are watched apart", () => { | |
| 57 | const w = new JamWatch(); | |
| 58 | for (const t of [0, 100, 200]) w.observe(1, { t, p: 2048, load: 90, target: 2400 }), w.observe(2, { t, p: 2048, load: 90, target: 2400 }); | |
| 59 | w.clear(1); | |
| 60 | expect(w.observe(1, { t: 300, p: 2048, load: 90, target: 2400 })).toBeNull(); | |
| 61 | expect(w.observe(2, { t: 300, p: 2048, load: 90, target: 2400 })).not.toBeNull(); | |
| 62 | }); | |
| 63 | }); |
Software/so101/src/jam-watch.ts added +88
| @@ -0,0 +1,88 @@ | ||
| 1 | /** | |
| 2 | * What a jam is on the stream: load without progress. A joint counts as jammed only when, for JAM_WINDOW_MS of | |
| 3 | * real time, every reading of its load is over JAM_LOAD and it has not moved toward its target (nor anywhere else) | |
| 4 | * by more than JAM_STILL_TICKS, while that target is still far. A servo accelerating hard toward a far target | |
| 5 | * pulls just as much load, but it is moving, so it is never a jam. | |
| 6 | * | |
| 7 | * The window is judged by the readings' own clock, not by how many there are: the stream's work runs in one queue, | |
| 8 | * so a backed-up queue can take several readings within a few milliseconds (measured on the arm, 2026-10-22: 17 jams | |
| 9 | * in an evening, each at 50-70 % load with the target 200-370 ticks away, at the start of a hard move). | |
| 10 | * Plain data in, plain data out; src/server.ts reads the bus and parks the joint. | |
| 11 | */ | |
| 12 | ||
| 13 | /** Load, percent of the servo's maximum, that counts as pushing against something. */ | |
| 14 | export const JAM_LOAD = 45; | |
| 15 | /** How long a joint must push without progress before it is a jam, ms. */ | |
| 16 | export const JAM_WINDOW_MS = 300; | |
| 17 | /** Movement within the window at or below this is "no progress": a jammed joint chattering against an obstacle still counts. */ | |
| 18 | export const JAM_STILL_TICKS = 8; | |
| 19 | /** A target this close is "there": pushing near it is holding, not a jam. */ | |
| 20 | export const JAM_AWAY_TICKS = 30; | |
| 21 | ||
| 22 | export interface JamReading { | |
| 23 | /** When the reading was taken, ms (any monotonic clock). */ | |
| 24 | t: number; | |
| 25 | /** Present_Position, ticks. */ | |
| 26 | p: number; | |
| 27 | /** Present_Load, percent, signed. */ | |
| 28 | load: number; | |
| 29 | /** Where the joint is being sent, ticks. */ | |
| 30 | target: number; | |
| 31 | } | |
| 32 | ||
| 33 | export interface Jam { | |
| 34 | position: number; | |
| 35 | target: number; | |
| 36 | /** The lowest load over the window, percent: it pushed at least this hard the whole time. */ | |
| 37 | load: number; | |
| 38 | /** How long it pushed without progress, ms. */ | |
| 39 | ms: number; | |
| 40 | /** How far it got toward the target over that time, ticks (negative: pushed back). */ | |
| 41 | progress: number; | |
| 42 | /** +1 or -1: the way it was pushing. */ | |
| 43 | direction: number; | |
| 44 | /** One line for people: what it did and why it was stopped. */ | |
| 45 | why: string; | |
| 46 | } | |
| 47 | ||
| 48 | /** Each joint's recent readings; `observe` says when one of them jams. */ | |
| 49 | export class JamWatch { | |
| 50 | readonly = new Map<number, JamReading[]>(); | |
| 51 | ||
| 52 | /** A new reading for a joint; returns the jam when the readings now make one, otherwise null. */ | |
| 53 | observe(id: number, reading: JamReading): Jam | null { | |
| 54 | const w = this..get(id) ?? []; | |
| 55 | const previous = w[w.length - 1]; | |
| 56 | // A clock that went back, or a gap as long as the window (a hold, a stalled queue), says nothing of what happened between: start over. | |
| 57 | if (previous && (reading.t < previous.t || reading.t - previous.t >= JAM_WINDOW_MS)) w.length = 0; | |
| 58 | w.push(reading); | |
| 59 | // Keep the newest reading at or before the window's start, so the readings kept span the whole window. | |
| 60 | while (w.length > 2 && w[1]!.t <= reading.t - JAM_WINDOW_MS) w.shift(); | |
| 61 | this..set(id, w); | |
| 62 | return judge(w); | |
| 63 | } | |
| 64 | ||
| 65 | /** Forget a joint's readings (it was parked, or its leader backed off); all joints without an id. */ | |
| 66 | clear(id?: number): void { | |
| 67 | if (id === undefined) this..clear(); | |
| 68 | else this..delete(id); | |
| 69 | } | |
| 70 | } | |
| 71 | ||
| 72 | /** The jam a run of readings makes, oldest first, or null. */ | |
| 73 | export function judge(w: readonly JamReading[]): Jam | null { | |
| 74 | const first = w[0], last = w[w.length - 1]; | |
| 75 | if (!first || !last) return null; | |
| 76 | const ms = last.t - first.t; | |
| 77 | if (ms < JAM_WINDOW_MS) return null; | |
| 78 | const away = last.target - last.p; | |
| 79 | if (Math.abs(away) <= JAM_AWAY_TICKS) return null; | |
| 80 | if (!w.every((r) => Math.abs(r.load) >= JAM_LOAD)) return null; | |
| 81 | const span = Math.max(...w.map((r) => r.p)) - Math.min(...w.map((r) => r.p)); | |
| 82 | const direction = Math.sign(away); | |
| 83 | const progress = (last.p - first.p) * direction; | |
| 84 | if (span > JAM_STILL_TICKS || progress > JAM_STILL_TICKS) return null; | |
| 85 | const load = Math.min(...w.map((r) => Math.abs(r.load))); | |
| 86 | const why = `pushed at ${Math.round(load)}%+ load for ${Math.round(ms)} ms and moved ${Math.max(0, Math.round(progress))} of the ${Math.abs(Math.round(away))} ticks to its target`; | |
| 87 | return { position: last.p, target: last.target, load, ms: Math.round(ms), progress, direction, why }; | |
| 88 | } |
Software/so101/src/page-feedback.test.ts +2
| @@ -67,6 +67,8 @@ describe("a refusal reaches the page", () => { | ||
| 67 | 67 | expect(clampLine({ what: "joint limit", detail: { id: 3, asked: 4000, sent: 3500, source: "stream" } })).toBe("joint limit: joint 3, asked 4000, sent 3500"); |
| 68 | 68 | expect(clampLine({ what: "zone", detail: { what: "table", link: "gripper", depthMm: 12 } })).toBe("zone: table 12 mm"); |
| 69 | 69 | expect(clampLine({ what: "x".repeat(300), detail: {} }).length).toBe(140); |
| 70 | const why = "shoulder_lift pushed at 90%+ load for 300 ms and moved 0 of the 300 ticks to its target, stopped there"; | |
| 71 | expect(clampLine({ what: "jam", detail: { why, id: 2, position: 2048, target: 2348, load: 90 } })).toBe(`jam: ${why}, joint 2`); // a jam tells the driver why, whole | |
| 70 | 72 | const entry = (kind: "sent" | "clamp", seq: number) => ({ seq, t: 0, kind, what: "jam", detail: {} }); |
| 71 | 73 | expect(refusalBatch([entry("sent", 1)])).toBeNull(); |
| 72 | 74 | expect(refusalBatch([entry("clamp", 2), entry("sent", 3), entry("clamp", 4)])).toEqual({ type: "refused", line: "jam", count: 2 }); |
Software/so101/src/server.ts +22 −22
| @@ -63,7 +63,7 @@ | ||
| 63 | 63 | * One thing drives the arm at a time. Every target is pulled inside the joint limits and every motion held to the speed and |
| 64 | 64 | * acceleration limits (src/safety.ts); a stream that goes silent holds, one whose connection is lost parks. Every target, whether a move or a stream frame, is run through |
| 65 | 65 | * the arm's own geometry first: a pose that would push a part into the table or the base is refused |
| 66 | * (or only reported, in "warn" mode). On the stream, a joint that pushes hard without moving is a jam: | |
| 66 | * (or only reported, in "warn" mode). On the stream, a joint that pushes hard for 300 ms without moving is a jam (src/jam-watch.ts): | |
| 67 | 67 | * its goal is parked where it is, and it follows again once the leader backs off. |
| 68 | 68 | */ |
| 69 | 69 | |
| @@ -90,7 +90,8 @@ import { deployedCommit, PrecisionHistory } from "./precision-history.ts"; | ||
| 90 | 90 | import { Tuning, TuningRefused } from "./tuning.ts"; |
| 91 | 91 | import { sweepJoint } from "./tuning-sweep.ts"; |
| 92 | 92 | import type { SweepResult, TuningDeps } from "./tuning-run.ts"; |
| 93 | import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts"; | |
| 93 | import { MotionError, moveToPose, playTrajectory, recordTrajectory } from "./motion.ts"; | |
| 94 | import { JamWatch, type Jam } from "./jam-watch.ts"; | |
| 94 | 95 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; |
| 95 | 96 | import { MAX_ZERO_OFFSET_DEG, zeroOffsetDeg } from "./joint-zero.ts"; |
| 96 | 97 | import { JOINTS, registerName, STS3215, ticksToDegrees } from "./sts3215.ts"; |
| @@ -182,13 +183,10 @@ const MAX_BODY = 1_000_000; | ||
| 182 | 183 | /** A table touch older than this is forgotten: the arm or the table may have moved since. */ |
| 183 | 184 | const TOUCH_TTL_MS = 60_000; |
| 184 | 185 | const URDF = "so101_new_calib.urdf"; |
| 185 | /** Load, in percent of the servo's maximum, that counts as pushing against something. */ | |
| 186 | 186 | /** The present block status reads in one sync read; Present_Velocity and Present_Voltage ride along unread. */ |
| 187 | 187 | const PRESENT = [STS3215.Present_Position, STS3215.Present_Load, STS3215.Present_Temperature] as const; |
| 188 | 188 | /** How stale the torque flags in a status may be, ms: positions go out at the bus cycle, torque at 2 Hz. */ |
| 189 | 189 | const TORQUE_EVERY_MS = 500; |
| 190 | const JAM_LOAD = 45; | |
| 191 | const JAM_WINDOW = 6; | |
| 192 | 190 | const VENDORED = new Set(["three", "urdf-loader"]); |
| 193 | 191 | /** A module the page loads from src/ui: plain JavaScript, never its tests. */ |
| 194 | 192 | const isUiModule = (name: string) => /^[a-z][a-z0-9-]*\.(js|css)$/.test(name); |
| @@ -467,7 +465,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 467 | 465 | }; |
| 468 | 466 | /** What the stream is up against right now, for /status. */ |
| 469 | 467 | let blocked: Violation | null = null; |
| 470 | const jams = new Map<number, { position: number; direction: number }>(); | |
| 468 | /** The stream's parked joints, by id: where and why (src/jam-watch.ts). */ | |
| 469 | const jams = new Map<number, Jam>(); | |
| 471 | 470 | let streamStats: { frames: number; dropped: number; refused: number } | null = null; |
| 472 | 471 | |
| 473 | 472 | // ------------------------------------------------------------------ files |
| @@ -668,6 +667,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 668 | 667 | tableZ: model ? model.tableZ(zoneSettings()) : null, |
| 669 | 668 | blocked, |
| 670 | 669 | jammed: [...jams.keys()], |
| 670 | /** Why each jammed joint was parked: the page says it, so a jam is never a silent snap-back. */ | |
| 671 | jams: [...jams].map(([id, j]) => ({ id, joint: JOINTS[id] ?? null, position: j.position, target: j.target, load: Number(j.load.toFixed(1)), why: j.why })), | |
| 671 | 672 | stream: streamStats, |
| 672 | 673 | /** The stream's dead-man hold (no frame lately); `held` is the STOP's, which only /resume ends. */ |
| 673 | 674 | deadman: held, |
| @@ -1299,27 +1300,25 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1299 | 1300 | armed = true; |
| 1300 | 1301 | }); |
| 1301 | 1302 | |
| 1302 | // The jam watch: a joint that pushes hard without moving toward its target is parked where it is. | |
| 1303 | const windows = new Map<number, Array<{ p: number; l: number }>>(); | |
| 1304 | const observe = async (id: number, p: number, l: number, target: number | undefined): Promise<boolean> => { | |
| 1303 | // The jam watch: a joint that pushes hard for a while without moving toward its target is parked where it is. | |
| 1304 | const watcher = new JamWatch(); | |
| 1305 | const observe = async (id: number, t: number, p: number, l: number, target: number | undefined): Promise<boolean> => { | |
| 1305 | 1306 | present.set(id, p); |
| 1306 | 1307 | tracker.sample(id, p, Date.now()); |
| 1307 | const w = windows.get(id) ?? []; | |
| 1308 | w.push({ p, l }); | |
| 1309 | if (w.length > JAM_WINDOW) w.shift(); | |
| 1310 | windows.set(id, w); | |
| 1311 | if (jams.has(id) || w.length < JAM_WINDOW || target === undefined) return jams.has(id); | |
| 1312 | const pushing = w.every((s) => Math.abs(s.l) >= JAM_LOAD * 10); | |
| 1313 | const travel = Math.max(...w.map((s) => s.p)) - Math.min(...w.map((s) => s.p)); | |
| 1314 | if (!(pushing && travel < STALL_SPAN_TICKS && Math.abs(target - p) > 30)) return false; | |
| 1308 | if (jams.has(id)) return true; | |
| 1309 | if (target === undefined) return false; | |
| 1310 | const jam = watcher.observe(id, { t, p, load: l / 10, target }); | |
| 1311 | if (!jam) return false; | |
| 1315 | 1312 | await bus.syncWrite(STS3215.Goal_Position, [[id, p]]); |
| 1316 | 1313 | governor.pin(id, p); |
| 1317 | jams.set(id, { position: p, direction: Math.sign(target - p) }); | |
| 1314 | watcher.clear(id); | |
| 1315 | jams.set(id, jam); | |
| 1318 | 1316 | tracker.snapBack("jam", Date.now(), id, target - p); |
| 1319 | 1317 | tracker.forget(id); |
| 1320 | 1318 | last?.set(id, p); |
| 1321 | metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10 }); | |
| 1322 | machine.note("clamp", "jam", { id, position: p, target, load: l / 10 }); | |
| 1319 | const joint = JOINTS[id] ?? `joint ${id}`; | |
| 1320 | metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10, minLoad: Number(jam.load.toFixed(1)), ms: jam.ms, progress: jam.progress }); | |
| 1321 | machine.note("clamp", "jam", { why: `${joint} ${jam.why}, stopped there`, id, position: p, target, load: l / 10 }); | |
| 1323 | 1322 | return true; |
| 1324 | 1323 | }; |
| 1325 | 1324 | const watch = setInterval( |
| @@ -1327,10 +1326,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1327 | 1326 | void run(async () => { |
| 1328 | 1327 | if (closed || !armed || last === null) return; |
| 1329 | 1328 | const positions = await bus.syncRead(ids, STS3215.Present_Position); |
| 1329 | const t = performance.now(); // when it was read, not when it was asked for: a backed-up queue reads in bursts | |
| 1330 | 1330 | const loads = await bus.syncRead(ids, STS3215.Present_Load); |
| 1331 | 1331 | for (const id of ids) { |
| 1332 | 1332 | const p = positions.get(id), l = loads.get(id); |
| 1333 | if (p !== undefined && l !== undefined) await observe(id, p, l, last.get(id)); | |
| 1333 | if (p !== undefined && l !== undefined) await observe(id, t, p, l, last.get(id)); | |
| 1334 | 1334 | } |
| 1335 | 1335 | }), |
| 1336 | 1336 | jamSampleMs, |
| @@ -1384,7 +1384,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1384 | 1384 | if (jam) { |
| 1385 | 1385 | if ((target - jam.position) * jam.direction > -10) continue; // still pushing into the obstacle: wait for the leader to back off |
| 1386 | 1386 | jams.delete(id); |
| 1387 | windows.delete(id); | |
| 1387 | watcher.clear(id); | |
| 1388 | 1388 | } |
| 1389 | 1389 | // A flick is followed along the governor's profile, never dropped for being far: the limits are what keep it safe. |
| 1390 | 1390 | targets.push([id, target]); |
Software/so101/src/ui/hold-line.js +6 −1
| @@ -5,7 +5,12 @@ const lost = "The arm has been unreachable for over a minute. This is the SIMULA | ||
| 5 | 5 | export function holdText(/** @type {any} */ d, /** @type {(v: any) => string} */ say, /** @type {Record<number, string>} */ names) { |
| 6 | 6 | if (d.reconnecting) return { bad: false, text: `Reconnecting to the arm… showing where it was at ${new Date(d.seenAt ?? 0).toLocaleTimeString()}. Driving is paused, and the arm holds where it is.` }; |
| 7 | 7 | if (typeof d.source === "string" && d.source.startsWith("simulated: offline")) return { bad: true, text: lost }; |
| 8 | if (d.jammed?.length) return { bad: true, text: `jam: ${d.jammed.map((/** @type {number} */ id) => names[id] ?? id).join(", ")} pushed against something and stopped — back the leader off` }; | |
| 8 | if (d.jammed?.length) { | |
| 9 | // Why it stopped, from the arm (src/jam-watch.ts): a jam is never a silent snap-back. An older arm sends only the ids. | |
| 10 | const whys = Array.isArray(d.jams) ? d.jams.filter((/** @type {any} */ j) => typeof j?.why === "string").map((/** @type {any} */ j) => `${names[j.id] ?? j.joint ?? j.id} ${j.why}`) : []; | |
| 11 | if (whys.length) return { bad: true, text: `jam: ${whys.join("; ")} — stopped it there; back the leader off` }; | |
| 12 | return { bad: true, text: `jam: ${d.jammed.map((/** @type {number} */ id) => names[id] ?? id).join(", ")} pushed against something and stopped — back the leader off` }; | |
| 13 | } | |
| 9 | 14 | if (d.blocked && d.zone === "enforce") return { bad: true, text: `held at the boundary: ${say(d.blocked)}` }; |
| 10 | 15 | return null; |
| 11 | 16 | } |
Software/so101/src/ui/hold-line.test.ts +4
| @@ -18,6 +18,10 @@ describe("which notices take the status strip's hold row", () => { | ||
| 18 | 18 | expect(holdText({ jammed: [1, 6] }, say, names)).toEqual({ bad: true, text: expect.stringContaining("jam: shoulder_pan, gripper") }); |
| 19 | 19 | expect(holdText({ jammed: [] }, say, names)).toBeNull(); |
| 20 | 20 | }); |
| 21 | test("a jam says why it stopped, joint by joint", () => { | |
| 22 | const jams = [{ id: 1, joint: "shoulder_pan", why: "pushed at 62%+ load for 300 ms and moved 2 of the 266 ticks to its target" }]; | |
| 23 | expect(holdText({ jammed: [1], jams }, say, names)!.text).toBe("jam: shoulder_pan pushed at 62%+ load for 300 ms and moved 2 of the 266 ticks to its target — stopped it there; back the leader off"); | |
| 24 | }); | |
| 21 | 25 | test("an enforce boundary hold", () => { |
| 22 | 26 | expect(holdText({ blocked: "b", zone: "enforce" }, say, names)).toEqual({ bad: true, text: "held at the boundary: <b>" }); |
| 23 | 27 | }); |
new above
offline