Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
TOP: the arm moves when you drive it - the collision guard blocks every move because the virtual table is 3 cm too high, and the driver is not registered #151
smith/arm:smith/144-top-the-arm-moves-when-you-drive-it-the → main 1 commit fork
4 files
changed +56
−6
Software/so101/src/relay.ts +7 −2
| @@ -362,16 +362,21 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> { | ||
| 362 | 362 | * The arm's /status, saying which arm it is: from the link while it is live, the last one it had while reconnecting, |
| 363 | 363 | * and from the simulation otherwise. |
| 364 | 364 | */ |
| 365 | /** Who holds the turn, as /status and the Machine panel tell it; null when nobody does. */ | |
| 366 | const turnHolder = () => { | |
| 367 | const d = lobby?.turns.snapshot().driver; | |
| 368 | return d ? { id: d.id, name: d.name, owner: d.owner } : null; | |
| 369 | }; | |
| 365 | 370 | const statusOf = async (path = "/status"): Promise<{ code: number; body: Record<string, unknown> }> => { |
| 366 | 371 | if (onArm()) { |
| 367 | 372 | const last = armLink!.last; |
| 368 | 373 | const reconnecting = mode() === "reconnecting"; |
| 369 | 374 | if (!last) return { code: 503, body: { error: `${RECONNECTING}…`, source: source(), reconnecting } }; |
| 370 | return { code: 200, body: { ...last, held: gate.held ?? last.held ?? null, source: source(), reconnecting, ...(reconnecting ? { seenAt: new Date(armLink!.lastAt).toISOString() } : {}) } }; | |
| 375 | return { code: 200, body: { ...last, driver: turnHolder(), held: gate.held ?? last.held ?? null, source: source(), reconnecting, ...(reconnecting ? { seenAt: new Date(armLink!.lastAt).toISOString() } : {}) } }; | |
| 371 | 376 | } |
| 372 | 377 | const answer = await fetch(simulation + path, { cache: "no-store", signal: AbortSignal.timeout(readTimeoutMs) }); |
| 373 | 378 | const body = (await answer.json()) as Record<string, unknown>; |
| 374 | return { code: answer.status, body: { ...body, held: gate.held ?? body.held ?? null, source: source() } }; | |
| 379 | return { code: answer.status, body: { ...body, driver: turnHolder(), held: gate.held ?? body.held ?? null, source: source() } }; | |
| 375 | 380 | }; |
| 376 | 381 | const status = async (res: ServerResponse, path: string) => { |
| 377 | 382 | const { code, body } = await statusOf(path); |
Software/so101/src/server.test.ts +16
| @@ -548,6 +548,22 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 548 | 548 | await zcall("/settings", { zone: "enforce" }, "PUT"); |
| 549 | 549 | }); |
| 550 | 550 | |
| 551 | test("an arm already in the table may back out, never go deeper; \"table here\" puts the table where it rests", async () => { | |
| 552 | await zcall("/settings", { zone: "warn", floorMarginMm: 0 }, "PUT"); | |
| 553 | await moved({ "2": ticks(2, 80) }); // the arm is in the table now | |
| 554 | await zcall("/settings", { zone: "enforce" }, "PUT"); | |
| 555 | const deeper = await zcall("/move", { targets: { "2": ticks(2, 95) } }); | |
| 556 | expect(deeper.status).toBe(409); | |
| 557 | expect(((await deeper.json()) as { error: string }).error).toMatch(/^refused: .*below the table/); | |
| 558 | await moved({ "2": ticks(2, 55) }); // up and out: allowed though still inside | |
| 559 | const here = (await (await zcall("/settings/table-here", {})).json()) as { tableOffsetMm: number }; | |
| 560 | expect(here.tableOffsetMm).toBeGreaterThan(0); | |
| 561 | const s = (await (await fetch(`${zurl}/status`)).json()) as { violation: unknown }; | |
| 562 | expect(s.violation).toBeNull(); | |
| 563 | await zcall("/settings", { tableOffsetMm: 0, floorMarginMm: 10 }, "PUT"); | |
| 564 | await moved({ "2": mid(2) }); | |
| 565 | }); | |
| 566 | ||
| 551 | 567 | test("a stream frame into the table holds the arm; a safe frame resumes it", async () => { |
| 552 | 568 | const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`); |
| 553 | 569 | await new Promise((r) => ws.once("open", r)); |
Software/so101/src/server.ts +23 −4
| @@ -27,6 +27,7 @@ | ||
| 27 | 27 | * PUT /calibration/<kind> store that JSON next to the poses (kind: camera, camera2, handeye, ...) |
| 28 | 28 | * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"}, |
| 29 | 29 | * and the safety limits of src/safety.ts (speed, acceleration, rate, dead-man, park, gripper force, joint limits) |
| 30 | * POST /settings/table-here the arm rests on the table now: put the table at its lowest point (tableOffsetMm) | |
| 30 | 31 | * PUT /settings change any of those |
| 31 | 32 | * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events |
| 32 | 33 | * GET /model.json the arm model: joints, the table plane, where the URDF is served |
| @@ -111,11 +112,13 @@ export interface Settings extends SafetySettings { | ||
| 111 | 112 | zone: "enforce" | "warn" | "off"; |
| 112 | 113 | floorMarginMm: number; |
| 113 | 114 | baseMarginMm: number; |
| 115 | /** How far below the model's table the real one is, mm: the arm resting on it is not "in" it. */ | |
| 116 | tableOffsetMm: number; | |
| 114 | 117 | /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */ |
| 115 | 118 | jointOffsetsDeg: Record<string, number>; |
| 116 | 119 | } |
| 117 | 120 | |
| 118 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 121 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 119 | 122 | |
| 120 | 123 | /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */ |
| 121 | 124 | export interface EStop { |
| @@ -225,7 +228,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 225 | 228 | console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`); |
| 226 | 229 | } |
| 227 | 230 | } |
| 228 | const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 }); | |
| 231 | const zoneSettings = () => ({ floorMargin: (settings.floorMarginMm - settings.tableOffsetMm) / 1000, baseMargin: settings.baseMarginMm / 1000 }); | |
| 229 | 232 | /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */ |
| 230 | 233 | const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745; |
| 231 | 234 | const toQ = (positions: ReadonlyMap<number, number>) => { |
| @@ -246,7 +249,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 246 | 249 | }; |
| 247 | 250 | const describeViolation = (v: Violation) => { |
| 248 | 251 | const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, ""); |
| 249 | const marginMm = v.what === "table" ? settings.floorMarginMm : settings.baseMarginMm; | |
| 252 | const marginMm = v.what === "table" ? settings.floorMarginMm - settings.tableOffsetMm : settings.baseMarginMm; | |
| 250 | 253 | const beyond = v.depth * 1000 - marginMm; // the depth includes the margin |
| 251 | 254 | return beyond > 0 |
| 252 | 255 | ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)` |
| @@ -256,11 +259,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 256 | 259 | const checkPath = (from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => { |
| 257 | 260 | if (!model || settings.zone === "off") return null; |
| 258 | 261 | let worst: Violation | null = null; |
| 262 | // A pose the arm is already in is never a reason to refuse: only going deeper than where it starts is. | |
| 263 | const startDepth = model.check(toQ(from), zoneSettings())?.depth ?? 0; | |
| 259 | 264 | for (let s = 1; s <= steps; s++) { |
| 260 | 265 | const mix = new Map<number, number>(); |
| 261 | 266 | for (const [id, a] of from) mix.set(id, Math.round(a + ((to.get(id) ?? a) - a) * (s / steps))); |
| 262 | 267 | const v = model.check(toQ(mix), zoneSettings()); |
| 263 | if (v && (!worst || v.depth > worst.depth)) worst = v; | |
| 268 | if (v && v.depth > startDepth + 0.001 && (!worst || v.depth > worst.depth)) worst = v; | |
| 264 | 269 | } |
| 265 | 270 | return worst; |
| 266 | 271 | }; |
| @@ -525,6 +530,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 525 | 530 | if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`); |
| 526 | 531 | out[key] = s[key]!; |
| 527 | 532 | } |
| 533 | if (s.tableOffsetMm !== undefined) { | |
| 534 | if (typeof s.tableOffsetMm !== "number" || !Number.isFinite(s.tableOffsetMm) || Math.abs(s.tableOffsetMm) > 100) throw new HttpError(400, "tableOffsetMm must be within ±100"); | |
| 535 | out.tableOffsetMm = s.tableOffsetMm; | |
| 536 | } | |
| 528 | 537 | if (s.jointOffsetsDeg !== undefined) { |
| 529 | 538 | if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object"); |
| 530 | 539 | for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) { |
| @@ -738,6 +747,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 738 | 747 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 739 | 748 | return json(res, 200, { ...settings, model: model !== null }); |
| 740 | 749 | } |
| 750 | case "POST /settings/table-here": { | |
| 751 | // The arm rests on the table now: put the table at its lowest point. | |
| 752 | if (!model) throw new HttpError(409, "no model loaded on this server: no table to set"); | |
| 753 | const lowest = model.lowest(toQ(await bus.syncRead(ids, STS3215.Present_Position))); | |
| 754 | const tableOffsetMm = Math.ceil(Math.max(-100, Math.min(100, (model.floorZ - lowest.z) * 1000))); | |
| 755 | settings = validateSettings({ ...settings, tableOffsetMm }); | |
| 756 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); | |
| 757 | machine.note("state", "table", { tableOffsetMm, link: lowest.link }); | |
| 758 | return json(res, 200, { ...settings, model: true }); | |
| 759 | } | |
| 741 | 760 | case "GET /model.json": |
| 742 | 761 | if (!model) return json(res, 404, { error: "no model loaded on this server" }); |
| 743 | 762 | return json(res, 200, { |
Software/so101/src/stop.test.ts +10
| @@ -178,6 +178,16 @@ describe("STOP through the relay, to a Pi that holds nothing itself", () => { | ||
| 178 | 178 | return relay.port; |
| 179 | 179 | }; |
| 180 | 180 | |
| 181 | test("/status says who holds the turn, the owner included", async () => { | |
| 182 | const port = await start(); | |
| 183 | const status = async () => ((await (await fetch(`http://127.0.0.1:${port}/status`)).json()) as { driver: { name: string; owner: boolean } | null }).driver; | |
| 184 | expect(await status()).toBeNull(); | |
| 185 | lobby!.turns.join({ id: "u-alice", name: "alice", owner: false }); | |
| 186 | expect(await status()).toMatchObject({ name: "alice", owner: false }); | |
| 187 | lobby!.turns.join({ id: "u-trevato", name: "trevato", owner: true }); | |
| 188 | expect(await status()).toMatchObject({ name: "trevato", owner: true }); | |
| 189 | }); | |
| 190 | ||
| 181 | 191 | test("(b) 100 commands sent while held, then STOP and resume: the Pi receives none of them, then the one new command", async () => { |
| 182 | 192 | const port = await start(); |
| 183 | 193 | lobby!.turns.join({ id: "u-alice", name: "alice", owner: false }); |
new above
offline